Robot RX

#include <PWMServo.h>

//servos
int pos1 = 50;  //myservo1 pin 6;
int pos2 = 30;   //myservo2 pin 7;

PWMServo myservo1;
PWMServo myservo2;


//  Connections:
//- Pin 3 ---> PWMA
//- Pin 8 ---> AIN2
//- Pin 11 ---> AIN1
//- Pin 7 ---> STBY
//- Pin 12 ---> BIN1
//- Pin 13 ---> BIN2
//- Pin 5 ---> PWMB

//Motor 1
int pinAIN1 = 11; //Direction
int pinAIN2 = 8; //Direction
int pinPWMA = 3; //Speed

//Motor 2
int pinBIN1 = 12; //Direction
int pinBIN2 = 13; //Direction
int pinPWMB = 5; //Speed

//Standby
int pinSTBY = 7;

//Constants to help remember the parameters
static boolean turnCW = 0;  //for motorDrive function
static boolean turnCCW = 1; //for motorDrive function
static boolean motor1 = 0;  //for motorDrive, motorStop, motorBrake functions
static boolean motor2 = 1;  //for motorDrive, motorStop, motorBrake functions

//distance sensor
int sensorPin = A0;

//ledRED
int ledPinRED = 4;
int ledStateRED = LOW;

//ledGreen
int ledPinGREEN = 6;

//timer 
unsigned long previousMillis = 0;

void setup()
{
  
  Serial.begin(115200);
  
//Set the PIN Modes
  pinMode(pinPWMA, OUTPUT);
  pinMode(pinAIN1, OUTPUT);
  pinMode(pinAIN2, OUTPUT);

  pinMode(pinPWMB, OUTPUT);
  pinMode(pinBIN1, OUTPUT);
  pinMode(pinBIN2, OUTPUT);

  pinMode(pinSTBY, OUTPUT);
  
  Serial.begin(115200); 
  myservo1.attach(SERVO_PIN_B);
  myservo2.attach(SERVO_PIN_A);
  
  pinMode(ledPinRED, OUTPUT);
  pinMode(ledPinGREEN, OUTPUT);

}

void loop()
{
  while (Serial.available() > 0) {
    
    digitalWrite(ledPinGREEN, HIGH);
    
    int val = analogRead(sensorPin);
    
    val = map(val, 0, 700, 500, 20);
    val = constrain(val, 20, 500);
  
  
    long interval = val;
    unsigned long currentMillis = millis();

    if(currentMillis - previousMillis >= interval) {
    
      previousMillis = currentMillis;   
    
      if (ledStateRED == LOW)
        ledStateRED = HIGH;
      else
        ledStateRED = LOW;

    digitalWrite(ledPinRED, ledStateRED);   
  } 

    char msg = Serial.read();
    
    constrain(pos1, 50, 160);
    constrain(pos2, 30, 180);
     
    if(msg == 'u' && val > 250) {
      motorDrive(motor1, turnCW, 255);
      motorDrive(motor2, turnCW, 255);
    }
    if(msg == 'd') {
      motorDrive(motor1, turnCCW, 255);
      motorDrive(motor2, turnCCW, 255);
    }
    if(msg == 'l') {
      motorDrive(motor1, turnCCW, 192);
      motorDrive(motor2, turnCW, 255);
    }
    if(msg == 'r') {
      motorDrive(motor1, turnCW, 225);
      motorDrive(motor2, turnCCW, 192);
    }
    
    if(msg == 'j') //left
    {
      if(pos1 < 160) {myservo1.write(pos1+=3);}
    }
    if(msg == 'h') //right
    {      
      if(pos1 > 50) {myservo1.write(pos1-=3);}
    }
    if(msg == 'i') //down
    {      
      if(pos2 < 180) {myservo2.write(pos2+=3);}
    }
    if(msg == 'k') //up
    {      
     if(pos2 > 30) {myservo2.write(pos2-=3);}
    }
        
    else if(msg == 's') {
      motorBrake(motor1);
      motorBrake(motor2);
      motorsStandby();
    }
  }
}
    
    




void motorDrive(boolean motorNumber, boolean motorDirection, int motorSpeed)
{
  /*
  This Drives a specified motor, in a specific direction, at a specified speed:
    - motorNumber: motor1 or motor2 ---> Motor 1 or Motor 2
    - motorDirection: turnCW or turnCCW ---> clockwise or counter-clockwise
    - motorSpeed: 0 to 255 ---> 0 = stop / 255 = fast
  */

  boolean pinIn1;  //Relates to AIN1 or BIN1 (depending on the motor number specified)

 
//Specify the Direction to turn the motor
  //Clockwise: AIN1/BIN1 = HIGH and AIN2/BIN2 = LOW
  //Counter-Clockwise: AIN1/BIN1 = LOW and AIN2/BIN2 = HIGH
  if (motorDirection == turnCW)
    pinIn1 = HIGH;
  else
    pinIn1 = LOW;

//Select the motor to turn, and set the direction and the speed
  if(motorNumber == motor1)
  {
    digitalWrite(pinAIN1, pinIn1);
    digitalWrite(pinAIN2, !pinIn1);  //This is the opposite of the AIN1
    analogWrite(pinPWMA, motorSpeed);
  }
  else
  { 
    digitalWrite(pinBIN1, pinIn1);
    digitalWrite(pinBIN2, !pinIn1);  //This is the opposite of the BIN1
    analogWrite(pinPWMB, motorSpeed);
  }
   
 

//Finally , make sure STBY is disabled - pull it HIGH
  digitalWrite(pinSTBY, HIGH);

}

void motorBrake(boolean motorNumber)
{
/*
This "Short Brake"s the specified motor, by setting speed to zero
*/

  if (motorNumber == motor1)
    analogWrite(pinPWMA, 0);
  else
    analogWrite(pinPWMB, 0);
   
}


void motorStop(boolean motorNumber)
{
  /*
  This stops the specified motor by setting both IN pins to LOW
  */
  if (motorNumber == motor1) {
    digitalWrite(pinAIN1, LOW);
    digitalWrite(pinAIN2, LOW);
  }
  else
  {
    digitalWrite(pinBIN1, LOW);
    digitalWrite(pinBIN2, LOW);
  } 
}


void motorsStandby()
{
  /*
  This puts the motors into Standby Mode
  */
  digitalWrite(pinSTBY, LOW);
}