#define ENA 5
#define IN1 4
#define IN2 3
#define IN3 8
#define IN4 9
#define ENB 10
#define potPin 0
int Mspeed;

void setup() {
  Serial.begin(9600);
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(potPin,INPUT);
}

void loop() 
{ 
  Mspeed= analogRead(potPin)/4;
  Serial.println(Mspeed);
  delay(100);
  control_Robot(IN1,IN2,IN3,IN4,100);
 
}
void RobotMove(byte inR1, byte inR2, byte inL1, byte inL2,byte action)
{
  /*
   * inR1 và inR2 là 2 chân tín hiệu điều khiển động cơ bên phải
   * inL1 và inL2 là 2 chân tín hiệu điều khiển động cơ bên trái
   * action 0 : đứng yên
   * action 1 : đi thẳng
   * action 2 : lệch phải
   * action 3 : lệch trái
   */
  switch (action)
  {
    case 0: // đứng yên
        controlMotor_direction(inR1, inR2, 0);
        controlMotor_direction(inL1, inL2, 0);
        break;
    case 1: // di thang
        controlMotor_direction(inR1, inR2, 1);
        controlMotor_direction(inL1, inL2, 1);
        break;
    case 2: // lech trai
        controlMotor_direction(inR1, inR2, 2);
        controlMotor_direction(inL1, inL2, 1);
        break;
     case 3: // lech phai
        controlMotor_direction(inR1, inR2, 1);
        controlMotor_direction(inL1, inL2, 2);
        break;
     default:
        action=0;
  }
}
void controlMotor_direction(byte in1, byte in2, byte direction)
{
// in1,in2 là tín hiệu điều khiển hướng motor
//0 : stop
//1 : di cùng chiều
//2 : đi ngược chiều

     switch (direction)
     {
        case 0:  // đứng im
        digitalWrite (in1, LOW);
        digitalWrite (in2, LOW);
        break;
        case 1:  // quay cùng chiều
        digitalWrite (in1, HIGH);
        digitalWrite (in2, LOW);
        break;
        case 2:  // quay ngược chiều
        digitalWrite (in1, LOW);
        digitalWrite (in2, HIGH);
        break;      
     }
}
void control_Robot(byte inR1, byte inR2, byte inL1, byte inL2, byte center_distance)
{
//  RobotMove(inR1,inR2,inL1,inL2,1);
//  Serial.println("Tiến");
  int distance = Mspeed;
  if(distance = center_distance)
    {
      RobotMove(inR1,inR2,inL1,inL2,1);
      analogWrite(ENA,Mspeed);
      analogWrite(ENB,Mspeed);
      Serial.println("đi thẳng");
      delay(1000);
    }
  if (distance > center_distance)
    {
      RobotMove(inR1,inR2,inL1,inL2,3);
      analogWrite(ENA,Mspeed);
      analogWrite(ENB,Mspeed);
      Serial.println("lệch phải");
      delay(1000);
    }

  if (distance < center_distance)
    {
      RobotMove(inR1,inR2,inL1,inL2,2);
      analogWrite(ENA,Mspeed);
      analogWrite(ENB,Mspeed);
      Serial.println("lệch trái");
      delay(1000);   
    }
}