Diff: main.cpp

Revision:

18:3ee1b02ed3aa

Parent:

17:caf5ae550f2e

Child:

19:adae367247d4

--- a/main.cpp	Tue Apr 25 12:26:04 2017 +0000
+++ b/main.cpp	Tue May 09 15:25:54 2017 +0000
@@ -1,7 +1,11 @@
 #include <mbed.h>
 #include "Roboter.h"
 
-DigitalOut led(LED1);
+
+//Periphery for Encoder
+EncoderCounter counterLeft(PB_6, PB_7);
+EncoderCounter counterRight(PA_6, PC_7);
+
 
 //Periphery for distance sensors
 AnalogIn distance(PB_1);
@@ -12,11 +16,10 @@
 IRSensor sensors[6];
 
 // Periphery for servos
-Servo servos1(PB_7);
-Servo servos2(PA_6);
+Servo servo1(PB_13);    //Greiferservo Anschluss D2
+Servo servo2(PA_10);    //Armservo Anschluss D0
 
 //Periphery for Switch ON/OFF
-
 DigitalIn switchOnOff(USER_BUTTON);
 
 //motor stuff
@@ -27,124 +30,71 @@
 //indicator leds arround robot
 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
 
-Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servos1,&servos2);
+// Erstellen eines Roboterobjekts
+Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servo1, &servo2, &counterLeft, &counterRight, &enableMotorDriver);
 
 int main()
 {
-    enable = 1;  // Sensoren einschalten
-    enableMotorDriver = 1; // Schaltet die Motoren ein
-    pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
-    pwmR.period(0.00005f);
-    pwmL = 0.5f;
-    pwmR = 0.5f;
-
-
-    int state = 0; // Diese Variable gibt an in welchem State man sich befindet
-    int tempState = 2;
-    int time1 = 0;
-    int time2 = 0;
+    int state = 0;         // Diese Variable gibt an in welchem State man sich befindet
+    int tempState = 2;     // Wird benötig um zwischen Kameraauswertung und fahren zu wechseln
+    enable = 1;            // Sensoren einschalten
+    servo1.Enable(1500,20000);
+    servo2.Enable(700,20000);
+    float camValue;
 
     while(1) {
 
-        int camValue = readCamera();
+        //printf("%d\n", state);
 
         switch(state) {
 
             case 0: // Roboter Anschalten mit Knopf
                 printf("0");
 
-                if (switchOnOff == 0) {
+                if (switchOnOff == 0) {     // Bei Betätigung des Userbuttons wird das Programm gestartet
                     state = 1;
                 }
 
                 break;
 
             case 1:
-                printf("1");
-                if(camValue == 1 || camValue == 2 || camValue == 3) {           // Die Kamera erkennt ein grünen Klotz
-                    state = 4;
-                }
-                if(camValue == 4) {     // Roboter in Position
-                    state = 5;
-                }
-                if(camValue == 0){
-                    state = tempState;
-                }
+                //printf("1");
+                camValue = readCamera();
+                state = roboter1.stateAuswahl(camValue, tempState); // Case-Auswahl aufgrund von der Kamera
                 break;
 
             case 2:
-                printf("2");
-                if(time1 < 20) {       // Im Kreis drehen für 1s
-                    pwmL = 0.4f;
-                    pwmR = 0.4f;
-                    time1 ++;
-                    state = 1;
-                    tempState = 2;
-                } else {
-                    time1 = 0;
-                    pwmL = 0.5f;
-                    pwmR = 0.5f;
-                    state = 3;
-                }
+                //printf("2");
+                state = roboter1.kreisFahren(); // Im Kreis fahren
+                tempState = 2;
                 break;
 
 
             case 3:
-                printf("3");
-                roboter1.bandeAusweichen();
-                if(time2 < 40) {    // gerade aus fahren
-                    pwmL = 0.6f;
-                    pwmR = 0.4f;
-                    time2 ++;
-                    state = 1;
-                    tempState = 3;
-                } else {
-                    time2 = 0;
-                    pwmL = 0.5f;
-                    pwmR = 0.5f;
-                    state = 2;
-                }
+                //printf("3");
+                roboter1.bandeAusweichen();  // Hindernissen ausweichen
+                state = roboter1.geradeFahren(); // Gerade Fahren
+                tempState = 3;
                 break;
 
 
-            case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
-                printf("4");
-                roboter1.bandeAusweichen();
-                if(camValue == 1) {     // links fahren
-                    pwmL = 0.45f;
-                    pwmR = 0.45f;
-                }
-                if(camValue == 2) {     // rechts fahren
-                    pwmL = 0.55f;
-                    pwmR = 0.55f;
-                }
-                if(camValue == 3) {     // gerade fahren
-                    pwmL = 0.55f;
-                    pwmR = 0.45f;
-                }
+            case 4:
+                //printf("4");
+                roboter1.bandeAusweichen();  // Hindernissen ausweichen
+                roboter1.anfahren(camValue); // Roboter erkennt gründer Klotz, Klotz wird angefahren
                 state = 1;
-
                 break;
 
-            case 5: // Aufnehmen des Klotzes
-                printf("5");
-                pwmL = 0.5f;
-                pwmR = 0.5f;
-                state = 1;
-                time1 = 0;
-                time2 = 0;
-
+            case 5:
+                //printf("5");
+                state = roboter1.pickup(); // Aufnehmen des Klotzes
                 break;
 
-
             default:
                 break;
 
-
         }
         
-        //wait(0.1f);
+        wait(0.01f);
     }
-
 }
-