Download (PDF, 685KB)
//motor a, placed on the right
#define brakeR 9
#define powerR 3
#define direcR 12
int powValR;
//motor b, placed on the left
#define brakeL 8
#define powerL 11
#define direcL 13
int powValL;
//motor speeds
const int quatS = 63;
const int halfS = 123;
const int thqtS = 191;
const int fullS = 255;
//switch for the motor
#define start 2
int switchVal;
bool motorOn = false;
//general control
int lastMsg = -1;
bool firstPart = true; //following the line
bool secondPart = false; //approaching the wall
bool thirdPart = false; //driving along the wall to the goal
//sensors
#define lightR A0 //right light sensor
#define lightL A5 //left light sensor
#define frUlSI A2 //front ultrasound sensor input (echo)
#define frUlSO 5 //front ultrasound sensor output (trigger)
#define sdUlSI A3 //right side ultrasound sensor input (echo)
#define sdUlSO 6 //right side ultrasound sensor output (trigger)
const int threshold = 45;
int litValR, litValL, litDif;
long dur, frDist, sdDist;
void setup() {
pinMode(direcR, OUTPUT);
pinMode(direcL, OUTPUT);
pinMode(brakeR, OUTPUT);
pinMode(brakeL, OUTPUT);
pinMode(powerR, OUTPUT);
pinMode(powerL, OUTPUT);
pinMode(start, INPUT);
pinMode(lightR, INPUT);
pinMode(lightL, INPUT);
pinMode(frUlSO, OUTPUT);
pinMode(frUlSI, INPUT);
pinMode(sdUlSO, OUTPUT);
pinMode(sdUlSI, INPUT);
Serial.begin(9600);
}
void loop() {
while(firstPart) {
checkSwitch();
if(motorOn) {
digitalWrite(brakeR, LOW);
digitalWrite(brakeL, LOW);
litValR = analogRead(lightR);
litValL = analogRead(lightL);
litDif = litValR - litValL;
if(litDif >= -(threshold) && litDif <= threshold) {
//all wheels running forward
digitalWrite(direcR, LOW);
digitalWrite(direcL, HIGH);
if(lastMsg != 0) {
lastMsg = 0;
}
powValR = fullS;
powValL = fullS;
}
else if (litDif < -(threshold)) {
if(lastMsg != 1) {
brake();
digitalWrite(brakeR, LOW);
digitalWrite(brakeL, LOW);
lastMsg = 1;
}
//right wheels runs backwards
digitalWrite(direcR, LOW);
digitalWrite(direcL, LOW);
//turn left
powValR = fullS;
powValL = halfS;
}
else if (litDif > threshold) {
if(lastMsg != 2) {
brake();
digitalWrite(brakeR, LOW);
digitalWrite(brakeL, LOW);
lastMsg = 2;
}
//left wheels runs backwards
digitalWrite(direcR, HIGH);
digitalWrite(direcL, HIGH);
//turn right
powValR = halfS;
powValL = fullS;
}
else {
Serial.println("ERROR: Something in the math went wrong!");
}
analogWrite(powerR, powValR);
analogWrite(powerL, powValL);
}
else {
digitalWrite(brakeR, HIGH);
digitalWrite(brakeL, HIGH);
analogWrite(powerR, 0);
analogWrite(powerL, 0);
}
if(litValR > 850 && litValL > 850) {
firstPart = false;
secondPart = true;
brake();
}
}
delay(200);
while(secondPart) {
checkSwitch();
frDist = measureDist(frUlSI, frUlSO);
if(motorOn) {
//Med/uden ledning: 15/10
while(frDist > 12 && motorOn) {
checkSwitch();
frDist = measureDist(frUlSI, frUlSO);
digitalWrite(brakeR, LOW);
digitalWrite(brakeL, LOW);
digitalWrite(direcR, LOW);
digitalWrite(direcL, HIGH);
powValR = fullS;
powValL = fullS;
analogWrite(powerR, powValR);
analogWrite(powerL, powValL);
}
brake();
delay(200);
while(frDist < 100 && motorOn) {
checkSwitch();
frDist = measureDist(frUlSI, frUlSO);
digitalWrite(brakeR, LOW);
digitalWrite(brakeL, LOW);
digitalWrite(direcR, LOW);
digitalWrite(direcL, LOW);
powValR = fullS;
powValL = halfS;
analogWrite(powerR, powValR);
analogWrite(powerL, powValL);
}
//Med/uden ledning: 500/300
delay(600);
brake();
secondPart = false;
thirdPart = true;
}
}
delay(200);
while(thirdPart) {
checkSwitch();
sdDist = measureDist(sdUlSI, sdUlSO);
if(motorOn) {
digitalWrite(brakeR, LOW);
digitalWrite(brakeL, LOW);
//Med/uden ledning: 10/8
if(sdDist > 10) {
//drej ind mod væg
digitalWrite(direcR, HIGH);
digitalWrite(direcL, HIGH);
powValR = thqtS;
powValL = fullS;
}
//Med/uden ledning: 8/6
else if (sdDist < 8) {
//drej væk fra væg
digitalWrite(direcR, LOW);
digitalWrite(direcL, LOW);
powValR = thqtS;
powValL = halfS;
}
else {
//kør lige ud
digitalWrite(direcR, LOW);
digitalWrite(direcL, HIGH);
powValR = thqtS;
powValL = thqtS;
}
analogWrite(powerR, powValR);
analogWrite(powerL, powValL);
}
}
checkSwitch();
}
void checkSwitch() {
switchVal = digitalRead(start);
if(switchVal > 0) {
if(motorOn) {
motorOn = false;
}
else {
motorOn = true;
}
firstPart = true;
secondPart = false;
thirdPart = false;
delay(200);
while(digitalRead(start) > 0);
}
}
void brake() {
analogWrite(powerR, 0);
analogWrite(powerL, 0);
digitalWrite(brakeR, HIGH);
digitalWrite(brakeL, HIGH);
}
int measureDist(int echoPin, int trigPin) {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
dur = pulseIn(echoPin, HIGH);
return (dur / 2) / 29.1; //converts to cm
}