Diff: source/movement.cpp

Revision:

26:58f90fa8dbaf

Parent:

25:08ee4525155b

Child:

27:df11ab63cda4

--- a/source/movement.cpp	Sun Mar 05 12:18:04 2017 +0000
+++ b/source/movement.cpp	Sun Mar 05 17:11:17 2017 +0000
@@ -30,6 +30,19 @@
 static int EncoderCounterRight = 0;
 DigitalOut led(LED1); // Board LED
 
+// Encoder Interrupt ***********************************************************
+void highPulseDetectedL()
+{
+    EncoderCounterLeft += 1;
+}
+
+void highPulseDetectedR()
+{
+    EncoderCounterRight += 1;
+}
+
+//******************************************************************************
+
 void move_init()
 {
     pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
@@ -43,12 +56,11 @@
     EncoderCounterRight = 0;
     PID_correction_value = 1.0f;
 }
-
+//******************************************************************************
 void move_forward_slow(float correction_value)
 {
     pwmL.write(power_value_slow);
     pwmR.write(1-power_value_slow*correction_value);
-
 }
 
 void move_forward_medium(float correction_value)
@@ -57,12 +69,6 @@
     pwmR = 1-power_value_medium*correction_value;
 }
 
-void move_forward_fast(float correction_value)
-{
-    pwmL = power_value_fast;
-    pwmR = 1-power_value_fast*correction_value;
-}
-
 void move_backward_slow(float correction_value)
 {
     pwmL = 1-power_value_slow*correction_value;
@@ -75,12 +81,7 @@
     pwmR = power_value_slow;
 
 }
-
-void move_backward_fast(float correction_value)
-{
-    pwmL = 1-power_value_slow*correction_value;
-    pwmR = power_value_slow;
-}
+//******************************************************************************
 
 void stop_movement()
 {
@@ -88,32 +89,15 @@
     pwmR = 0.5f;
 }
 
-// Encoder Interrupt
-void highPulseDetectedL()
-{
-    EncoderCounterLeft += 1;
-}
-
-void highPulseDetectedR()
-{
-    EncoderCounterRight += 1;
-}
-
 void sync_movement(bool speed, bool direction)
 {
-
-// PID correction Value calcualtion
-    //printf("left: %d || right: %d\r\n",EncoderCounterLeft,EncoderCounterRight);
     if(EncoderCounterLeft > EncoderCounterRight) {
         PID_correction_value += 0.0001f;
-
-        //  printf("Left higher  ");
     } else {
         if(EncoderCounterLeft < EncoderCounterRight) {
             PID_correction_value -= 0.0001f;
-            //   printf("Right higher  ");
         } else {
-            //   printf("Even  ");
+            // even 
         }
     }
 
@@ -142,8 +126,10 @@
     }
 }
 
-void termiante_sync_movement()
+void terminate_movement()
 {
     PID_correction_value = 1.0f;
+    pwmL.write(0.5f);
+    pwmR.write(0.5f);
 }