Bluetooth Car

bt_car.ino

#include <AFMotor.h>
#include <SoftwareSerial.h>

SoftwareSerial btSerial(A5, A4); // RX, TX

AF_DCMotor motor1(3);
AF_DCMotor motor2(4);

void setup() {
  // Set max speed
  motor1.setSpeed(255);
  motor1.run(RELEASE);
  motor2.setSpeed(255);
  motor2.run(RELEASE);

  Serial.begin(57600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  btSerial.begin(9600);
  
}

char c;
int dir1 = RELEASE;
int dir2 = RELEASE;
int new_dir1 = RELEASE;
int new_dir2 = RELEASE;

void loop() {

  if (btSerial.available()){
    c = btSerial.read();

    Serial.println(c);
  
    switch(c){
      case 'W':
        new_dir1 = FORWARD;
        new_dir2 = FORWARD;
        break;
      case 'S':
        new_dir1 = BACKWARD;
        new_dir2 = BACKWARD;
        break;
      case 'A':
        new_dir1 = BACKWARD;
        new_dir2 = FORWARD;
        break;
      case 'D':
        new_dir1 = FORWARD;
        new_dir2 = BACKWARD;
        break;
      case 'O':
        new_dir1 = RELEASE;
        new_dir2 = RELEASE;
        dir1 = RELEASE;
        dir2 = RELEASE;
        break;
    }

    if (new_dir1 != dir1 || new_dir2 != dir2){
      motor1.run(RELEASE); 
      motor2.run(RELEASE);
      delay(50);
    }

    // rotate
    if(new_dir1 != new_dir2){
      motor1.run(new_dir1);
      motor2.run(new_dir2);
      motor1.setSpeed(255);
      motor2.setSpeed(255);
      delay(200);
      motor1.run(RELEASE);
      motor2.run(RELEASE);
      delay(50);     
      motor1.run(dir1);
      motor2.run(dir2);
      motor1.setSpeed(255);
      motor2.setSpeed(255); 
    } else {
      motor1.run(new_dir1);
      motor2.run(new_dir2);
      motor1.setSpeed(255);
      motor2.setSpeed(255);
      dir1 = new_dir1;
      dir2 = new_dir2;
    }
  }
}