//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);

}

Last modified: Wednesday, 1 March 2023, 4:51 PM