File content as of revision 8:1c8a747c49c8:

#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 LEDs Blinker/Surri/Button
DigitalIn button(USER_BUTTON);  //Moduswählknopf
DigitalOut myled(LED1);     //Heartbeat (evt auch Anzeige für Modus, vor start
DigitalOut blinker0(PA_4);  //Blinker links
DigitalOut blinker1(PC_1);  //Blinker rechts
DigitalOut surri(PC_0);     //

//Initialisierung LEDs Nightrider
PwmOut led0(PB_0);  //von links nach rechts
PwmOut led1(PB_8);
PwmOut led2(PB_3);
PwmOut led3(PB_5);
PwmOut led4(PB_4);
PwmOut led5(PB_10);
PwmOut led6(PB_9);

//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("Start main\r\n");
    
    
    
    
    int lab[5][10];
    int path = 0;
    int pathArr[50];
    int directions[50];
    int modeStart=0;
    int startx = 0;
    int starty = 10;
    int ausr = 1;
    int c = 0;
    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
    enable = 1;

    int wallRight = 0;
    int wallFront = 0;
    int wallLeft = 0;
    int blackLine = 0;
    int dontStop = 0;
    //int way = 220;

//    int way[5][10][4] = {{0},{0},{0}}; // Abspeicherung wird in roboterkoordinaten gespeichert vierstellige Zahl => 1.Zahl gefahrene Richtung, 2.Zahl Wand rechts, 3.Zahl Wand vorne, 4.Zahl Wand links

    //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); //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) {
                    //printf("WARTE AUF 1.START...");
                    //controller.setDesiredSpeedRight(0.5f);
                    //controller.setDesiredSpeedLeft(-0.5f);
                    wait(1.0f);         //Die Ruhe vor dem Sturm...
                }

                //if(modeStart == 1) {
                //    state = 1;
                //}

                state = 1;
                break;
            }
            case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus


                checkWalls.check(); //prueft wo Waende vorhanden sind

                turn.turning();

                //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth verändert und speichert diese in ein Array
                if(wallLeft == 0) {
                    switch(ausr) {
                        case 1: {
                            ausr = 2;
                            lab[startx][starty] = ausr;
                            startx--;
                            break;
                        }
                        case 2: {
                            //lab[startx][starty] =2;
                            ausr = 4;
                            lab[startx][starty] = ausr;
                            starty++;
                            break;
                        }
                        case 3: {
                            //lab[startx][starty]=2;
                            ausr = 1;
                            lab[startx][starty] = ausr;
                            starty--;
                            break;
                        }
                        case 4: {
                            //lab[startx][starty]=2;
                            ausr = 3;
                            lab[startx][starty] = ausr;
                            startx++;
                            break;
                        }
                    }

                } else if(wallFront == 0) {
                    switch(ausr) {
                        case 1: {
                            //lab[startx][starty]=1;
                            ausr = 1;
                            lab[startx][starty] = ausr;
                            starty--;
                            break;
                        }
                        case 2: {
                            //lab[startx][starty]=1;
                            ausr = 2;
                            lab[startx][starty] = ausr;
                            startx--;
                            break;
                        }
                        case 3: {
                            //lab[startx][starty]=1;
                            ausr = 3;
                            lab[startx][starty] = ausr;
                            startx++;
                            break;
                        }
                        case 4: {
                            //lab[startx][starty]=1;
                            ausr = 4;
                            lab[startx][starty] = ausr;
                            starty++;
                            break;
                        }
                    }
                } else if(wallRight == 0) {
                    switch(ausr) {
                        case 1: {
                            //lab[startx][starty]=3;
                            ausr = 3;
                            lab[startx][starty] = ausr;
                            startx++;
                            break;
                        }
                        case 2: {
                            //lab[startx][starty]=3;
                            ausr = 1;
                            lab[startx][starty] = ausr;
                            starty--;
                            break;
                        }
                        case 3: {
                            //lab[startx][starty]=3;
                            ausr = 4;
                            lab[startx][starty] = ausr;
                            starty++;
                            break;
                        }
                        case 4: {
                            //lab[startx][starty]=3;
                            ausr = 2;
                            lab[startx][starty] = ausr;
                            startx--;
                            break;
                        }
                    }
                } else {
                    switch(ausr) {
                        case 1: {
                            //lab[startx][starty]=4;
                            ausr = 4;
                            lab[startx][starty] = ausr;
                            starty++;
                            break;
                        }
                        case 2: {
                            //lab[startx][starty]=4;
                            ausr = 3;
                            lab[startx][starty] = ausr;
                            startx++;
                            break;
                        }
                        case 3: {
                            //lab[startx][starty]=4;
                            ausr = 2;
                            lab[startx][starty] = ausr;
                            startx--;
                            break;
                        }
                        case 4: {
                            //lab[startx][starty]=4;
                            ausr = 1;
                            lab[startx][starty] = ausr;
                            starty--;
                            break;
                        }
                    }
                }


                //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...

                //dontStop = way%10;
                //way = way/10;


                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 und Wartemodus fuer 2. Lauf

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

                startx = 0;
                starty = 9;

                    
                //wait(30.4f);
                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);
                    //printf("Array = %d , num = %d\n\r", pathArr[c],num);
                    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;
                        }
                        //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]);
                    } else if(pathArr[c-1]==pathArr[c]) {
                        directions[c]=1;
                        //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]);
                    } 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;
                            }
                        }
                        //printf("pathArr: %d, directions: %d \r\n", pathArr[c], directions[c]);
                    }
                    c++;
                    myled =! myled; // LED is ON Heartbeat
                    wait(0.1f);
                    //printf("Array = %d \n\r", directions[c]);

                }
                c=0;

                while (button) {
                    //printf("WARTE AUF 2.START...");
                    wait(1.0f);
                }
                blackLine = 0;
                state = 3;
                myled =0; // LED is ON Heartbeat

               
                c=0;
                break;
            }
            case 3: {//2. Lauf Fahrzyklus
                //übergibt Anweisung an turn
                modeStart=1;
                
                path=directions[c];
                
                printf("path %d, modeStart %d \r\n", path, modeStart);
                
                //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden
                //checkWalls.check(); //prueft wo Waende vorhanden sind
                //
                //integer fuer kuerzesten weg...3 rechts 1 gerade aus 2 links und 4 180 Grad drehung
                // wallRight = 0;
                // wallFront = 0;
                // wallLeft = 0;

                turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...

                //dontStop = 2;

                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) {
                    //printf("SCHLUSS...");
                    wait(1.0f);
                }

                state = 5;
                break;
            }
            default: {
                state = 0;
                break;
            }
        }




        //printf("DistanzRechts = %.0f mm\n\r", irSensor0.read());
        //printf("DistanzVorne = %.0f mm\n\r", irSensor1.read());
        //printf("DistanzLinksVorne = %.0f mm\n\r", irSensor2.read());
        //printf("DistanzLinksHinten = %.0f mm\n\r", irSensor3.read());
        //printf("WandRechts = %d \n\r", wallRight);
        //printf("WandVorne = %d \n\r", wallFront);
        //printf("WandLinks = %d \n\r", wallLeft);


        // printf("CountsRight = %d\n", counterRight.read());
        // printf("CountsLeft = %d\n", counterLeft.read());
        //kontrastSensor.check();
        //printf("SchwarzeLinie  =  %d", blackLine);
        myled =! myled; // LED is ON Heartbeat
        wait(0.1f);




    }
}