How to Build a Motion Control Robot

#define LS 40    //Left motor
#define RS 39    //Right motor
#define LD 28    //Left motor direction pin
#define RD 29    //Right motor direction pin

int speedr = 0, speedl = 0; //sets the speed of motor using PWM
int ldir = 0, rdir = 0;  // 0 forward // 1 backward
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  Serial1.begin(9600);
  pinMode(LD, OUTPUT);
  pinMode(RD, OUTPUT);
}

void loop() {
   char c = 0;

  // put your main code here, to run repeatedly: 
  if(Serial1.available()){
    c = Serial1.read();
    Serial.println(c);
    }
  switch(c)
  {
    case 'F' : ldir = rdir = 0;  //Forward Movement
               speedr = 50;
               speedl = 50;
               digitalWrite(LD, ldir);
               digitalWrite(RD, rdir);
               analogWrite(LS, speedl);
               analogWrite(RS, speedr);
               break;
               
    case 'B' : ldir = rdir = 1;  //Backward Movement
               speedr = 50;
               speedl = 50;
               digitalWrite(LD, ldir);
               digitalWrite(RD, rdir);
               analogWrite(LS, speedl);
               analogWrite(RS, speedr);
               break;
               
    case 'R' : ldir = 0;  //Move towards left
               speedl = 50;
               speedr = 0;
               digitalWrite(LD, ldir);
               digitalWrite(RD, rdir);
               analogWrite(LS, speedl);
               analogWrite(RS, speedr);
               break;
               
    case 'L' : rdir = 0;  //Move towards right
               speedl = 0;
               speedr = 50;
               digitalWrite(LD, ldir);
               digitalWrite(RD, rdir);
               analogWrite(LS, speedl);
               analogWrite(RS, speedr);
               break;

    case 'P' : speedr = speedl = 0;  //stop
               digitalWrite(LD, ldir);
               digitalWrite(RD, rdir);
               analogWrite(LS, speedl);
               analogWrite(RS, speedr);
               break;
               
     default : speedr = speedl = 0;  //do nothing when no char
               break;
  }

delay(0);
}

Attachments area