Diff: main.cpp

Revision:

10:e8110fb94686

Parent:

9:ab19796bf14a

diff -r ab19796bf14a -r e8110fb94686 main.cpp
--- a/main.cpp	Wed May 16 16:41:44 2018 +0000
+++ b/main.cpp	Tue May 22 14:20:20 2018 +0000
@@ -46,6 +46,7 @@
 
 int main()
 {
+    //printf("--------\n\rREADY\n\r--------\n\r");
     enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
     enable = 1;
 
@@ -57,25 +58,25 @@
     int lab[5][10];
     int path = 0;
     int pathNext = 0;
-    int pathArr[50];
-    int directions[50];
+    int pathArr[51];
+    int directions[51];
     int modeStart = 0;
     int startx = 0;
-    int starty = 10;
+    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...    
+    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) {
@@ -83,38 +84,37 @@
                 }
                 firstMove = 1;
                 state = 1;
-                
+
                 break;
             }
             case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
-            
+
                 checkWalls.check(); //prueft wo Waende vorhanden sind
-                
+
 
-                
+
                 turn.turning();
-                
-                if(firstMove == 1){
+
+                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++;
-                    }
-                    /***********************/ 
+                    //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: {
@@ -225,16 +225,16 @@
                         }
                     }
                 }
-                
+
                 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);
@@ -242,12 +242,12 @@
 
                 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;
@@ -273,21 +273,21 @@
 
                         }
                     }//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) {
@@ -298,7 +298,7 @@
                     } else if(pathArr[c-1]==pathArr[c]) {
                         directions[c]=1;
                     } else {
-                        
+
                         if(pathArr[c]==1) {
                             if(pathArr[c-1]==3) {
                                 directions[c]=2;
@@ -347,6 +347,21 @@
                 }
                 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);
                 }
@@ -358,29 +373,31 @@
                 break;
             }
             case 3: {//2. Lauf Fahrzyklus
-            
+
                 //uebergibt Anweisung an turn
-                
+
                 modeStart=1;
                 path = directions[c];
-                pathNext = directions[c+1]; 
-                
-                   
+                pathNext = directions[c+1];
+
+
                 turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
-                
-                
+
+
                 /**************************/
-                if(firstMove == 1){
+                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;
                 }