Elegoo Drive Test
//Elegoo
// Driving Sample
//Jrich 9/2022 for V4
// Initialize Variables
#include <Servo.h>
int servo_Pin = 10; // Servo Control Pin
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
//Driving Subroutines
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!");
}
// Run Once Setup
void setup()
{
Serial.begin(9600);
pinMode(DirR,OUTPUT);
pinMode(DirL,OUTPUT);
pinMode(MovR,OUTPUT);
pinMode(MovL,OUTPUT);
pinMode(Active,OUTPUT);
digitalWrite(Active,HIGH);
Stop();
}
// Looping Section
void loop()
{
Forward(50);
delay (1000);
Back(50);
delay (1000);
Right(50);
delay (1000);
Left(50);
delay (1000);
Stop();
delay (1000);
}