File content as of revision 10:e8110fb94686:

#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()
{
    //printf("--------\n\rREADY\n\r--------\n\r");
    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[51];
    int directions[51];
    int modeStart = 0;
    int startx = 0;
    int starty = 9;
    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 == 20) {
                    //    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;


                for(int p=0; p<20; p++) {
                    //printf("Anw: %d\r\n", directions[p]);
                }

                for(int p=0; p<9; p++) {
                    for(int q=0; q<4; q++) {
                        //printf(" %d ", lab[q][p]);
                    }
                    //printf("\r\n");
                }




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