Test All Routine - V4

//Elegoo
//2016.09.12
//
//Jrich 2/2018 for V2
// Modified 3/19 to remove servos
// 9/2022 for V4

int track1 = A0; // Track 1 Input
int track2 = A1; // Track 2 Input
int track3 = A2; // Track 3 Input
int Echo = 12; // Echo Pin for UltraSonic Rangefinder
int Trig = 13; // Trigger for UltraSonic Rangefinder
int Active = 3; // Set Motor Control Active - HIGH = On LOW = Off
int MovR = 5; // Analog Speed 0-254
int MovL = 6; // Analog Speed 0-254
int DirR = 7; // HIGH = Forward LOW = Back
int DirL = 8; // HIGH = Forward LOW = Back
int distance; // distance in cm
int duration; // time for sound to travel and echo to return

void Forward(int speedValue)
{
digitalWrite(Active,HIGH);
digitalWrite(DirR,HIGH);
digitalWrite(DirL,HIGH);
analogWrite(MovR,speedValue);
analogWrite(MovL,speedValue);
Serial.print(speedValue);
Serial.println(" go forward!");
}

void Back(int speedValue)
{
digitalWrite(Active,HIGH);
digitalWrite(DirR,LOW);
digitalWrite(DirL,LOW);
analogWrite(MovR,speedValue);
analogWrite(MovL,speedValue);
Serial.print(speedValue);
Serial.println(" go back!");
}

void Left(int speedValue)
{
digitalWrite(DirR,HIGH);
digitalWrite(DirL,LOW);
analogWrite(MovR,speedValue);
analogWrite(MovL,speedValue);
Serial.print(speedValue);
Serial.println(" go left!");
}

void Right(int speedValue)
{
digitalWrite(DirR,LOW);
digitalWrite(DirL,HIGH);
analogWrite(MovR,speedValue);
analogWrite(MovL,speedValue);
Serial.print(speedValue);
Serial.println(" go right!");
}
void Stop()
{
digitalWrite(Active,LOW);
Serial.println("Stop!");
}
/*Ultrasonic distance measurement Sub function*/
int Distance_test()
{
digitalWrite(Trig, HIGH);
delayMicroseconds(1000);
digitalWrite(Trig, LOW);
duration = pulseIn(Echo, HIGH);
distance = (duration/2) / 29.1;
return distance;
}

void setup()
{

Serial.begin(9600);
pinMode(12, OUTPUT);
pinMode(Echo, INPUT);
pinMode(Trig, OUTPUT);
pinMode(DirR,OUTPUT);
pinMode(DirL,OUTPUT);
pinMode(MovR,OUTPUT);
pinMode(MovL,OUTPUT);
pinMode(Active,OUTPUT);
digitalWrite(Active,HIGH);

Stop();
}

void loop()
{
Distance_test();
Serial.print(" Distance=");
Serial.println(distance);


Serial.print(" track1 = ");
Serial.println(digitalRead(track1));
Serial.print(" track2 = ");
Serial.println(digitalRead(track2));
Serial.print(" track3 = ");
Serial.println(digitalRead(track3));

Forward(50);
delay (1000);

Back(50);
delay (1000);

Right(50);
delay (1000);

Left(50);
delay (1000);

Stop();
delay (1000);
}