sandre peters
programma voor stofzuigrobot
Dependencies: Coordinates mbed LCD Servo HCSR04 MMA8451Q
Diff: main.cpp
- Revision:
- 0:9e2069672c96
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Jul 03 09:52:44 2019 +0000
@@ -0,0 +1,640 @@
+#include "mbed.h"
+#include "hcsr04.h"
+#include "servo.h"
+#include "coordinates.h"
+//#include "MMA8451Q.h"
+#include <LiquidCrystal_I2C.h>
+#include <string>
+//#include <vector>
+
+
+InterruptIn startbutton(PC_10);
+InterruptIn pausebutton(PC_12);
+
+Timer readtimer;
+//acceleratie sensor, need SCL and SDA
+//MMA8451Q sensor(D14, D15, 0x1D<<1);//D14: SDA, D15: SCL
+LiquidCrystal_I2C lcd(0x27<<1, 16, 2);
+
+//ultrasoon sensoren
+HCSR04 sonarback(PB_12, PA_12); //PB_12: trig, PA_12: echo
+HCSR04 sonarright(D5, D4); //D5: trig, D4: echo
+HCSR04 sonarfront(PB_7, PA_15); //PB_7: trig, PA_15: echo
+HCSR04 sonarleft(D9, D7); //D6: trig, D7: echo
+//HCSR04 sonarbottomfront(D11, D10); //D11: trig, D10: echo
+//HCSR04 sonarbottomback(D13, D12); //D13: trig, D12: echo
+
+//sharps
+AnalogIn sensorfront(PA_0);
+AnalogIn sensorback(PA_1);
+
+//servos
+Servo servoRV = Servo(-10, 40, 'L', PA_11);
+Servo servoLV = Servo(30, 30, 'R', PB_15);
+Servo servoRA = Servo(-60, -60, 'R', PB_13);
+Servo servoLA = Servo(-20, -10, 'L', PB_14);
+
+
+//motoren
+//pins: LV; PC_10, PC_12 RV;PA_13, PA_14 LA; PC_13, PC_14 RA; PC_2, PC_3
+DigitalOut RVV(PA_13);
+DigitalOut RVA(PA_14);
+DigitalOut LVV(PC_10);
+DigitalOut LVA(PC_12);
+DigitalOut RAV(PC_13);
+DigitalOut RAA(PC_14);
+DigitalOut LAV(PC_2);
+DigitalOut LAA(PC_3);
+
+PwmOut speed();
+
+#define mindist 5 //minimum afstand tot een muur in cm
+#define distToGround 5 //minimum afstand tot de grond
+#define strookdist 10 //breedte van de auto
+
+
+
+
+//prototypes
+void driving(int direction);
+void forward();
+void reverse();
+void driveright();
+void driveleft();
+bool checkfront();
+bool checkleft();
+bool checkright();
+bool checkback();
+bool checkbottom();
+int setdirection();
+void turnright();
+void turnstraight();
+void brake();
+int redirect(int situation);
+int setsituation(string wall);
+void start();
+void pause();
+void trackX();
+
+
+//coordinaten stelsel
+vector<Coordinates> coordinates;
+Coordinates coordinate;
+
+//global variables
+bool isdriving = false;
+int situation;
+bool starting = true;
+int restdistX;
+bool busy;
+
+
+Serial pc(USBTX, USBRX);
+
+int main() {
+ startbutton.rise(&start);
+ pausebutton.rise(&pause);
+
+
+ lcd.begin();
+ lcd.backlight();
+ readtimer.reset(); //reset de timer
+ readtimer.start(); //start de timer
+ lcd.print("started");
+ printf("started\n");
+ turnstraight(); //zet alle wielen recht
+
+ bool wallfront;
+ bool wallleft;
+ bool wallright;
+ bool wallback;
+
+ //bool bottom;
+ //float x,y,z;
+ busy = false;
+ //situation = 1;
+
+ while(1){
+
+ if(starting){ //als we gaan starten
+ start();
+ }
+ //printf("timer: %i\n",readtimer.read_ms());
+ while(busy){
+ if(readtimer.read() > 2){
+ wallfront = checkfront();
+ wallleft = checkleft();
+ wallright = checkright();
+ wallback = checkback();
+ //bottom = checkbottom();
+
+ printf("wallfront: %d\n", wallfront);
+ printf("wallleft: %d\n", wallleft);
+ printf("wallright: %d\n", wallright);
+ printf("wallback: %d\n", wallback);
+ //printf("bottom: %d\n\r", bottom);
+ readtimer.reset();
+
+ //if(!bottom){
+// //bottom = checkbottom();
+// if(!bottom){
+// brake();
+// lcd.print("no bottom");
+// printf("no bottom\n");
+// busy = false;
+// }
+// }
+ //else if(bottom){
+ switch(situation){
+ case 1: //naar voren met muur rechts
+ lcd.print("front with wall ");
+ printf("front with wall\n");
+ if(wallfront || !wallright){ //als voor een muur is of de muur rechts kwijtraakt
+ brake(); //remmen
+ situation = redirect(situation); //stel nieuwe situatie in
+ }
+ break;
+ case 2: //naar achter geen muur
+ lcd.print("back no wall ");
+ printf("back no wall\n");
+ if(wallback){ //als er achter een muur is
+ brake(); //remmen
+ situation = redirect(situation); //stel nieuwe situatie in
+ }
+ break;
+ case 3: //naar voren geen muur
+ lcd.print("front no wall ");
+ printf("front no wall\n");
+ if(wallfront){ //als voor een muur is
+ brake(); //remmen
+ situation = redirect(situation); //stel nieuwe situatie in
+ }
+ break;
+ case 4: //naar achter muur rechts
+ lcd.print("back with wall ");
+ printf("back with wall\n");
+ if(wallback || !wallright){ //als achter een muur en muur rechts weg
+ brake(); //remmen
+ situation = redirect(situation); //stel nieuwe situatie in
+ }
+ break;
+ case 5: //klaar
+ brake(); //remmen
+ lcd.print("done ");
+ printf("done\n");
+ busy = false; //zet programma op standby
+ break;
+ default: //this is an error(should not happen)
+ printf("no situation\n");
+ situation = redirect(situation);
+
+ break;
+
+ //}
+ }
+ }
+ }
+
+
+ //hier komt een update systeem voor de afstand en coordinatenstelsel
+
+ }
+}
+
+void pause(){
+ if(busy){
+ busy = false;
+ }
+ else if(!busy){
+ busy = true;
+ }
+}
+
+void start(){
+ bool wallfront = checkfront();
+ bool wallright = checkright();
+ bool wallleft = checkleft();
+ bool backleft = checkback(); // = checkbackleft(); //linker achter sensor
+ bool backright = checkback(); // = checkbackright(); //rechter achter sensor
+ //int situation;
+ //bool once = false;
+
+ //reverse();
+ //while(!busy){
+// while(!backleft && !backright){
+// backleft = checkback(); // = checkbackleft(); //check sensoren zolang ze niets zien
+// backright = checkback(); // = checkbackright();
+// }
+//
+// if(!once){
+// printf("%d\n", backleft);
+// brake();
+// once = true;
+// }
+//
+// if(backleft && backright){
+// turnright(); //draai de wielen
+// wait(0.5f); //wacht tot ze gedraaid zijn
+// driveright(); //naar rechts rijden
+// while(!wallright){ //zolang er geen muur rechts
+// wallright = checkright(); //check of er muur rechts is
+// }
+// brake(); //remmen
+// situation = 1; //situatie is vooruit met muur rechts
+// printf("lets go\n");
+//
+// }
+// }
+ //bovenste deel kan alleen gebruikt worden als de robot haaks op een muur geplaatst word
+ situation = 1;
+
+ busy = true;
+ starting = false;
+ lcd.print("lets go");
+ printf("lets go\n");
+ //return situation;
+
+}
+
+int redirect(int situation){
+ bool wallfront = checkfront();
+ bool wallleft = checkleft();
+ bool wallright = checkright();
+ bool wallback = checkback();
+ bool bottom = checkbottom();
+ int newsituation;
+ int dist;
+
+
+ if(situation == 1){ //je reed naar voren met een muur rechts
+ if(!wallfront && wallright){ //geen muur voor, wel muur rechts
+ newsituation = 1; //error, toch niks aan de hand
+ }
+ else if(!wallfront && !wallright){ //geen muur voor en geen muur rechts(we zijn rechter muur kwijt)
+ forward(); //laatste deel van de auto moet ook nog voorbij de muur
+ wait(1); //wacht tot achterste deel ook voorbij muur is(kan je ook doen met afstand lezen van encoder
+ brake(); //remmen
+ turnright(); //draai de wielen
+ wait(0.5f); //wacht tot ze gedraaid zijn
+ driveright(); //naar rechts rijden
+ while(!wallright){ //wacht tot er muur rechts is
+ wallright = checkright(); //check of er muur rechts is
+ }
+ brake(); //remmen
+ turnstraight(); //draai wielen recht
+ wait(0.5f); //wacht tot de wielen gedraaid zijn
+ newsituation = setsituation("right"); //maak nieuwe situatie aan, geef mee dat we net naar rechts zijn gegaan
+
+ }
+ else if(wallfront && !wallleft){ //er is een muur voor en geen muur links(we kunnen dus naar links toe
+ turnright(); //draai wielen
+ wait(0.5f); //wacht to ze gedraaid zijn
+ driveleft(); //naar nieuwe strook links rijden
+ while(dist < strookdist){ //rij tot de nieuwe strook bereikt is
+ wait(3); //puur voor demo wachten
+ dist = strookdist+1; //vraag afgelegde afstand op bij motor(dus niet gelijkstellen)
+ }
+ wait(2);
+ brake();
+ turnstraight(); //draai wielen recht
+ wait(0.5f); //wacht tot wielen recht staan
+ newsituation = setsituation("wallfront"); //maak nieuwe situatie aan, geef mee dat er een muur voor was
+ }
+ else if(wallfront && wallleft){ //er is een muur voor, en links, rechts was dus ook al muur en achter je kom je vandaan
+ newsituation = 5; //klaar
+ }
+ }
+ else if(situation == 2){ //je reed naar achter zonder muur
+ if(!wallback && !wallright){//geen muur achter en geen muur rechts
+ newsituation = 2; //error was toch niks veranderd, behoud huidige situatie
+ }
+ else if(wallback && !wallleft){ //muur achter geen muur links dus we willen naar links
+ turnright(); //draai wielen
+ wait(0.5f); //wacht tot ze gedraaid zijn
+ driveleft(); //naar links rijden
+ while(dist < strookdist){ //rij tot de nieuwe strook bereikt is
+ wait(3); //wacht puur voor de demo
+ dist = strookdist+1; //vraag afgelegde afstand op bij motor(dus niet gelijkstellen)
+ }
+ wait(2);
+ brake(); //remmen
+ turnstraight(); //draai wielen recht
+ wait(0.5f); //wacht tot wielen recht staan
+ newsituation = setsituation("wallback"); //maak nieuwe situatie aan. geef mee dat er een muur achter je was
+ }
+ else if(wallback && wallleft){ //muur achter en links, voor and rechts kom je vandaan, je kan nergens meer heen
+ newsituation = 5; //klaar
+ }
+ }
+ else if(situation == 3){ //je reed naar voren zonder muur
+ if(!wallfront && !wallright){ //geen muur voor en geen muur rechts
+ newsituation = 3; //error, behoud huidige situatie
+ }
+ else if(wallfront && !wallleft){//muur voor geen muur links, we willen naar links
+ turnright(); //draai wielen
+ wait(0.5f); //wacht tot wielen gedraaid zijn
+ driveleft(); //naar links rijden
+ while(dist < strookdist){ //rij tot de nieuwe strook bereikt is
+ wait(3); //wacht puur voor de demo
+ dist = strookdist+1; //vraag afgelegde afstand op bij motor(dus niet gelijkstellen)
+ }
+ wait(2);
+ brake();
+ turnstraight(); //draai wielen recht
+ wait(0.5f); //wacht tot ze rcht gedraaid zijn
+ newsituation = setsituation("wallfront"); //maak nieuwe situatie aan, geef mee dat er een muur voor was
+ }
+ else if(wallfront && wallleft){ //muur voor en links, rechts en achter kom je vandaan
+ newsituation = 5; //klaar
+ }
+ }
+ else if(situation == 4){ //je reed naar achter met muur rechts
+ if(!wallback && wallright){ //geen muur achter en muur rechts
+ newsituation = 4; //error was toch niks veranderd, behoud huidige situatie
+ }
+ else if(wallback && !wallleft){ //muur achter geen muur links
+ reverse(); //laatste deel van de auto moet ook nog voorbij de muur
+ wait(1); //wacht aantal cm, is beter om door wielen uit te laten lezen
+ brake(); //remmen
+ turnright(); //draai wielen naar rechts
+ wait(0.5f); //wachtt ot de wielen gedraaid zijn
+ driveleft(); //naar links rijden
+ while(dist < strookdist){ //rij tot de nieuwe strook bereikt is
+ wait(3); //wacht puur voor de demo
+ dist = strookdist+1; //vraag afgelegde afstand op bij motor(dus niet gelijkstellen)
+ }
+ wait(2);
+ brake(); //remmen
+ turnstraight(); //draai wielen recht
+ wait(0.5f); //wacht tot de wielen gedraaid zijn
+ newsituation = setsituation("wallback"); //maak nieuwe situatie aan, geef mee dat er muur achter was
+ }
+
+ else if(!wallback && !wallright){ //geen muur achter, geen muur rechts, we willen naar rechts
+ turnright(); //draai wielen naar rechts
+ wait(0.5f); //wacht tot wielen gedraaid zijn
+ driveright(); //naar rechts rijden
+ //bool rightline = false; //zeg dat we nog niet op juiste lijn zitten
+// while(!rightline){ //zolang we nog niet op juiste lijn zitten
+// for(int i =0; i < coordinates.size(); i++){ //loop door alle coordinaten heen
+// int y = coordinates.at(i).getY(); //sla de y positie op
+// int x = coordinates.at(i).getX(); //sla de x positie op
+// if((coordinates.at(coordinates.size()-1).getY() == y) && (coordinates.at(coordinates.size()-1).getX()-1 == x)){ //als huidige y coordinaat klopt met een eerder bekende coordinaat, en de huidgie x positie is naar de al gereden strook
+// printf("X: %i Y: %i\n\r", x, y); //output voor testen
+// rightline = true; //we zitten nu op de juiste lijn
+// //update afgelegde afstand
+// }
+// }
+// }
+ wait(2); //bovenste deel kan nog niet gebuikt worden, dus wait
+ brake(); //remmen
+ turnstraight(); //draai wielen recht
+ wait(0.5f); //wacht tot wielen gedraaid zijn
+ newsituation == setsituation("right"); //maak nieuwe situatie aan, vertel dat we net naar rechts zijn gegaan
+ }
+ else if(wallback && wallleft){ //achter muur en links, rechts is een muur en voor heb je al gehad
+ newsituation = 5; //we zijn klaar
+ }
+ }
+ else{
+ printf("choosing from nothing\n");
+ newsituation = setsituation("empty");
+ }
+
+ return newsituation; //geef te nieuwe situatie door
+}
+
+
+int setsituation(string wall){
+ bool wallfront = checkfront();
+ bool wallright = checkright();
+ bool wallback = checkback();
+
+ //bool bottom = checkbottom(); //moet eigenlijk tussendoor apart gechecked worden
+
+ if(wall == "wallback"){ //je zag een muur achter en ging naar links(potentie op rechts langs muur rijden)
+ if(!wallback){ //er is geen muur achter, je gaat dus door in dezelfde richting
+ reverse(); //ga achteruit rijden
+ wait(3); //wacht tot die de rechts kan zien
+ situation = 4; //nieuwe situatie is naar achter met muur rechts
+ }
+ else if(wallback){ //er is achter een muur
+ forward(); //vooruit rijden
+ situation = 3; //nieuwe situatie is naar voren geen muur
+ }
+ }
+ else if(wall == "wallfront"){ //je zag een muur voor en ging naar links(potentie op rechts langs muur rijden)
+ if(!wallfront){ //er is geen muur voor, we gaan dus verder in dezelfde richting
+ forward(); //ga vooruit rijden
+ wait(3); //wacht tot die de muur rechts kan zien
+ situation = 1; //nieuwe situatie naar voren met muur rechts
+ }
+ else if(wallfront){ //er is voor wel een muur
+ reverse(); //achteruit rijden
+ situation = 2; //nieuwe situatie is naar achter geen muur
+ }
+ }
+ else if(wall == "right"){ //je bent net naar rechts gereden
+
+ if(!wallfront && wallright){ //er is voor geen muur en muur rechts
+ forward(); //vooruit rijden
+ wait(3); //stukje naar voren rijden voordat die de muur rechts kan zien
+ situation = 1; //nieuwe situatie is naar voren met muur rechts
+ }
+ else if(wallfront && !wallright){ //er is voor een muur en geen muur rechts
+ reverse(); //achteruit rijden
+ situation = 2; //nieuwe situatie naar achter zonder muur
+ }
+ else if(wallback && !wallright){ //er is achter een muur en rechts geen muur
+ forward(); //vooruit rijden
+ situation = 3; //nieuwe situatie is naar voren geen muur
+ }
+ else if(!wallback && wallright){ //er is geen muur achter en wel een muur rechts
+ reverse(); //achteruit rijden
+ wait(3); //stukje naar achter rijden voorodat de muur rechts te zien is
+ situation = 4; //nieuwe situatie is naar achter met muur rechts
+ }
+ else if(!wallback && !wallright){ //er is geen muur achter en geen muur rechts
+ reverse(); //achteruit rijden
+ situation = 2; //nieuwe situatie is naar achter geen muur
+ }
+ else if(wallfront && wallback){ //er is een muur voor en een muur achter, je kan geen kant meer op
+ situation = 5; //klaar(of error)
+ }
+ }
+ else{ //er was nog geen situatie, je hebt nog niks gedaan
+ if(!wallfront && wallright){ //er is geen muur voor en een muur rechts
+ situation = 1; //situatie is naar voren met muur rechts
+ forward(); //naar voren rijden
+ }
+ else if(wallfront && !wallback && !wallright){ //er is voor een muur, achter en rechts niet
+ situation = 2; //situatie is naar achter rijden zonder muur
+ reverse(); //naar achter rijden
+ }
+ else if(wallfront && !wallback && wallright){ //er is een muur voor en geen muur achter en een muur rechts
+ situation = 4; //situatie is achter rijden met muur rechts
+ reverse(); //naar achter rijden
+ }
+ else if(!wallfront && !wallright){ //er is geen muur voor en geen muur rechts
+ situation = 3; //situatie is naar voor rijden geen muur rechts
+ forward(); //vooruit rijden
+ }
+ }
+
+ return situation; //geef de nieuwe situatie door
+}
+
+
+bool checkfront(){ //check de afstand aan de voorkant
+ bool wall;
+ long frontdist = sonarfront.distance(); //sla de afstand op
+ if(frontdist <= mindist){ //als de afstand kleiner of gelijk als minimum afstand
+ wall = true; //er is een muur
+ }
+ else if(frontdist > mindist){ //als de afstand groter is dan minimum
+ wall = false; //er is geen muur
+ }
+ return wall; //return of er een muur is
+}
+
+bool checkleft(){ //check muur links(kijk of we naar links kunnen rijden)
+ bool wall;
+ long leftdist = sonarleft.distance(); //sla de afstand op tot de muur links
+ if(leftdist > mindist+strookdist){ //als de afstand groter is als miniumum afstand + breedte van de robot
+ wall = false; //er is geen muur
+ }
+ else if(leftdist <= mindist+strookdist){ //als de afstand kleiner of gelijk is als de minimum afstand + breedte van de robot
+ wall = true; //er is een muur
+ } //we kunnen dus niet naar links rijden
+
+ return wall; //geef terug of er een muur is
+}
+
+
+bool checkright(){ //check rechter sonar
+ bool wall;
+ long rightdist = sonarright.distance(); //sla de afstand tot rechter muur op
+ if(rightdist > mindist+1){ //als de afstand groter is als minimum afstand + 1
+ wall = false; //er is geen muur
+ }
+ else if(rightdist <= mindist && rightdist >= mindist-1){ //als de afstand tussen de minimale afstand en de minimale afstand-1 ligt
+ wall = true; // er is een muur
+ } //je kan dit gebruiken om te kijken of je recht langs een muur rijd
+ else if(rightdist < mindist-1){ //als afstand kleiner is als minimum
+ wall = true; //er is een muur
+ }
+ return wall;
+}
+
+bool checkback(){ //check achterste sonar
+ bool wall;
+ long backdist = sonarback.distance(); //sla de afstand op tot achterste muur
+ if(backdist <= mindist){ //als afstand kleiner of gelijk is als de minimum afstand
+ wall = true; //er is een muur
+ }
+ else if(backdist > mindist){ //als afstand meer is als de minimum afstand
+ wall = false; //er is geen muur
+ }
+ return wall; //geen terug of er een muur achter is
+}
+
+bool checkbottom(){ //check de onderste sonars
+ bool ground;
+ double volts;
+ int distancefront;
+ int distanceback;
+
+ volts = double(sensorfront.read())*3.4; // value from sensor * (5/1024)
+ distancefront = 13*pow(volts, -1); // worked out from datasheet graph
+
+ volts = double(sensorback.read())*3.4; // value from sensor * (5/1024)
+ distanceback = 13*pow(volts, -1); // worked out from datasheet graph
+
+
+ if(distancefront > distToGround || distanceback > distToGround){ //als afstand groter dan toegestane
+ ground = false;
+ }
+ else if(distancefront <= distToGround && distanceback <= distToGround){ //als afstand kleiner dan toegestane
+ ground = true;
+ }
+
+ return ground;
+}
+
+
+void turnright(){ //draai de servos naar rechts
+ servoRV.turnRight();
+ servoLV.turnRight();
+ servoRA.turnRight();
+ servoLA.turnRight();
+
+}
+
+void turnstraight(){ //zet de servos in rechte positie
+ servoRV.servoCenter();
+ servoLV.servoCenter();
+ servoRA.servoCenter();
+ servoLA.servoCenter();
+}
+
+void brake(){ //remmen
+ //stop motor
+ RVV = 0;
+ LVV = 0;
+ RAV = 0;
+ LAV = 0;
+ RVA = 0;
+ LVA = 0;
+ RAA = 0;
+ LAA = 0;
+
+ isdriving = false;
+
+ printf("brake\n");
+}
+
+void forward(){ //vooruit rijden
+ RVV = 1;
+ RAV = 1;
+ LVV = 1;
+ LAV = 1;
+ isdriving = true;
+ printf("forward\n");
+}
+
+void reverse(){ //achteruit rijden
+ RVA = 1;
+ RAA = 1;
+ LAA = 1;
+ LVA = 1;
+ isdriving = true;
+ printf("reverse\n");
+}
+
+void driveright(){
+ RVA = 1;
+ LAA = 1;
+ LVV = 1;
+ RAV = 1;
+
+}
+
+void driveleft(){
+ RVV = 1;
+ LAV = 1;
+ LVA = 1;
+ RAA = 1;
+
+}
+
+void trackX(){ //convert de x positie naar stappen
+ int realdist = 0; //vraag afgelegde afstand op bij wielen
+ int dist = realdist + restdistX;
+ if(dist > stepsize){
+ restdistX = dist - stepsize;
+ coordinate.increaseX();
+ coordinates.push_back(coordinate);
+
+ }
+ realdist = 0;
+}
\ No newline at end of file