Diff: source/movement.cpp

Revision:

27:df11ab63cda4

Parent:

26:58f90fa8dbaf

Child:

29:e7d0208bf2af

--- a/source/movement.cpp	Sun Mar 05 17:11:17 2017 +0000
+++ b/source/movement.cpp	Mon Mar 06 15:10:24 2017 +0000
@@ -5,7 +5,8 @@
 
 
 #include "mbed.h"
-#include "move.h"
+#include "movement.h"
+#include "EncoderCounter.h"
 
 static double time_counter = 0.0f;
 static double timer0 = 0.0f;
@@ -16,6 +17,9 @@
 static float power_value_fast = 0.8f;
 static float ludicrous_value = 1.0f;
 
+//Motor Encoders
+EncoderCounter counterLeft(PB_6, PB_7);
+EncoderCounter counterRight(PA_6, PC_7);
 
 //motor stuff
 DigitalOut enableMotorDriver(PB_2);
@@ -26,20 +30,8 @@
 
 //DigitalOut led(LED1); // Board LED
 
-static int EncoderCounterLeft = 0;
-static int EncoderCounterRight = 0;
 DigitalOut led(LED1); // Board LED
 
-// Encoder Interrupt ***********************************************************
-void highPulseDetectedL()
-{
-    EncoderCounterLeft += 1;
-}
-
-void highPulseDetectedR()
-{
-    EncoderCounterRight += 1;
-}
 
 //******************************************************************************
 
@@ -52,9 +44,8 @@
     pwmR.write(0.5f);
     enableMotorDriver = 1;
 
-    EncoderCounterLeft = 0;
-    EncoderCounterRight = 0;
     PID_correction_value = 1.0f;
+    
 }
 //******************************************************************************
 void move_forward_slow(float correction_value)
@@ -87,14 +78,17 @@
 {
     pwmL = 0.5f;
     pwmR = 0.5f;
+    counterLeft.reset();
+    counterRight.reset();
 }
 
 void sync_movement(bool speed, bool direction)
 {
-    if(EncoderCounterLeft > EncoderCounterRight) {
+ printf("Left: %d || Right: %d\r\n",counterLeft.read(), -counterRight.read());
+    if(counterLeft.read() > -counterRight.read()) {
         PID_correction_value += 0.0001f;
     } else {
-        if(EncoderCounterLeft < EncoderCounterRight) {
+        if(counterLeft.read() < -counterRight.read()) {
             PID_correction_value -= 0.0001f;
         } else {
             // even