Diff: main.cpp

Revision:

5:b8b1a979b0d5

Parent:

4:e3f388933954

Child:

7:5ef09519a6e9

--- a/main.cpp	Thu Apr 12 16:14:02 2018 +0000
+++ b/main.cpp	Wed Apr 25 12:07:03 2018 +0000
@@ -37,7 +37,7 @@
 IRSensor irSensor0(distance0);    //rechts
 IRSensor irSensor1(distance1);    //vorne
 IRSensor irSensor2(distance2);    //links-vorne
-IRSensor irSensor3(distance3);    //links-hinten 
+IRSensor irSensor3(distance3);    //links-hinten
 
 
 //AnalogIn distance(PB_1); //Kreieren der Ein - und Ausgangsobjekte
@@ -66,117 +66,161 @@
 
 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
 
-
-//Turn turn(); //GEHT NICHT!!!
 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 wallLeft = 0;
     int blackLine = 0;
     
+//    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
 
-    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
-    enable = 1;
+    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);   //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); //20cm nach vorne...//Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
 
 
     while(1) { // Wiederholungsschleife
 
-        //Ablauf ist jeweils immer Feldweise...Waende checken, allenfalls drehen (Ausrichten), waehrend der Fahrt nur Ausrichtung der Geradeausfahrt...
-    
-        //printf("WARTE AUF START....");
+
+        switch(state) { //Wartemodus 1. Lauf
+            case 0: {
+
+
+                while (button) {
+                    //printf("WARTE AUF 1.START...");
+                    wait(1.0f);         //Die Ruhe vor dem Sturm...
+                }
+
+                state = 1;
+                break;
+            }
+            case 1: {// 1.Lauf Fahrzyklus und Aufzeichungszyklus
+
+                checkWalls.check(); //prueft wo Waende vorhanden sind
+
+                //wallRight = 1;
+                //wallFront = 1;
+                //wallLeft = 1;
+
+                turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
+
+
+                //saveWay
+                //Abspeicherung der Waende...
+//                    if(wallRight == 1){
+//                        way[0][0][1] = 1;
+//                    } else {
+//                        way[0][0][1] = 0;
+//                    }
+//
+//                    if(wallRight == 1){
+//                        way[0][0][2] = 1;
+//                    } else{
+//                        way[0][0][2] = 0;
+//                    }
+//
+//                    if(wallRight == 1){
+//                        way[0][0][3] = 1;
+//                    } else{
+//                        way[0][0][3] = 0;
+//                    }
+                //Abspeicherung des entschiedenen Wegs und Anpassung des
+
+
+//                    printf("%d", way[0][0][0]);
+
+                drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
+
+
+
+                //blackLine = 1;
+                if(blackLine == 1) { //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
 
-        while (button) {
-           wait(1.0f); //Die Ruhe vor dem Sturm...
+                    state = 2;
+                }
+                break;
+            }
+            case 2: { //Ziel erreicht und Wartemodus fuer 2. Lauf
+
+                controller.setDesiredSpeedRight(0.0f);
+                controller.setDesiredSpeedLeft(0.0f);
+
+                while (button) {
+                    //printf("WARTE AUF 2.START...");
+                    wait(1.0f);
+                }
+                blackLine = 0;
+                state = 3;
+                break;
+            }
+            case 3: {//2. Lauf Fahrzyklus
+
+
+                //pruefug der waender entfaellt, da jetzt die daten aus dem speicher geladen werden
+                checkWalls.check(); //prueft wo Waende vorhanden sind
+
+                turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
+
+
+
+                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.
+
+                    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;
+            }
         }
 
 
 
-        while(mode==1) {
 
-            //Ermittlung der Abstaende rund ums Fahrzeug
-            float distanceRight = irSensor0.read();
-            float distanceFront = irSensor1.read();
-            float distanceLeftFront = irSensor2.read();
-            float distanceLeftBack = irSensor3.read();
-             
-
-            
-            CheckWalls checkWalls(distanceRight, distanceFront, distanceLeftFront, wallRight, wallFront, wallLeft); //Ermittlung wo freie Wege(0) bzw welche Waende vorhanden (1)
-            //Decide??
-            Turn turn(counterLeft, counterRight, controller, wallRight, wallFront, wallLeft);   //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); //20cm nach vorne...
-                                                           
-            
-            switch(state) { //Ausrichten
-                case 0: {
-                    // case 0 wird evtl nicht benötigt
-                    state = 1;
-                    break;
-                }
-                case 1: {//Fahrzyklus
-                    
-                    checkWalls.check(); //prueft wo Waende vorhanden sind
-                    
-                    //wallRight = 1;
-                    //wallFront = 1;
-                    //wallLeft = 1;
-                    
-                    //turn.turning(); //entscheidet welche Richtung und Wendet dann zu dieser...
-                    
-
-                    drive.driving(); //faehrt, kontrolliert ob richtig steht und achtet auch schwarze Linie am Boden
-
-                   
-                    
-                    //blackLine = 1;
-                    if(blackLine == 1){  //ist schwarze Linie ueberfahren...stoppt der Fahrzyklus hier.
-                    
-                       state = 2;
-                    }
-                    break;
-                }
-                case 2: { //Ziel erreicht
-                    
-                        controller.setDesiredSpeedRight(0.0f);
-                        controller.setDesiredSpeedLeft(0.0f);
-                    
-                    //"Stop"
-                    //mode=2
-                    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("DistanzRechts = %.0f mm\n\r", distanceRight);
-            printf("DistanzVorne = %.0f mm\n\r", distanceFront);
-            printf("DistanzLinksVorne = %.0f mm\n\r", distanceLeftFront);
-            printf("DistanzLinksHinten = %.0f mm\n\r", distanceLeftBack);
-            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.5f); 
+        // 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);
 
-        }//Ende while mode==1
         
 
+
     }
 }