Rowan Mottram's BOE Bot code (QMUL 2013)
BOE bot with IR navigation.
/*
* Robotics with the BOE Shield - Maze Navigation
* This code was designed for the lab floor with less than satisfactory IR Sensors that required delicate positioning.
* These numbers can be modified in the area of code beneath the library call
*/
#include <Servo.h> // Include servo library
/*
* This section contains variables to represent numbers that might need some changing due to the surfaces. The suggest values are the numbers used in the project in general.
*/
int SpinDetectRotation = 300; // Largely dependant on the surface. You want it so that each rotation results in SpinDetectAnglePer degrees around. Values 220, 300 worked.
int SpinDetectAnglePer = 13; // Should be kept small so it doesn't skip an area with no IR detection Values 9 , 13 respectively worked.
int ReverseUponSingleDetection = 60; // The time (ms) that the robot reverses for after it has detected an object to either its left or right (but not both) Values 40, 60 worked
int RotateUponSingleDetection = 250; // The time (ms) that the robot rotates for after it has detected an object to either its left or right (but not both) Values 200, 300 worked
int RepeatBumpReverseFactor = 60; // The time (ms) multipled per repeated collision the robot should reverse for. Values 60-100 worked
int RepeatBumpRotateFactor = 40; // The time (ms) multipied per repeated collision the robot should rotate for Values 40-100 worked
int LongTimeNoDetectionReverseFactor = 500; // The time (ms) that the robot should reverse for if it hasn't detected anything for a while Values 500-700 worked
int LongTimeNoDetectionRotateFactor = 500; // in the situation it is unable to detect an object it is stuck on Values 300-700 worked
/*
* The main body of the code follows
*/
Servo servoLeft; // Declare left and right servos
Servo servoRight;
int lcounter = 0; // List of variables in use
int rcounter = 0;
int fcounter = 0;
int randy = 0;
void setup()
{
pinMode(10, INPUT); pinMode(9, OUTPUT); // Left IR LED & Receiver
pinMode(3, INPUT); pinMode(2, OUTPUT); // Right IR LED & Receiver
pinMode(8, OUTPUT); pinMode(7, OUTPUT); // Indicator LEDs
//Serial.begin(9600); // For debugging on serial monitor
tone(4, 3200, 200); // Initialization tone
delay(250);
tone(4, 3200, 200);
delay(1000);
servoLeft.attach(13);
servoRight.attach(12);
}
void loop()
{
int irLeft = irDetect(9, 10, 44000); // Detect object on left, if = 0, detection has occurred
int irRight = irDetect(2, 3, 44500); // Detect object on right, if = 0, detection has occurred
if((irLeft == 0) && (irRight == 0)) // If object is detected infront
{
move(-90, -70, 60); // Move back
fcounter = 0; // Not moving forwards excessively, reset counter
int ALocal = 0; // Going to be rotating, set ALocal to 0 again
while((irLeft == 0) && (irRight == 0)) // While object is detected infront,
{
digitalWrite(7, HIGH); // Brighten both LEDS
digitalWrite(8, HIGH);
move(0,0,10); // Stop
move(-90,-70,40); // Reverse
move(0,0,10); // Stop
move(-200,200,(SpinDetectRotation)); // Rotate
ALocal = ALocal+(SpinDetectAnglePer); // Add some angle change
if((irLeft != 0) && (irRight != 0)) // If it is able to escape but the angle change is not as required in the below condition, turn LEDS off
{
digitalWrite(7, LOW); // Dim both LEDs
digitalWrite(8, LOW);
}
int irLeft = irDetect(9, 10, 44000); // Redetect if the object is still there (since there has been a delay of 360 above)
int irRight = irDetect(2, 3, 44500);
if(((irLeft != 0) && (irRight != 0)) && ((ALocal > 225) || (ALocal < 145))) // If angle is not 180±45 degree turn and there is no object detected
{
break; // Escape from the while.
}
} // If it didn't escape from the while it will return to [1] above
}
else if((irLeft == 0) && (irRight != 0) && (lcounter < 3)) // If an object is detected to the left and no object is detected to the right
{
digitalWrite(8, HIGH); // Light up the left LED
digitalWrite(7, LOW); // Dim the right LED
move(-90,-70,(ReverseUponSingleDetection)); // Slight move backwards
move(200,-200,(RotateUponSingleDetection)); // Rotate
move(90, 70, 80); // Slight move forwards
move(0,0,10); // Pause to allow accurate motion.
rcounter = (0); // Set the amount of times the object has been to the right to 0 (since it just did the left option)
lcounter = (lcounter+1); // Add to the amount of the times the object has been to the left of the object - See later for what this causes
fcounter = (0); // Reset the forward counter - see later
}
else if((irLeft != 0) && (irRight == 0) && (rcounter < 3)) // The above code except left is considered right
{
digitalWrite(7, HIGH);
digitalWrite(8, LOW);
move(-90,-70,(ReverseUponSingleDetection));
move(-200,200,(RotateUponSingleDetection));
move(90,70,80);
move(0,0,10);
lcounter = (0);
rcounter = (rcounter+1);
fcounter = (0);
}
else if((irLeft == 0) && (irRight != 0) && (lcounter > 2)) // After the object has detected something to the left repeatedly, this being the third time
{
digitalWrite(8, HIGH);
digitalWrite(7, LOW);
move(-90,-70,((ReverseUponSingleDetection)+((RepeatBumpReverseFactor)*lcounter))); // Move backwards by an amount dependant on the left counter value. The more times it hits something, the more it moves back
move(200,-200,((RotateUponSingleDetection)+((RepeatBumpRotateFactor)*lcounter))); // Rotate by an amount dependant on the left counter value. The more times it hits something, the more it rotates
move(90,70,80);
move(0,0,10); // Pause to allow accurate motion.
fcounter = 0;
}
else if((irLeft != 0) && (irRight == 0) && (rcounter > 2)) // The above code, except left is considered right
{
digitalWrite(7, HIGH);
digitalWrite(8, LOW);
move(-90,-70,((ReverseUponSingleDetection)+((RepeatBumpReverseFactor)*rcounter))); // The value for 100*rcounter might need to be decreased to 60
move(-200,200,((RotateUponSingleDetection)+((RepeatBumpRotateFactor)*rcounter))); // The value for 100*rcounter might need to be decreased to 40
move(90,70,80);
move(0,0,10);
fcounter = 0;
}
else if((irLeft != 0) && (irRight != 0) && (fcounter > 15)) // [2] When the object has reached a forward counter of 15 it will assume it is stuck unable to detect the obstacle
{
randy = random(1,2); // Pick between 1 and 2
if(randy == 1)
{
move(-90,-70,(LongTimeNoDetectionReverseFactor)); // Large reverse
move(-200,200,(LongTimeNoDetectionRotateFactor)); // Large rotation
move(90,70,80);
move(0,0,10); // Pause to allow accurate motion.
fcounter = 0;
}
else if(randy == 2) // The above code except rotating the opposite direction
{
move(-90,-70,(LongTimeNoDetectionReverseFactor));
move(200,-200,(LongTimeNoDetectionRotateFactor));
move(90,70,80);
move(0,0,10);
fcounter = 0;
}
}
else // If nothing satisfies the conditions above (so no obstructions), it will move forwards
{
digitalWrite(7, LOW); // No obstacles, so dim the LEDs
digitalWrite(8, LOW);
move(90,70,250); // Move forwards a set amount
move(0,0,10); // Pause to allow accurate motion.
fcounter = (fcounter+1); // Each bit of movement forwards adds 1 to the counter. Eventually it will stop and carry out the code at [2] because it >might< be stuck.
}
}
int irDetect(int irLedPin, int irReceiverPin, long frequency) // Important stuff about the IR sensors.
{
tone(irLedPin, frequency, 8); // Emit some frequency wave
delay(1); // Wait 1ms
int ir = digitalRead(irReceiverPin); // Read if there is a frequency returned (the wave has bounced off the object into the sensor)
delay(1); // Wait 1ms
return ir; // Return 1 is no wave detected, 0 if wave detected.
}
void move(int left, int right, int time) // Simple movement function.
{
servoLeft.writeMicroseconds(1500 + left);
servoRight.writeMicroseconds(1500 - right);
delay(time);
}