SparkFun Electronics Forum

#define PwmPinMotorA 5
#define PwmPinMotorB 6
#define DirectionPinMotorA 9
#define DirectionPinMotorB 3
#define SerialSpeed 9600
#define BufferLength 16
#define LineEnd '#'
 
char inputBuffer[BufferLength];
 
void setup()
{
  // motor pins must be outputs
  pinMode(PwmPinMotorA, OUTPUT);
  pinMode(PwmPinMotorB, OUTPUT);
  pinMode(DirectionPinMotorA, OUTPUT);
  pinMode(DirectionPinMotorB, OUTPUT);
 
  Serial.begin(SerialSpeed); 
}
 
// process a command string
void HandleCommand(char* input, int length)
{
  Serial.println(input);
  if (length < 2) { // not a valid command
    return;
  }
  int value = 0;
  // calculate number following command
  if (length > 2) {
    value = atoi(&input[2]);
  }
  int* command = (int*)input;
  // check commands
  // note that the two bytes are swapped, ie 'RA' means command AR
  switch(*command) {
    
    
    case 'FA':
      // motors A & B BACKWARDS
      analogWrite(PwmPinMotorA, value);
      digitalWrite(DirectionPinMotorA, HIGH);
      analogWrite(PwmPinMotorB, value);
      digitalWrite(DirectionPinMotorB, LOW);
      break;
      
    case 'RA':
      // motor A & B FORWARDS
      analogWrite(PwmPinMotorA, value);
      digitalWrite(DirectionPinMotorA, LOW);
      analogWrite(PwmPinMotorB, value);
      digitalWrite(DirectionPinMotorB, HIGH);
      break;
      
    case 'FB':
      // motor B forwards
      analogWrite(PwmPinMotorB, value);
      digitalWrite(DirectionPinMotorB, HIGH);
      analogWrite(PwmPinMotorA, value);
      digitalWrite(DirectionPinMotorA, HIGH);
      break;
      
    case 'RB':
      // motor B reverse
      analogWrite(PwmPinMotorB, value);
      digitalWrite(DirectionPinMotorB, LOW);
      analogWrite(PwmPinMotorA, value);
      digitalWrite(DirectionPinMotorA, LOW);
      break;
      
          default:
      break;
  }  
} 
 
void loop()
{ 
  // get a command string form the serial port
  int inputLength = 0;
  do {
    while (!Serial.available()); // wait for input
    inputBuffer[inputLength] = Serial.read(); // read it in
  } while (inputBuffer[inputLength] != LineEnd && ++inputLength < BufferLength);
  inputBuffer[inputLength] = 0; //  add null terminator
  HandleCommand(inputBuffer, inputLength);
}