​

/*
 * Modified by Yohi Zucker
 * December 2019
 * 
 */
int mot1 = 9;//pin 9 to IN1
int mot2 = 6;//pin 6 to IN2
int mot3 = 5;//pin 5 to In3
int mot4 = 3;//pin 3 to In4
//Both ENA pins should be shorten with a jumper to allowed PWM controlling

int left = 13;//pin 13 to RIGHT IR sensor to control LEFT movements
int right = 12;//pin 12 to LEFT IR sensor to control RIGHT movements

int Left = 0;
int Right = 0;

void LEFT (void);
void RIGHT (void);
void STOP (void);

void setup()
{
  pinMode(mot1, OUTPUT);
  pinMode(mot2, OUTPUT);
  pinMode(mot3, OUTPUT);
  pinMode(mot4, OUTPUT);

  pinMode(left, INPUT);
  pinMode(right, INPUT);

  digitalWrite(left, HIGH);
  digitalWrite(right, HIGH);

}

void loop()
{

  analogWrite(mot1, 80);
  analogWrite(mot2, 0);
  analogWrite(mot3, 80);
  analogWrite(mot4, 0);

  while (1)
  {
    Left = digitalRead(left);
    Right = digitalRead(right);

    if ((Left == 0 && Right == 1) == 1)
      LEFT();
    else if ((Right == 0 && Left == 1) == 1)
      RIGHT();
  }
}

void LEFT (void)
{
  analogWrite(mot3, 0);
  analogWrite(mot4, 30);

  while (Left == 0)
  {
    Left = digitalRead(left);
    Right = digitalRead(right);
    if (Right == 0)
    {
      int lprev = Left;
      int rprev = Right;
      STOP();
      while (((lprev == Left) && (rprev == Right)) == 1)
      {
        Left = digitalRead(left);
        Right = digitalRead(right);
      }
    }
    analogWrite(mot1, 80);
    analogWrite(mot2, 0);
  }
  analogWrite(mot3, 80);
  analogWrite(mot4, 0);
}

void RIGHT (void)
{
  analogWrite(mot1, 0);
  analogWrite(mot2, 30);

  while (Right == 0)
  {
    Left = digitalRead(left);
    Right = digitalRead(right);
    if (Left == 0)
    {
      int lprev = Left;
      int rprev = Right;
      STOP();
      while (((lprev == Left) && (rprev == Right)) == 1)
      {
        Left = digitalRead(left);
        Right = digitalRead(right);
      }
    }
    analogWrite(mot3, 80);
    analogWrite(mot4, 0);
  }
  analogWrite(mot1, 80);
  analogWrite(mot2, 0);
}
void STOP (void)
{
  analogWrite(mot1, 0);
  analogWrite(mot2, 0);
  analogWrite(mot3, 0);
  analogWrite(mot4, 0);

}