File content as of revision 9:ab19796bf14a:

#include "mbed.h"

#include "IRSensor.h"
#include "EncoderCounter.h"
#include "LowpassFilter.h"
#include "Controller.h"
#include "KontrastSensor.h"
#include "Drive.h"
#include "CheckWalls.h"
#include "Turn.h"

Serial pc(USBTX,USBRX,460800);

//Initialisierung LED/Button
DigitalIn button(USER_BUTTON);  //Moduswählknopf
DigitalOut myled(LED1);     //Heartbeat (evt auch Anzeige für Modus, vor start


//Initialisierung IR-Sensoren
AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
AnalogIn distance1(PC_3);
AnalogIn distance2(PC_5);
AnalogIn distance3(PB_1);
IRSensor irSensor0(distance0);    //rechts
IRSensor irSensor1(distance1);    //vorne
IRSensor irSensor2(distance2);    //links-vorne
IRSensor irSensor3(distance3);    //links-hinten

//Initialisierung Kontrastsensor
AnalogIn kontrast(PC_0);

//Initialisierung Motor und Encoder
DigitalOut enableMotorDriver(PB_2);
PwmOut pwmLeft(PA_8);   //Motor links
PwmOut pwmRight(PA_9);  //Motor rechts
EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links
EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts

DigitalOut enable(PC_1);

Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);

int state = 0;
int mode = 1;


int main()
{
    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
    enable = 1;

    int wallRight = 0;
    int wallFront = 0;
    int wallLeft = 0;
    int blackLine = 0;
    int dontStop = 0;
    int lab[5][10];
    int path = 0;
    int pathNext = 0;
    int pathArr[50];
    int directions[50];
    int modeStart = 0;
    int startx = 0;
    int starty = 10;
    int ausr = 1;
    int c = 0;
    int firstMove = 0;
    
    //Funktionsdeklarationen
    
    CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
    Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft, dontStop, modeStart, path);   //Nach Ausrichtung bewegt sich der Roboter um ein Feld weiter und die Ausrichutung beginnt von vorne
    KontrastSensor kontrastSensor(kontrast, blackLine);
    Drive drive(kontrastSensor, counterLeft, counterRight, controller, irSensor0, irSensor1, irSensor2, irSensor3, dontStop, modeStart, path, pathNext); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...    


    while(1) { // Wiederholungsschleife
    
        switch(state) { //Wartemodus 1. Lauf
            case 0: {
                while (button) {
                    wait(1.0f);         //Die Ruhe vor dem Sturm...
                }
                firstMove = 1;
                state = 1;
                
                break;
            }
            case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
            
                checkWalls.check(); //prueft wo Waende vorhanden sind
                

                
                turn.turning();
                
                if(firstMove == 1){
                    dontStop = 1;
                    firstMove = 0;
                    
                    /* **************************** */
                    /* Anpassung 16.02.18 */
                    if (wallFront == 0) {
                        ausr = 1;
                        lab[startx][starty] = ausr;
                        starty--;
                    }else if (wallRight == 0) {
                        ausr = 3;       // Ausrichtung zu kontrollieren!!!!
                        lab[startx][starty] = ausr;
                        startx++;
                    }
                    /***********************/ 
                }
                
                
                //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth veraendert und speichert diese in ein Array
                
                if(wallLeft == 0) {
                    switch(ausr) {
                        case 1: {
                            ausr = 2;
                            lab[startx][starty] = ausr;
                            startx--;
                            break;
                        }
                        case 2: {
                            ausr = 4;
                            lab[startx][starty] = ausr;
                            starty++;
                            break;
                        }
                        case 3: {
                            ausr = 1;
                            lab[startx][starty] = ausr;
                            starty--;
                            break;
                        }
                        case 4: {
                            ausr = 3;
                            lab[startx][starty] = ausr;
                            startx++;
                            break;
                        }
                    }

                } else if(wallFront == 0) {
                    switch(ausr) {
                        case 1: {
                            ausr = 1;
                            lab[startx][starty] = ausr;
                            starty--;
                            break;
                        }
                        case 2: {
                            ausr = 2;
                            lab[startx][starty] = ausr;
                            startx--;
                            break;
                        }
                        case 3: {
                            ausr = 3;
                            lab[startx][starty] = ausr;
                            startx++;
                            break;
                        }
                        case 4: {
                            ausr = 4;
                            lab[startx][starty] = ausr;
                            starty++;
                            break;
                        }
                    }
                } else if(wallRight == 0) {
                    switch(ausr) {
                        case 1: {
                            ausr = 3;
                            lab[startx][starty] = ausr;
                            startx++;
                            break;
                        }
                        case 2: {
                            ausr = 1;
                            lab[startx][starty] = ausr;
                            starty--;
                            break;
                        }
                        case 3: {
                            ausr = 4;
                            lab[startx][starty] = ausr;
                            starty++;
                            break;
                        }
                        case 4: {
                            ausr = 2;
                            lab[startx][starty] = ausr;
                            startx--;
                            break;
                        }
                    }
                } else {
                    switch(ausr) {
                        case 1: {
                            ausr = 4;
                            lab[startx][starty] = ausr;
                            starty++;
                            break;
                        }
                        case 2: {
                            ausr = 3;
                            lab[startx][starty] = ausr;
                            startx++;
                            break;
                        }
                        case 3: {
                            ausr = 2;
                            lab[startx][starty] = ausr;
                            startx--;
                            break;
                        }
                        case 4: {
                            ausr = 1;
                            lab[startx][starty] = ausr;
                            starty--;
                            break;
                        }
                    }
                }
                
                drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
                
                if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
                    lab[startx][starty] = 5;
                    state = 2;
                }
                break;
            }
            
            case 2: { //Ziel erreicht => Wartemodus fuer 2. Lauf und Berechnung der schnellsten Weges

                controller.setDesiredSpeedRight(0.0f);
                controller.setDesiredSpeedLeft(0.0f);

                startx = 0;
                starty = 9;
                
                int num;
                c = 0;
                
                //Speichert die 2-Dimensionale Karte in ein 1D-Array welche nun aus anweisungen besteht
                
                while(lab[startx][starty] != 5) {
                    num = lab[startx][starty];
                    pathArr[c] = num;

                    switch(num) {
                        case 1: {
                            starty--;
                            break;
                        }
                        case 2: {
                            startx--;
                            break;
                        }
                        case 3: {
                            startx++;
                            break;
                        }
                        case 4: {
                            starty++;
                            break;
                        }
                        case 5: {

                        }
                    }//Switch End
                    
                    myled =! myled; // LED is ON Heartbeat
                    wait(0.4f);
                    c++;
                }//While Ende
                
                pathArr[c]= 5; //setzt end bit
                
                
                c=0;

                

                //übersetzt den 1-Dimensionalen Array der Ausrichtung in einen Array nun Anweisungen zur Lösung darstellt
                
                while(pathArr[c]!= 5) {
                    if(c==0) {
                        if(pathArr[c]==1) {
                            directions[c]=1;
                        } else {
                            directions[c]=3;
                        }
                    } else if(pathArr[c-1]==pathArr[c]) {
                        directions[c]=1;
                    } else {
                        
                        if(pathArr[c]==1) {
                            if(pathArr[c-1]==3) {
                                directions[c]=2;
                            }
                            if(pathArr[c-1]==2) {
                                directions[c]=3;
                            }
                            if(pathArr[c-1]==4) {
                                directions[c-1]=4;
                            }
                        } else if(pathArr[c]==2) {
                            if(pathArr[c-1]==1) {
                                directions[c]=2;
                            }
                            if(pathArr[c-1]==3) {
                                directions[c]=4;
                            }
                            if(pathArr[c-1]==4) {
                                directions[c]=3;
                            }
                        } else if(pathArr[c]==3) {
                            if(pathArr[c-1]==1) {
                                directions[c]=3;
                            }
                            if(pathArr[c-1]==2) {
                                directions[c]=4;
                            }
                            if(pathArr[c-1]==4) {
                                directions[c]=2;
                            }
                        } else if(pathArr[c]==4) {
                            if(pathArr[c-1]==1) {
                                directions[c]=4;
                            }
                            if(pathArr[c-1]==2) {
                                directions[c]=2;
                            }
                            if(pathArr[c-1]==3) {
                                directions[c]=3;
                            }
                        }
                    }
                    c++;
                    myled =! myled; // LED is ON Heartbeat
                    wait(0.1f);
                }
                c = 0;

                while (button) {
                    wait(1.0f);
                }
                blackLine = 0;
                firstMove = 1;
                state = 3;
                myled = 0; // LED is ON Heartbeat
                c = 0;
                break;
            }
            case 3: {//2. Lauf Fahrzyklus
            
                //uebergibt Anweisung an turn
                
                modeStart=1;
                path = directions[c];
                pathNext = directions[c+1]; 
                
                   
                turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
                
                
                /**************************/
                if(firstMove == 1){
                    dontStop = 1;
                    firstMove = 0;
                }
                /***********************/
                
            
                
                drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
                c++;
                
                if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
                    state = 4;
                }
                break;
            }
            case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf
                controller.setDesiredSpeedRight(0.0f);
                controller.setDesiredSpeedLeft(0.0f);

                while (button) {
                    wait(1.0f);
                }
                state = 5;
                break;
            }
            default: {
                state = 0;
                break;
            }
        }
        myled =! myled; // LED is ON Heartbeat
        wait(0.1f);
    }
}