I am having an issue with the code below for my tank where whenever a motor goes into reverse it doesn't brake when it's supposed to... I am probably overlooking something simple, I would very much appreciate if anybody can help me out.
moving forward, is fine.
turning left/right, the motor that is reversing will run without stopping.
moving backward, both will run without stopping.
// Firmware for the Android Shield Board for tank robots
// Pan/tilt servos now work: 0 pin off, 255 pin on, 1~254 8 bit granularity servo movement (5 microseconds).
#define LeftMotorPWM 11
#define RightMotorPWM 6
#define LeftMotorDIR 3
#define RightMotorDIR 5
#define ServoPin1 0
#define ServoPin2 0
#define ServoFlip1 false
#define ServoFlip2 false
#define mySerialSpeed 9600 // arduino 2009: 4800 or lower!
#define debugSerialSpeed 9600 // arduino 2009: 4800 or lower!
#define BufferLength 16
#define LineEnd1 13
#define LineEnd2 10
#define ServoTimingStep 5
#define ServoCenter 1500
#define ServoTimingFloor ServoCenter-(127*ServoTimingStep)
//#define serialout
#define debugout
#include <SoftwareSerial.h>
SoftwareSerial mySerial(12, 255); // rx only
char charin = 80;
char inputBuffer[BufferLength];
int value = 128;
int speed = 128;
int timer = 15;
int timermax = 15;
int inputLength = 0;
int servoval1 = 127;
int servoval2 = 127;
int tempval1, tempval2;
void setup()
{
// motor pins must be outputs
pinMode(LeftMotorPWM, OUTPUT);
pinMode(RightMotorPWM, OUTPUT);
pinMode(LeftMotorDIR, OUTPUT);
pinMode(RightMotorDIR, OUTPUT);
mySerial.begin(mySerialSpeed);
#ifdef debugout
Serial.begin(debugSerialSpeed);
#endif
}
// process a command string
void HandleCommand(char* input, int length)
{
#ifdef debugout
Serial.print(">");
Serial.print(input);
Serial.print("<");
Serial.print(length);
Serial.println("|");
#endif
if (length < 1) { // not a valid command
return;
}
// calculate number following command (d10~d255)
if (length > 1) {
value = atoi(&input[1]);
if (value > 255)
value = 255;
if (value < 0)
value = 0;
switch(input[0])
{
case 'd':
case 'D':
if (value > 127)
value = 127;
speed = value*2;
break;
case '/':
timermax = value;
break;
case 'c':
case 'C':
#ifdef ServoFlip1
servoval1 = 256 - value;
#else
servoval1 = value;
#endif
break;
case 'v':
case 'V':
#ifdef ServoFlip2
servoval2 = 256 - value;
#else
servoval2 = value;
#endif
break;
default:
break;
}
}
timer = timermax;
int* command = (int*)input;
// check commands
// note that the two bytes are swapped, ie 'RA' means command AR
switch(*command) {
case '2':
case '2j':
case '2J':
analogWrite(LeftMotorPWM,100);
digitalWrite(LeftMotorDIR,HIGH);
analogWrite(RightMotorPWM,100);
digitalWrite(RightMotorDIR,HIGH);
break;
case '8':
case '8j':
case '8J':
analogWrite(LeftMotorPWM,100);
digitalWrite(LeftMotorDIR,LOW);
analogWrite(RightMotorPWM,100);
digitalWrite(RightMotorDIR,LOW);
break;
case '6':
case '6j':
case '6J':
analogWrite(LeftMotorPWM,100);
digitalWrite(LeftMotorDIR,LOW);
analogWrite(RightMotorPWM,100);
digitalWrite(RightMotorDIR,HIGH);
break;
case '4':
case '4j':
case '4J':
analogWrite(LeftMotorPWM,100);
digitalWrite(LeftMotorDIR,HIGH);
analogWrite(RightMotorPWM,100);
digitalWrite(RightMotorDIR,LOW);
break;
case '9':
case '9j':
case '9J':
analogWrite(RightMotorPWM, speed);
digitalWrite(RightMotorDIR, HIGH);
break;
case '1':
case '1j':
case '1J':
analogWrite(RightMotorPWM, speed);
digitalWrite(RightMotorDIR, LOW);
break;
case '3':
case '3j':
case '3J':
analogWrite(LeftMotorPWM, speed);
digitalWrite(LeftMotorDIR, HIGH);
break;
case '7':
case '7j':
case '7J':
analogWrite(LeftMotorPWM, speed);
digitalWrite(LeftMotorDIR, LOW);
break;
default: // stop, just to be safe
analogWrite(LeftMotorPWM, 0);
digitalWrite(LeftMotorDIR, LOW);
analogWrite(RightMotorPWM, 0);
digitalWrite(RightMotorDIR, LOW);
break;
}
}
void loop()
{
// get a command string form the mySerial port
inputLength = 0;
do {
while (!mySerial.available()){
// note: arduino cannot handle fullduplex on myserial so no output here!
// do servos here
tempval1 = (servoval1*ServoTimingStep) + ServoTimingFloor;
tempval2 = (servoval2*ServoTimingStep) + ServoTimingFloor;
if (servoval1 > 0)
digitalWrite(ServoPin1,HIGH);
delayMicroseconds(tempval1);
if (servoval1 < 255)
digitalWrite(ServoPin1,LOW);
if (servoval2 > 0)
digitalWrite(ServoPin2,HIGH);
delayMicroseconds(tempval2);
if (servoval2 < 255)
digitalWrite(ServoPin2,LOW);
delayMicroseconds(5000 - tempval1 - tempval2);
delay(15); // reduce/remove if we're doing more things here
// decrease the timer
if (--timer < 0)
{
timer=0;
analogWrite(LeftMotorPWM, 0);
analogWrite(RightMotorPWM, 0);
}
};
// wait for input
{
charin = mySerial.read(); // read it in
#ifdef debugout
Serial.print(charin);
tempval1 = charin;
Serial.println(tempval1);
#endif
if ((charin > 46 && charin < 58) || (charin=='d') || (charin=='j') || (charin=='c') || (charin=='v'))
{
inputBuffer[inputLength]=charin;
inputLength++;
#ifdef serialout
mySerial.print("$PD,11,");
mySerial.print(timer);
mySerial.print(",");
mySerial.print(value);
mySerial.println("*");
#endif
}
}
}
while (charin>46 && charin<119 && charin != LineEnd1 && charin != LineEnd2 && inputLength < BufferLength);
inputBuffer[inputLength] = 0; // add null terminator
HandleCommand(inputBuffer, inputLength);
}