Hi all,
I'm fairly new at this so please bare with me!
I'm building a robot and have been working on various logic processes or algorithms to get it to navigate a room....some more advanced than others, so I'm starting to try to program the basic one to begin with!
The intention of the robot is to make it's way to the top right corner of a room.
I have included my logic process and the code i have written so far. I'm having a few issues converting the logic into code but the one i particularly need help with is:
at the moment the robot turns left or right dependent on it's surroundings, but i want it to follow the logic process with regards to getting back on track. For example
if front <=15cm and left <=15cm
turn right
move forward until left >15cm
turn left
move forward
or another branch on the logic might call for the vehicle to turn 180 and back track before getting back on track.
Do i need to code every possible eventuality (to an extent) or is there some easy way of writing a sub-routine and calling it in a particular situation?
I hope this makes sense, hopefully the logic and code will help!
Many thanks in advance
Alex
const int numOfReadingsLeft = 10;
int readingsLeft[numOfReadingsLeft];
int arrayIndexLeft = 0;
int totalLeft = 0;
int averageDistanceLeft = 0;
const int numOfReadingsFront = 10;
int readingsFront[numOfReadingsFront];
int arrayIndexFront = 0;
int totalFront = 0;
int averageDistanceFront = 0;
const int numOfReadingsRight = 10;
int readingsRight[numOfReadingsRight];
int arrayIndexRight = 0;
in totalRight = 0;
int averageDistanceRight = 0;
// Setup pins for three SRF-05’s //
int durationLeft; // Stores the duration of the pulse
int distanceLeft; // Stores the distance
int durationFront;
int distanceFront;
int durationRight;
int distanceRight;
int pingPinLeft = 3; // Sets the ping pin to pin 3
int echoPinLeft = 4; // Sets the echo pin to pin 4
int pingPinFront = 5;
int echoPinFront = 6;
int pingPinRight = 7;
int echoPinRight = 8;
// Setup Pins for two Motors //
int MotorLeftPin1 = 2;
int motorLeftPin2 = 11;
int enableMotorLeft = 9;
int motorRightPin1 = 12;
int motorRightPin2 = 13;
int enableMotorRight = 10;
// Set motor pins to I/O //
void setup ( ) {
pinMode(motorLeftPin1, OUTPUT);
pinMode(motorLeftPin2, OUTPUT);
pinMode(enableMotorLeft, OUTPUT);
pinMode(motorRightPin1, OUTPUT);
pinMode(motorRightPin2, OUTPUT);
pinMode(enableMotorRight, OUTPUT);
digitalWrite(enableMotorLeft, HIGH);
digitalWrite(enableMotorRight, HIGH);
// Setup serial port //
void setup() {
Serial.begin(9600);
}
// Set SRF-05 pins to I/O //
void setup() {
pinMode(pingPinLeft, OUTPUT);
pinMode(pingPinFront, OUTPUT);
pinMode(pingPinRight, OUTPUT);
pinMode(echoPinLeft, INPUT);
pinMode(echoPinFront, INPUT);
pinMode(echoPinRight, INPUT);
// create array loop to iterate over every item in the array
for (int thisReadingLeft = 0; thisReadingLeft < numOfReadingsLeft;
thisReadingLeft++) { readingsLeft[thisReadingLeft] = 0
}
for (int thisReadingFront = 0; thisReadingFront < numOfReadingsFront;
thisReadingFront++) { readingsFront[thisReadingFront] = 0
}
for (int thisReadingRight = 0; thisReadingRight < numOfReadingsRight;
thisReadingRight++) { readingsRight[thisReadingRight] = 0
}
}
void loop() {
digitalWrite(pingPinLeft, HIGH);
delayMicroseconds(10);
digitalWrite(pingPinLeft, LOW);
digitalWrite(pingPinFront, HIGH);
delayMicroseconds(10);
digitalWrite(pingPinFront, LOW);
digitalWrite(pingPinRight, HIGH);
delayMicroseconds(10);
digitalWrite(pingPinRight, LOW);
pulseTimeLeft = pulseInLeft(echoPinLeft, HIGH);
distanceLeft = pulseTimeLeft/58;
pulseTimeFront = pulseInFront(echoPinFront, HIGH);
distanceFront = pulseTimeLeft/58;
pulseTimeRight = pulseInRight(echoPinRight, HIGH);
distanceRight = pulseTimeRight/58;
totalLeft = totalLeft – readingsLeft[arrayIndexLeft];
readingsLeft[arrayIndexLeft] = distanceLeft;
totalLeft = totalLeft + readingsLeft[arrayIndexLeft];
arrayIndexLeft = arrayIndexLeft + 1;
totalFront = totalFront – readingsFront[arrayIndexFront];
readingsFront[arrayIndexFront] = distanceFront;
totalFront = totalFront + readingsFront[arrayIndexFront
totalRight = totalRight – readingsRight[arrayIndexRight];
readingsRight[arrayIndexRight] = distanceRight;
totalRight = totalRight + readingsRight[arrayIndexRight];
arrayIndexRight = arrayIndexRight + 1;
// At the end of the array (10 items) then start again
if (arrayIndexLeft >= numOfReadingsLeft) {
arrayIndexLeft = 0;
}
if (arrayIndexFront >= numOfReadingsFront) {
arrayIndexFront = 0;
}
if (arrayIndexRight >= numOfReadingsRight) {
arrayIndexRight = 0;
}
// Calculate the average distance for each heading //
averageDistanceLeft = totalLeft / numOfReadingsLeft;
delay(10);
averageDistanceFront = totalFront / numOfReadingsFront;
delay(10);
averageDistanceRight = totalRight / numOfReadingsRight;
delay(10);
// LOGIC PROCESS – Assess distance and move accordingly //
// Move Forward
if (averageDistanceFront >15) {
digitalWrite(motorLeftPin1, LOW);
digitalWrite(motorLeftPin2, HIGH);
digitalWrite(motorRightPin1, LOW);
digitalWrite(motorRightPin2, HIGH);
}
// STOP
if (averageDistanceFront <=15 && averageDistanceLeft <=15 &&
averageDistanceRight <=15) {
digitalWrite(motorLeftPin1, LOW);
digitalWrite(motorLeftPin2, LOW);
digitalWrite(motorRightPin1, LOW);
digitalWrite(motorRightPin2, LOW);
}
// Turn Right //
if (averageDistanceFront <= 15 && averageDistanceLeft <=15) {
digitalWrite(motorLeftPin1, LOW);
digitalWrite(motorLeftPin2, HIGH);
digitalWrite(motorRightPin1, HIGH);
digitalWrite(motorRightPin2, LOW);
}
// Turn Left //
if averageDistanceFront <= 15 && averageDistanceRight <=15) {
digitalWrite(motorLeftPin1, HIGH);
digitalWrite(motorLeftPin2, LOW);
digitalWrite(motorRightPin1, LOW);
digitalWrite(motorRightPin2, HIGH);
}
LOGIC.pdf (317 KB)
