Diff: source/movement.cpp

Revision:

22:c8e187b9d949

Parent:

21:cb40c0533bc2

Child:

23:4ddc4216f335

--- a/source/movement.cpp	Thu Mar 02 20:07:41 2017 +0000
+++ b/source/movement.cpp	Fri Mar 03 20:44:01 2017 +0000
@@ -16,7 +16,6 @@
 static float power_value_fast = 0.8f;
 static float ludicrous_value = 1.0f;
 
-DigitalOut led(LED1); // Board LED
 
 //motor stuff
 DigitalOut enableMotorDriver(PB_2);
@@ -25,10 +24,7 @@
 DigitalIn motorDriverFault(PB_14);
 DigitalIn motorDriverWarning(PB_15);
 
-InterruptIn EncoderLeftA(PB_6);
-InterruptIn EncoderLeftB(PB_7);
-InterruptIn EncoderRightA(PA_6);
-InterruptIn EncoderRightB(PA_7);
+DigitalOut led(LED1); // Board LED
 
 static int EncoderCounterLeft = 0;
 static int EncoderCounterRight = 0;
@@ -50,8 +46,9 @@
 
 void move_forward_slow(float correction_value)
 {
-    pwmL = power_value_slow;
-    pwmR = 1-power_value_slow*correction_value;
+   pwmL = power_value_slow;
+   pwmR = 1-power_value_slow*correction_value;
+
 }
 
 void move_forward_medium(float correction_value)
@@ -76,6 +73,7 @@
 {
     pwmL = 1-power_value_slow*correction_value;
     pwmR = power_value_slow;
+
 }
 
 void move_backward_fast(float correction_value)
@@ -93,6 +91,7 @@
 void highPulseDetectedL()
 {
     EncoderCounterLeft += 1;
+   // led = 1;
 }
 
 void highPulseDetectedR()
@@ -100,35 +99,35 @@
     EncoderCounterRight +=1;
 }
 
-
-
-
 void sync_movement(bool speed, bool direction)
 {
 
-    EncoderLeftA.rise(&highPulseDetectedL);
-    EncoderLeftB.rise(&highPulseDetectedL);
-    EncoderRightA.rise(&highPulseDetectedR);
-    EncoderRightB.rise(&highPulseDetectedR);
+ /*   if(EncoderLeftA) {
+        led = 1;
+    } else {
+        led = 0;
+    }*/
 
 
 // PID correction Value calcualtion
 
-    if(EncoderCounterLeft > EncoderCounterRight) {
-        PID_correction_value -= 0.0001f;
-        led = 1;
-    } else {
-        led = 0;
-        PID_correction_value += 0.0001f;
-    }
+       if(EncoderCounterLeft > EncoderCounterRight) {
+           PID_correction_value += 0.0001f;
+
+           printf("Left higher");
+       } else {
 
-    if(PID_correction_value < 0.0f) {
-        PID_correction_value = 0;
-    }
-    if(PID_correction_value > 2.0f) {
-        PID_correction_value = 2;
-    }
-    
+           PID_correction_value -= 0.0001f;
+           printf("right higher");
+       }
+
+    /*    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
 // speed 0 = slow, speed 1 = medium
@@ -143,7 +142,7 @@
         move_backward_medium(PID_correction_value);
     }
     if(!direction && !speed) {
-    move_backward_slow(PID_correction_value);
+        move_backward_slow(PID_correction_value);
     }
 }