Diff: MotorDriver.cpp

Revision:

1:4808f55970e8

Parent:

0:e38b500d6e74

--- a/MotorDriver.cpp	Thu Apr 19 11:31:49 2018 +0000
+++ b/MotorDriver.cpp	Sun Apr 22 09:55:40 2018 +0000
@@ -16,6 +16,8 @@
 EncoderCounter counterLeft(PB_6, PB_7);
 EncoderCounter counterRight(PA_6, PC_7);
 
+DigitalOut myled(LED2);
+
 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
 
 void resetCounter(void);
@@ -27,44 +29,26 @@
     counterRight.reset();
 }
 
-void driveOne(int NumbField, int Speed)
-{
-    switch(Speed) {
-        case 1:
+void driveOne(int NumbField, int SpeedMode)
 
-            controller.setDesiredSpeedLeft(-30.0f); // Drehzahl in [rpm]
-            controller.setDesiredSpeedRight(30.0f);
-            printf("SpeedSet\n");
-            //Fährt bis Anzahl umdrehungen erreicht sind
-            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 1000*NumbField) {
+{
+    int AnzahlZyklen = 0;
+    float distance;
+    float CorrectFactor;
+    float Speed = 0.0f;
 
-            }
-            printf("number erreicht");
-            stop();
-
+    switch(SpeedMode) {
+        case 1:
+            Speed = 20.0f;
             break;
 
 
         case 2:
-            controller.setDesiredSpeedLeft(-150.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
-            controller.setDesiredSpeedRight(150.0f);
-            //Fährt bis Anzahl umdrehungen erreicht sind
-            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 2000*NumbField) {
-
-            }
-            stop();
-
+            Speed = 40.0f;
             break;
 
         case 3:
-            controller.setDesiredSpeedLeft(-20.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
-            controller.setDesiredSpeedRight(20.0f);
-            //Fährt bis Anzahl umdrehungen erreicht sind
-            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 2000*NumbField) {
-                //do nothing
-            }
-            stop();
-
+            Speed = 80.0f;
             break;
 
 
@@ -72,6 +56,25 @@
 
             break;
     }
+
+    //Fährt bis Anzahl umdrehungen erreicht sind
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 1000*NumbField) {
+        distance = readSensorValue(1);      //Distanz zur linken Wand wird gemessen
+        AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
+        distance = distance - 7.8f;         //Distanz zwischen Position und Ideallinie wird berechnet
+        CorrectFactor = distance*0.8f;      //P Faktor des P-Reglers
+
+        //printf("%f\n",CorrectFactor);
+        if(abs(CorrectFactor) <= 5.0f) {    //erkennt Wand oder Lücke auf linker Seite
+            controller.setDesiredSpeedLeft((Speed * (-1.0f)) - CorrectFactor); //Bei Wand wird korrigiert
+            controller.setDesiredSpeedRight(Speed - CorrectFactor);
+        } else {
+            controller.setDesiredSpeedLeft(Speed * (-1.0f));                 //Bei Lücke wird nach Counter gefahren
+            controller.setDesiredSpeedRight(Speed);
+        }
+    }
+    stop();
+    printf("Das Programm wurde: %d durchlaufen", AnzahlZyklen);
 }
 
 int turnRight(int direction)
@@ -129,17 +132,3 @@
     stop();
 }
 
-void orientate(void)
-{
-    while(ReadSensorValue(1) > ReadSensorValue(3))
-    {
-        driveDist(1,1);
-    }
-    while(ReadSensorValue(1) < ReadSensorValue(3))
-    {
-        driveDist(1,-1);
-    }
-    //Differenz Sensor zu Drehpunkt vorwärts fahren
-    
-}
-