File content as of revision 0:9e2069672c96:

#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;
}