Untitled

#include <DRV8835MotorShield.h>
#include <NewPing.h>
#define MAX_DISTANCE 25

#define TRIGGER_PIN_LEFT  4
#define ECHO_PIN_LEFT     3

#define TRIGGER_PIN_CENTER 12
#define ECHO_PIN_CENTER 2

#define TRIGGER_PIN_RIGHT 11
#define ECHO_PIN_RIGHT 5

#define LEFT_MOTOR 1
#define RIGHT_MOTOR 0

#define STOP           3
#define STRAIGHT_AHEAD 0   
#define LEFT           1
#define RIGHT          2

DRV8835MotorShield motors;
NewPing sonarLeft(TRIGGER_PIN_LEFT, ECHO_PIN_LEFT, MAX_DISTANCE);
NewPing sonarRight(TRIGGER_PIN_RIGHT, ECHO_PIN_RIGHT, MAX_DISTANCE);
NewPing sonarCenter(TRIGGER_PIN_CENTER, ECHO_PIN_CENTER, MAX_DISTANCE);

unsigned int MAX_SPEED = 50;
unsigned int MIN_SPEED = 10;
unsigned int LEFT_MOTOR_PWM = 0;
unsigned int RIGHT_MOTOR_PWM = 0;
unsigned int LAST_COMMAND_DIRECTION = STRAIGHT_AHEAD;
unsigned int ACTUAL_COMMAND_DIRECTION = STRAIGHT_AHEAD;
unsigned int LAST_COMMAND_SPEED = MAX_SPEED;

float kp = 5;
float kd = 2;
int lastErrorLeft = 0;
int lastErrorRight = 0;
int countLeft = 0;
int countRight = 0;
int flagLeft = 0;
int flagRight = 0;

unsigned long leftDistance = 0;
unsigned long rightDistance = 0;
unsigned long frontDistance = 0;
int optimDistance = 0;

void setup() {
  Serial.begin(115200);

}

void loop() {
  
  delay(50);
  rightDistance = sonarRight.ping_cm();
  delay(50);
  frontDistance = sonarCenter.ping_cm();
  delay(50);
  leftDistance = sonarLeft.ping_cm();
  //Serial.print("left:");
  //Serial.println(left_distance);
  //Serial.print("right:");
  //Serial.println(right_distance);
  readSerial();
  run();
  doLeft();
  doRight();
  doStop();
  parking();
}

unsigned int serialInput;
void readSerial(){
  if(Serial.available())
  {
    serialInput = Serial.read() - '0';
    Serial.println(serialInput);
    LAST_COMMAND_DIRECTION=ACTUAL_COMMAND_DIRECTION;
    switch(serialInput)
    {
      case 0://merge inainte
        ACTUAL_COMMAND_DIRECTION = STRAIGHT_AHEAD;
      break;
      case 1://viraj la stanga
        ACTUAL_COMMAND_DIRECTION = LEFT;
        flagLeft = 0;
        flagRight = 0;
        countRight = 0;
        countLeft = 0;
      break;
      case 2://viraj la dreapta
        ACTUAL_COMMAND_DIRECTION = RIGHT;
        flagLeft = 0;
        flagRight = 0;
        countRight = 0;
        countLeft = 0;
      break;
      case 3://oprire
         ACTUAL_COMMAND_DIRECTION = STOP;
         flagLeft = 0;
         flagRight = 0;
         countRight = 0;
         countLeft = 0;
      break;
    }
  }   

  LAST_COMMAND_SPEED = MAX_SPEED;
}
void run(){
  optimDistance=10;
  if(leftDistance != 0 && rightDistance !=0 &&  leftDistance < 10 && rightDistance < 10){
    optimDistance = (leftDistance + rightDistance)/2;
  }
  if(leftDistance == 0 && rightDistance == 0 && flagRight == 0 && flagLeft == 0){
    setMotorPwm(LEFT_MOTOR,LAST_COMMAND_SPEED);
    setMotorPwm(RIGHT_MOTOR,LAST_COMMAND_SPEED+4);
    controlMotors();
  }
  if((leftDistance < 12 && flagRight == 0) || flagLeft == 1)//<13
     pidControlLeft();
  if((rightDistance < 12 && flagLeft == 0) || flagRight == 1)
     pidControlRight();
}
void doLeft(){
  if(ACTUAL_COMMAND_DIRECTION == STRAIGHT_AHEAD && LAST_COMMAND_DIRECTION == LEFT){
    Serial.print('L');
    flagLeft = 1;
    countLeft++;
  }
  if(countLeft >= 25){
    flagLeft = countLeft = 0;
    LAST_COMMAND_DIRECTION = STRAIGHT_AHEAD;
  } 
}

void doRight(){ 
  if(ACTUAL_COMMAND_DIRECTION == STRAIGHT_AHEAD && LAST_COMMAND_DIRECTION == RIGHT){
    Serial.print('R');
    flagRight = 1;
    countRight++;
  }
  if(countRight >= 25){
    flagRight = countRight = 0;
    LAST_COMMAND_DIRECTION = STRAIGHT_AHEAD;
  }    
}
void pidControlLeft(){
 if(leftDistance == 0 && flagLeft == 1){
    leftDistance = 17;
    optimDistance = 7;
 }
 if(flagLeft == 1)
   optimDistance = 7;
 if(leftDistance != 0){
  int error = leftDistance - optimDistance;
  int motorSpeed = kp * error + kd * (error - lastErrorLeft);
  //Serial.println("pid_left");
  //Serial.println(motorSpeed);
  lastErrorLeft = error;
  int rightMotorSpeed = LAST_COMMAND_SPEED + motorSpeed;
  int leftMotorSpeed = LAST_COMMAND_SPEED - motorSpeed;
  if(rightMotorSpeed > MAX_SPEED) 
    rightMotorSpeed = MAX_SPEED;
  if(leftMotorSpeed > MAX_SPEED) 
    leftMotorSpeed = MAX_SPEED;
  if(rightMotorSpeed < 0) 
    rightMotorSpeed = MIN_SPEED;
  if(leftMotorSpeed < 0) 
    leftMotorSpeed = MIN_SPEED; 
    
  setMotorPwm(LEFT_MOTOR,leftMotorSpeed);
  setMotorPwm(RIGHT_MOTOR, rightMotorSpeed);
  controlMotors();
 }
}
void pidControlRight(){
  if(rightDistance == 0 && flagRight == 1){
    rightDistance = 17;
    optimDistance= 7;
  }
  if(flagRight == 1)
    optimDistance = 7;
  if(rightDistance != 0){
    int error = rightDistance - optimDistance;
    int motorSpeed = kp * error + kd * (error - lastErrorRight);
    //Serial.println("pid_right");
    //Serial.println(motorSpeed);
    lastErrorRight = error;
    int rightMotorSpeed = LAST_COMMAND_SPEED - motorSpeed;
    int leftMotorSpeed = LAST_COMMAND_SPEED + motorSpeed;
    if(rightMotorSpeed > MAX_SPEED) 
      rightMotorSpeed = MAX_SPEED;
    if(leftMotorSpeed > MAX_SPEED)
      leftMotorSpeed = MAX_SPEED;
    if(rightMotorSpeed < 0)
      rightMotorSpeed = MIN_SPEED;
    if(leftMotorSpeed < 0)
      leftMotorSpeed = MIN_SPEED; 
  
  setMotorPwm(LEFT_MOTOR,leftMotorSpeed);
  setMotorPwm(RIGHT_MOTOR, rightMotorSpeed);
  controlMotors();
  }
}
void doStop(){
  if(ACTUAL_COMMAND_DIRECTION == STRAIGHT_AHEAD && LAST_COMMAND_DIRECTION == STOP)
  {
    //if((right_distance ==0 && left_distance == 0) || (right_distance==0 && left_distance!= 0) || (right_distance != 0 && left_distance==0))
   // {
       setMotorPwm(LEFT_MOTOR,0);
       setMotorPwm(RIGHT_MOTOR,0);
       controlMotors();
       LAST_COMMAND_DIRECTION = STRAIGHT_AHEAD;
       delay(3000);
       setMotorPwm(LEFT_MOTOR,LAST_COMMAND_SPEED);
       setMotorPwm(RIGHT_MOTOR,LAST_COMMAND_SPEED+4);
       controlMotors();
    // }
  }
}

void parking(){
   if(ACTUAL_COMMAND_DIRECTION == STRAIGHT_AHEAD && LAST_COMMAND_DIRECTION == STRAIGHT_AHEAD){
     if(rightDistance != 0 && leftDistance != 0 && frontDistance >= 1 && frontDistance <= 3){
       setMotorPwm(LEFT_MOTOR,0);
       setMotorPwm(RIGHT_MOTOR,0);
       controlMotors();
       delay(3000);
       Serial.println("PARKING_MODE");
       while(1);
     }
   }
}

void setMotorPwm(boolean motor,unsigned int motorPwm){
  (motor)?(LEFT_MOTOR_PWM = motorPwm):(RIGHT_MOTOR_PWM = motorPwm);
}

void controlMotors(){
  int motSpdLeft = 0;
  int motSpdRight = 0;
  motSpdLeft = map(LEFT_MOTOR_PWM,0,100,0,400);
  motSpdRight = map(RIGHT_MOTOR_PWM,0,100,0,400);
  motors.setM1Speed(motSpdLeft);
  motors.setM2Speed(motSpdRight);
}