Diff: source/movement.cpp

Revision:

25:08ee4525155b

Parent:

24:6c2fec64f890

Child:

26:58f90fa8dbaf

diff -r 6c2fec64f890 -r 08ee4525155b source/movement.cpp
--- a/source/movement.cpp	Sat Mar 04 15:04:14 2017 +0000
+++ b/source/movement.cpp	Sun Mar 05 12:18:04 2017 +0000
@@ -28,7 +28,7 @@
 
 static int EncoderCounterLeft = 0;
 static int EncoderCounterRight = 0;
-
+DigitalOut led(LED1); // Board LED
 
 void move_init()
 {
@@ -88,48 +88,41 @@
     pwmR = 0.5f;
 }
 
+// Encoder Interrupt
 void highPulseDetectedL()
 {
     EncoderCounterLeft += 1;
-    //led = 1;
 }
 
 void highPulseDetectedR()
 {
-    EncoderCounterRight +=1;
+    EncoderCounterRight += 1;
 }
 
 void sync_movement(bool speed, bool direction)
 {
 
-    /*   if(EncoderLeftA) {
-           led = 1;
-       } else {
-           led = 0;
-       }*/
-
-
 // PID correction Value calcualtion
-
+    //printf("left: %d || right: %d\r\n",EncoderCounterLeft,EncoderCounterRight);
     if(EncoderCounterLeft > EncoderCounterRight) {
         PID_correction_value += 0.0001f;
 
-        printf("Left higher");
+        //  printf("Left higher  ");
     } else {
         if(EncoderCounterLeft < EncoderCounterRight) {
             PID_correction_value -= 0.0001f;
-            printf("Right higher");
+            //   printf("Right higher  ");
         } else {
-            printf("Even");
+            //   printf("Even  ");
         }
     }
 
-    /*    if(PID_correction_value < 0.0f) {
-            PID_correction_value = 0;
-        }
-        if(PID_correction_value > 2.0f) {
-            PID_correction_value = 2;
-        }*/
+    if(PID_correction_value < 0.0f) {
+        PID_correction_value = 0;
+    }
+    if(PID_correction_value > 2.0f) {
+        PID_correction_value = 2;
+    }
 
 // Call movement:
 // direction 0 = backward, direction 1 = forward