/*
Testing autonomous guidance with simple if statements
3/14/17 by Brian Patton
Feel free to do whatever you want with this code example
*/
#include <Servo.h>
Servo R_Servo; // create servo object to control a servo
Servo L_Servo; // create servo object to control a servo
const int lPhoto = A1; // Left Photoresistor
const int rPhoto = A0; // Right Photoresistor
const int sharpPin = A2; // Sharp Sensor
const int LED = 13; // Onboard LED location
int lPhotoVal; // Variable to store L photoresistor value
int rPhotoVal; // Variable to store R photoresistor value
int sharpVal; // Variable to store Sharp Sensor value
int valDif; // Variable to store difference between photo values
//**************************************************************
//***************** Setup ************************************
//**************************************************************
void setup()
{
Serial.begin(9600); // OPen Serial Port
pinMode(LED, OUTPUT); // Set LED port direction to output
R_Servo.attach(2); // Attach R Servo
L_Servo.attach(1); // Attach L Servo
for (int i = 4; i >= 0; i--) { //Flash LED 4 times
digitalWrite(LED, HIGH);
delay(500);
digitalWrite(LED, LOW);
delay(500);
}
}
//***************** Forward(int Dlay) ***********************
// Move the robot Slowly Forward
//**************************************************************
void Forward(int Dlay)
{
R_Servo.writeMicroseconds(1450); // sets the servo position
L_Servo.writeMicroseconds(1620); // sets the servo position
delay(Dlay);
}
//***************** Reverse(int Dlay) ***********************
// Reverse the robot
//**************************************************************
void Reverse(int Dlay)
{
R_Servo.writeMicroseconds(2000); // sets the servo position
L_Servo.writeMicroseconds(1000); // sets the servo position
delay(Dlay);
}
//***************** stopBot(int Dlay) ***********************
// Stop the robot
//**************************************************************
void stopBot(int Dlay)
{
R_Servo.writeMicroseconds(1500); // sets the servo position
L_Servo.writeMicroseconds(1500); // sets the servo position
delay(Dlay);
}
//***************** TLeftSlow(int Dlay) ***********************
// Slow Left Turn for a duration
//**************************************************************
void TLeftSlow(int Dlay)
{
R_Servo.writeMicroseconds(1500); // sets the servo position
L_Servo.writeMicroseconds(1600); // sets the servo position
delay(Dlay);
}
//***************** TRightSlow(int Dlay) ***********************
// Slow Right Turn for a duration
//**************************************************************
void TRightSlow(int Dlay)
{
R_Servo.writeMicroseconds(1450); // sets the servo position
L_Servo.writeMicroseconds(1500); // sets the servo position
delay(Dlay);
}
//******************** checkSensors() **************************
// Check value of Sensors Stop bot if object is close
//**************************************************************
void checkSensors()
{
rPhotoVal = analogRead(rPhoto);
lPhotoVal = analogRead(lPhoto);
valDif = abs(rPhotoVal - lPhotoVal); // looking for threshold
sharpVal = analogRead(sharpPin);
if (sharpVal >= 500) {
stopBot(1000);
}
}
//******************** printSensors() **************************
// Print the sensor values Slows robot when in use!!!
//**************************************************************
void printSensors() {
Serial.println("Right Value = " + (String)rPhotoVal);
Serial.println("Left Value = " + (String)lPhotoVal);
Serial.println("Difference Value = " + (String)valDif);
Serial.println(" ");
delay(100);
}
//************************ loop() ****************************
//********************** Main Loop ***************************
//**************************************************************
void loop() {
checkSensors();
while (lPhotoVal > 1000) { //slowly turn until a light is seen
TLeftSlow(5);
checkSensors(); // While checking sensors
}
if (valDif > 70) { // If the difference between senors is big enough.....
if (lPhotoVal > rPhotoVal) {// If L photo is bigger than R photo
TLeftSlow(5); // Turn a speck to the Left
// Serial.println("left");
}
else { //Or if R photo is bigger than L photo
TRightSlow(5); // Turn a speck to the Right
// Serial.println("Right");
}
}
else { // If the difference between sensors wasn't big enough....
Forward(50); // Go forward a bit.
// Serial.println("forward");
}
// printSensors();
}