Arduino Challenge09

//RobotCarMeimei7_2DCMotorsWizUltrasonic

//Start 2020/03/08

//Up 2020/03/08

int E1=6;  //Left wheel

int M1=7;

int E2=5;  //Right wheel 

int M2=4;

int TriggerPin1=8;

int EchoPin1=9;

double Duration1;

double Distance1;

boolean OnFlg=false;

void Forward()

{

  digitalWrite(M1,HIGH);

  digitalWrite(M2,LOW);

  analogWrite(E1,250);

  analogWrite(E2,250);

}

void Back()

{

  digitalWrite(M1,LOW);

  digitalWrite(M2,HIGH);

  analogWrite(E1,250);

  analogWrite(E2,250);

}

void Stop()

{

  digitalWrite(M1,LOW);

  digitalWrite(M2,HIGH);

  analogWrite(E1,0);

  analogWrite(E2,0);

}

void TurnRight()

{

  digitalWrite(M1,HIGH);

  digitalWrite(M2,LOW);

  analogWrite(E1,250);

  analogWrite(E2,150);

}

void TurnLeft()

{

  digitalWrite(M1,HIGH);

  digitalWrite(M2,LOW);

  analogWrite(E1,150);

  analogWrite(E2,250);

}

void QuickTurnRight()

{

  digitalWrite(M1,HIGH);

  digitalWrite(M2,HIGH);

  analogWrite(E1,250);

  analogWrite(E2,250);

}

void QuickTurnLeft()

{

  digitalWrite(M1,LOW);

  digitalWrite(M2,LOW);

  analogWrite(E1,250);

  analogWrite(E2,250);

}

void setup()

{

  pinMode(M1,OUTPUT);

  pinMode(M2,OUTPUT);

  pinMode(TriggerPin1,OUTPUT);

  pinMode(EchoPin1,INPUT);

}

void loop() 

{

  digitalWrite(TriggerPin1,LOW);

  delayMicroseconds(2);

  digitalWrite(TriggerPin1,HIGH);

  delayMicroseconds(10);

  digitalWrite(TriggerPin1,LOW);

  Duration1=pulseIn(EchoPin1,HIGH); 

  if (Duration1 > 0 )

  {

    Duration1=Duration1 /2;

    Distance1=Duration1 *340 *100 /1000000;

    if (Distance1 >= 30)

      {

        if(!OnFlg)

        {

          Forward();

          OnFlg=true;

        }

      }

      else

      {

        OnFlg=false;

        Stop();

        delay(200);

        Back();

        delay(1000);

        QuickTurnLeft();

        delay(1000);

        Forward();       

      }

    }

  delay(200);  

}