//Elegoo
//2016.09.12
//
//Jrich 2/2018 for V2
// Modified 3/19 to remove servos
// 9/2022 for V4
// 1/24 - no servo test
// To Do
// Test All as Arduino Web with Chromebook
// Servo Control
// Create Individual Tests
#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
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;
}
int Servo_test(int angle)
{
int servo_Pin = 10;
Servo myservo; // create servo object to control a servo myservo.begin();
myservo.attach(servo_Pin);
myservo.write(angle); //sets the servo position according to the 90（middle）
delay(250);
}
void setup()
{
// int servo_Pin = 10;
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);
// delay(250);
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);
}