main.cpp

00001 #include "mbed.h"
00002 
00003 #include "IRSensor.h"
00004 #include "EncoderCounter.h"
00005 #include "LowpassFilter.h"
00006 #include "Controller.h"
00007 #include "KontrastSensor.h"
00008 #include "Drive.h"
00009 #include "CheckWalls.h"
00010 #include "Turn.h"
00011 
00012 Serial pc(USBTX,USBRX,460800);
00013 
00014 //Initialisierung LED/Button
00015 DigitalIn button(USER_BUTTON);  //Moduswählknopf
00016 DigitalOut myled(LED1);     //Heartbeat (evt auch Anzeige für Modus, vor start
00017 
00018 
00019 //Initialisierung IR-Sensoren
00020 AnalogIn distance0(PC_2); //Kreieren der Eingangsobjekte
00021 AnalogIn distance1(PC_3);
00022 AnalogIn distance2(PC_5);
00023 AnalogIn distance3(PB_1);
00024 IRSensor irSensor0(distance0);    //rechts
00025 IRSensor irSensor1(distance1);    //vorne
00026 IRSensor irSensor2(distance2);    //links-vorne
00027 IRSensor irSensor3(distance3);    //links-hinten
00028 
00029 //Initialisierung Kontrastsensor
00030 AnalogIn kontrast(PC_0);
00031 
00032 //Initialisierung Motor und Encoder
00033 DigitalOut enableMotorDriver(PB_2);
00034 PwmOut pwmLeft(PA_8);   //Motor links
00035 PwmOut pwmRight(PA_9);  //Motor rechts
00036 EncoderCounter counterLeft(PB_6, PB_7); //Encoder für Motor links
00037 EncoderCounter counterRight(PA_6, PC_7);//Encoder für Motor rechts
00038 
00039 DigitalOut enable(PC_1);
00040 
00041 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
00042 
00043 int state = 0;
00044 int mode = 1;
00045 
00046 
00047 int main()
00048 {
00049     //printf("--------\n\rREADY\n\r--------\n\r");
00050     enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
00051     enable = 1;
00052 
00053     int wallRight = 0;
00054     int wallFront = 0;
00055     int wallLeft = 0;
00056     int blackLine = 0;
00057     int dontStop = 0;
00058     int lab[5][10];
00059     int path = 0;
00060     int pathNext = 0;
00061     int pathArr[51];
00062     int directions[51];
00063     int modeStart = 0;
00064     int startx = 0;
00065     int starty = 9;
00066     int ausr = 1;
00067     int c = 0;
00068     int firstMove = 0;
00069 
00070     //Funktionsdeklarationen
00071 
00072     CheckWalls checkWalls(irSensor0, irSensor1, irSensor2, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
00073     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
00074     KontrastSensor kontrastSensor(kontrast, blackLine);
00075     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...
00076 
00077 
00078     while(1) { // Wiederholungsschleife
00079 
00080         switch(state) { //Wartemodus 1. Lauf
00081             case 0: {
00082                 while (button) {
00083                     wait(1.0f);         //Die Ruhe vor dem Sturm...
00084                 }
00085                 firstMove = 1;
00086                 state = 1;
00087 
00088                 break;
00089             }
00090             case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
00091 
00092                 checkWalls.check(); //prueft wo Waende vorhanden sind
00093 
00094 
00095 
00096                 turn.turning();
00097 
00098                 if(firstMove == 1) {
00099                     dontStop = 1;
00100                     firstMove = 0;
00101 
00102                     /* **************************** */
00103                     /* Anpassung 16.02.18 */
00104                     //if (wallFront == 0) {
00105                     //    ausr = 1;
00106                     //    lab[startx][starty] = ausr;
00107                     //    starty--;
00108                     //} else if (wallRight == 20) {
00109                     //    ausr = 3;       // Ausrichtung zu kontrollieren!!!!
00110                     //    lab[startx][starty] = ausr;
00111                     //    startx++;
00112                     //}
00113                     /***********************/
00114                 }
00115 
00116                 //merkt sich wie sich die Ausrichtung des Fahrzeugs im Labyrinth veraendert und speichert diese in ein Array
00117 
00118                 if(wallLeft == 0) {
00119                     switch(ausr) {
00120                         case 1: {
00121                             ausr = 2;
00122                             lab[startx][starty] = ausr;
00123                             startx--;
00124                             break;
00125                         }
00126                         case 2: {
00127                             ausr = 4;
00128                             lab[startx][starty] = ausr;
00129                             starty++;
00130                             break;
00131                         }
00132                         case 3: {
00133                             ausr = 1;
00134                             lab[startx][starty] = ausr;
00135                             starty--;
00136                             break;
00137                         }
00138                         case 4: {
00139                             ausr = 3;
00140                             lab[startx][starty] = ausr;
00141                             startx++;
00142                             break;
00143                         }
00144                     }
00145 
00146                 } else if(wallFront == 0) {
00147                     switch(ausr) {
00148                         case 1: {
00149                             ausr = 1;
00150                             lab[startx][starty] = ausr;
00151                             starty--;
00152                             break;
00153                         }
00154                         case 2: {
00155                             ausr = 2;
00156                             lab[startx][starty] = ausr;
00157                             startx--;
00158                             break;
00159                         }
00160                         case 3: {
00161                             ausr = 3;
00162                             lab[startx][starty] = ausr;
00163                             startx++;
00164                             break;
00165                         }
00166                         case 4: {
00167                             ausr = 4;
00168                             lab[startx][starty] = ausr;
00169                             starty++;
00170                             break;
00171                         }
00172                     }
00173                 } else if(wallRight == 0) {
00174                     switch(ausr) {
00175                         case 1: {
00176                             ausr = 3;
00177                             lab[startx][starty] = ausr;
00178                             startx++;
00179                             break;
00180                         }
00181                         case 2: {
00182                             ausr = 1;
00183                             lab[startx][starty] = ausr;
00184                             starty--;
00185                             break;
00186                         }
00187                         case 3: {
00188                             ausr = 4;
00189                             lab[startx][starty] = ausr;
00190                             starty++;
00191                             break;
00192                         }
00193                         case 4: {
00194                             ausr = 2;
00195                             lab[startx][starty] = ausr;
00196                             startx--;
00197                             break;
00198                         }
00199                     }
00200                 } else {
00201                     switch(ausr) {
00202                         case 1: {
00203                             ausr = 4;
00204                             lab[startx][starty] = ausr;
00205                             starty++;
00206                             break;
00207                         }
00208                         case 2: {
00209                             ausr = 3;
00210                             lab[startx][starty] = ausr;
00211                             startx++;
00212                             break;
00213                         }
00214                         case 3: {
00215                             ausr = 2;
00216                             lab[startx][starty] = ausr;
00217                             startx--;
00218                             break;
00219                         }
00220                         case 4: {
00221                             ausr = 1;
00222                             lab[startx][starty] = ausr;
00223                             starty--;
00224                             break;
00225                         }
00226                     }
00227                 }
00228 
00229                 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
00230 
00231                 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
00232                     lab[startx][starty] = 5;
00233                     state = 2;
00234                 }
00235                 break;
00236             }
00237 
00238             case 2: { //Ziel erreicht => Wartemodus fuer 2. Lauf und Berechnung der schnellsten Weges
00239 
00240                 controller.setDesiredSpeedRight(0.0f);
00241                 controller.setDesiredSpeedLeft(0.0f);
00242 
00243                 startx = 0;
00244                 starty = 9;
00245 
00246                 int num;
00247                 c = 0;
00248 
00249                 //Speichert die 2-Dimensionale Karte in ein 1D-Array welche nun aus anweisungen besteht
00250 
00251                 while(lab[startx][starty] != 5) {
00252                     num = lab[startx][starty];
00253                     pathArr[c] = num;
00254 
00255                     switch(num) {
00256                         case 1: {
00257                             starty--;
00258                             break;
00259                         }
00260                         case 2: {
00261                             startx--;
00262                             break;
00263                         }
00264                         case 3: {
00265                             startx++;
00266                             break;
00267                         }
00268                         case 4: {
00269                             starty++;
00270                             break;
00271                         }
00272                         case 5: {
00273 
00274                         }
00275                     }//Switch End
00276 
00277                     myled =! myled; // LED is ON Heartbeat
00278                     wait(0.4f);
00279                     c++;
00280                 }//While Ende
00281 
00282                 pathArr[c]= 5; //setzt end bit
00283 
00284 
00285                 c=0;
00286 
00287 
00288 
00289                 //übersetzt den 1-Dimensionalen Array der Ausrichtung in einen Array nun Anweisungen zur Lösung darstellt
00290 
00291                 while(pathArr[c]!= 5) {
00292                     if(c==0) {
00293                         if(pathArr[c]==1) {
00294                             directions[c]=1;
00295                         } else {
00296                             directions[c]=3;
00297                         }
00298                     } else if(pathArr[c-1]==pathArr[c]) {
00299                         directions[c]=1;
00300                     } else {
00301 
00302                         if(pathArr[c]==1) {
00303                             if(pathArr[c-1]==3) {
00304                                 directions[c]=2;
00305                             }
00306                             if(pathArr[c-1]==2) {
00307                                 directions[c]=3;
00308                             }
00309                             if(pathArr[c-1]==4) {
00310                                 directions[c-1]=4;
00311                             }
00312                         } else if(pathArr[c]==2) {
00313                             if(pathArr[c-1]==1) {
00314                                 directions[c]=2;
00315                             }
00316                             if(pathArr[c-1]==3) {
00317                                 directions[c]=4;
00318                             }
00319                             if(pathArr[c-1]==4) {
00320                                 directions[c]=3;
00321                             }
00322                         } else if(pathArr[c]==3) {
00323                             if(pathArr[c-1]==1) {
00324                                 directions[c]=3;
00325                             }
00326                             if(pathArr[c-1]==2) {
00327                                 directions[c]=4;
00328                             }
00329                             if(pathArr[c-1]==4) {
00330                                 directions[c]=2;
00331                             }
00332                         } else if(pathArr[c]==4) {
00333                             if(pathArr[c-1]==1) {
00334                                 directions[c]=4;
00335                             }
00336                             if(pathArr[c-1]==2) {
00337                                 directions[c]=2;
00338                             }
00339                             if(pathArr[c-1]==3) {
00340                                 directions[c]=3;
00341                             }
00342                         }
00343                     }
00344                     c++;
00345                     myled =! myled; // LED is ON Heartbeat
00346                     wait(0.1f);
00347                 }
00348                 c = 0;
00349 
00350 
00351                 for(int p=0; p<20; p++) {
00352                     //printf("Anw: %d\r\n", directions[p]);
00353                 }
00354 
00355                 for(int p=0; p<9; p++) {
00356                     for(int q=0; q<4; q++) {
00357                         //printf(" %d ", lab[q][p]);
00358                     }
00359                     //printf("\r\n");
00360                 }
00361 
00362 
00363 
00364 
00365                 while (button) {
00366                     wait(1.0f);
00367                 }
00368                 blackLine = 0;
00369                 firstMove = 1;
00370                 state = 3;
00371                 myled = 0; // LED is ON Heartbeat
00372                 c = 0;
00373                 break;
00374             }
00375             case 3: {//2. Lauf Fahrzyklus
00376 
00377                 //uebergibt Anweisung an turn
00378 
00379                 modeStart=1;
00380                 path = directions[c];
00381                 pathNext = directions[c+1];
00382 
00383 
00384                 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
00385 
00386 
00387                 /**************************/
00388                 if(firstMove == 1) {
00389                     dontStop = 1;
00390                     firstMove = 0;
00391                 }
00392                 /***********************/
00393 
00394 
00395 
00396                 drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
00397                 c++;
00398 
00399 
00400 
00401                 if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
00402                     state = 4;
00403                 }
00404                 break;
00405             }
00406             case 4: { //Ziel erreicht und Wartemodus fuer 2. Lauf
00407                 controller.setDesiredSpeedRight(0.0f);
00408                 controller.setDesiredSpeedLeft(0.0f);
00409 
00410                 while (button) {
00411                     wait(1.0f);
00412                 }
00413                 state = 5;
00414                 break;
00415             }
00416             default: {
00417                 state = 0;
00418                 break;
00419             }
00420         }
00421         myled =! myled; // LED is ON Heartbeat
00422         wait(0.1f);
00423     }
00424 }