Diff: source/movement.cpp

Revision:

21:cb40c0533bc2

Parent:

20:859c7aebf8a6

Child:

22:c8e187b9d949

--- a/source/movement.cpp	Thu Mar 02 16:20:41 2017 +0000
+++ b/source/movement.cpp	Thu Mar 02 20:07:41 2017 +0000
@@ -16,11 +16,23 @@
 static float power_value_fast = 0.8f;
 static float ludicrous_value = 1.0f;
 
+DigitalOut led(LED1); // Board LED
 
 //motor stuff
 DigitalOut enableMotorDriver(PB_2);
 PwmOut pwmL(PA_8);
 PwmOut pwmR(PA_9);
+DigitalIn motorDriverFault(PB_14);
+DigitalIn motorDriverWarning(PB_15);
+
+InterruptIn EncoderLeftA(PB_6);
+InterruptIn EncoderLeftB(PB_7);
+InterruptIn EncoderRightA(PA_6);
+InterruptIn EncoderRightB(PA_7);
+
+static int EncoderCounterLeft = 0;
+static int EncoderCounterRight = 0;
+
 
 void move_init()
 {
@@ -29,70 +41,98 @@
 
     pwmL = 0.5f; // Setzt die Duty-Cycle auf 50%
     pwmR = 0.5f;
-    enableMotorDriver = 0;
+    enableMotorDriver = 1;
+
+    EncoderCounterLeft = 0;
+    EncoderCounterRight = 0;
+    PID_correction_value = 1.0f;
 }
 
 void move_forward_slow(float correction_value)
 {
     pwmL = power_value_slow;
     pwmR = 1-power_value_slow*correction_value;
-    enableMotorDriver = 1;
 }
 
 void move_forward_medium(float correction_value)
 {
     pwmL = power_value_medium;
     pwmR = 1-power_value_medium*correction_value;
-    enableMotorDriver = 1;
 }
 
 void move_forward_fast(float correction_value)
 {
     pwmL = power_value_fast;
     pwmR = 1-power_value_fast*correction_value;
-    enableMotorDriver = 1;
 }
 
 void move_backward_slow(float correction_value)
 {
     pwmL = 1-power_value_slow*correction_value;
     pwmR = power_value_slow;
-    enableMotorDriver = 1;
 }
 
 void move_backward_medium(float correction_value)
 {
     pwmL = 1-power_value_slow*correction_value;
     pwmR = power_value_slow;
-    enableMotorDriver = 1;
 }
 
 void move_backward_fast(float correction_value)
 {
     pwmL = 1-power_value_slow*correction_value;
     pwmR = power_value_slow;
-    enableMotorDriver = 1;
 }
 
 void stop_movement()
 {
-    enableMotorDriver = 1;
+    pwmL = 0.5f;
+    pwmR = 0.5f;
 }
 
-void sync_movement(bool speed, bool direction)
-// PID correction Value calcualtion
+void highPulseDetectedL()
+{
+    EncoderCounterLeft += 1;
+}
+
+void highPulseDetectedR()
+{
+    EncoderCounterRight +=1;
+}
 
 
 
 
+void sync_movement(bool speed, bool direction)
+{
+
+    EncoderLeftA.rise(&highPulseDetectedL);
+    EncoderLeftB.rise(&highPulseDetectedL);
+    EncoderRightA.rise(&highPulseDetectedR);
+    EncoderRightB.rise(&highPulseDetectedR);
 
 
+// PID correction Value calcualtion
 
+    if(EncoderCounterLeft > EncoderCounterRight) {
+        PID_correction_value -= 0.0001f;
+        led = 1;
+    } else {
+        led = 0;
+        PID_correction_value += 0.0001f;
+    }
 
+    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
-{
+
     if(direction && speed) {
         move_forward_medium(PID_correction_value);
     }
@@ -102,12 +142,13 @@
     if(!direction && speed) {
         move_backward_medium(PID_correction_value);
     }
-    if(!direction && !speed {
+    if(!direction && !speed) {
     move_backward_slow(PID_correction_value);
     }
 }
 
-void termiante_sync_movement(){
+void termiante_sync_movement()
+{
     PID_correction_value = 1.0f;
-    }
+}