mirror of
https://github.com/caperren/school_archives.git
synced 2025-11-09 13:41:13 +00:00
98 lines
1.6 KiB
C++
98 lines
1.6 KiB
C++
|
|
#include <SoftwareSerial.h>
|
|
#include <Sabertooth.h>
|
|
|
|
int SabertoothPin = 8;
|
|
|
|
int LeftRoverPin = 2;
|
|
int RightRoverPin = 3;
|
|
int KillSwitchPin = 4;
|
|
|
|
SoftwareSerial SWSerial(NOT_A_PIN, SabertoothPin);
|
|
Sabertooth ST(128, SWSerial);
|
|
|
|
int Left1;
|
|
int Right1;
|
|
|
|
int Left;
|
|
int Right;
|
|
|
|
int LeftPrev = 1500;
|
|
int RightPrev = 1500;
|
|
|
|
int LeftSmoothVal = 65;
|
|
int RightSmoothVal = 65;
|
|
|
|
void setup() {
|
|
SWSerial.begin(9600);
|
|
ST.autobaud();
|
|
Serial.begin(9600);
|
|
|
|
pinMode(LeftRoverPin, INPUT);
|
|
pinMode(RightRoverPin, INPUT);
|
|
pinMode(KillSwitchPin, INPUT);
|
|
}
|
|
|
|
void loop() {
|
|
|
|
Left1 = pulseIn(LeftRoverPin, HIGH);
|
|
Right1 = pulseIn(RightRoverPin, HIGH);
|
|
//Left
|
|
if((LeftPrev > Left1) && ((LeftPrev - Left1) > LeftSmoothVal)){
|
|
LeftPrev -= LeftSmoothVal;
|
|
Left = map(LeftPrev, 1000, 2000, -127, 127);
|
|
|
|
}
|
|
else if((LeftPrev < Left1) && ((Left1 - LeftPrev) > LeftSmoothVal)){
|
|
LeftPrev += LeftSmoothVal;
|
|
Left = map(LeftPrev, 1000, 2000, -127, 127);
|
|
}
|
|
else{
|
|
Left = map(Left1, 1000, 2000, -127, 127);
|
|
LeftPrev = Left1;
|
|
}
|
|
//Right
|
|
if((RightPrev > Right1) && ((RightPrev - Right1) > RightSmoothVal)){
|
|
RightPrev -= RightSmoothVal;
|
|
Right = map(RightPrev , 2000, 1000, -127, 127);
|
|
|
|
}
|
|
else if((RightPrev < Right1) && ((Right1 - RightPrev) > RightSmoothVal)){
|
|
RightPrev += RightSmoothVal;
|
|
Right = map(RightPrev, 2000, 1000, -127, 127);
|
|
|
|
}
|
|
else{
|
|
Right = map(Right1, 2000, 1000, -127, 127);
|
|
RightPrev = Right1;
|
|
}
|
|
//Drive
|
|
if(((Left1 & Right1) != 0) & (pulseIn(KillSwitchPin, HIGH) > 1500)){
|
|
ST.motor(1, Left);
|
|
ST.motor(2, Right);
|
|
}
|
|
else{
|
|
ST.motor(1, 0);
|
|
ST.motor(2, 0);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|