Diff: main.cpp

Revision:

1:b9485bb1ca69

Parent:

0:99dd5499928a

diff -r 99dd5499928a -r b9485bb1ca69 main.cpp
--- a/main.cpp	Thu Nov 19 03:01:07 2015 +0000
+++ b/main.cpp	Wed Nov 18 19:15:26 2015 -0800
@@ -11,8 +11,8 @@
 PwmOut RMotorForward(PB_10);
 PwmOut RMotorReverse(PB_4);
 
-QEI LeftEncoder(PA_15, PB_3, NC, 12);
-QEI RightEncoder(PA_1, PA_2, NC, 12);
+AVEncoder LeftEncoder(PA_15, PB_3);
+AVEncoder RightEncoder(PA_1, PA_2);
 
 Serial pc(USBTX, USBRX);
 
@@ -45,7 +45,8 @@
 
 void pid()
 {
-    speed += P_controller(0) + D_controller(0) + I_controller(0);
+	unsigned long int error = LeftEncoder.getPulses() - RightEncoder.getPulses();
+    speed += P_controller(error) + D_controller(error) + I_controller(error);
     
     if(speed < 0)
     {
@@ -55,6 +56,9 @@
     {
         speed = 1;
     }
+	
+	LeftEncoder.reset();
+	RightEncoder.reset();
 }
 
 double P_controller(int error)