Dhiren Lad / PID_Test

code to go straight with pid

Dependencies:   AVEncoder QEI mbed-src-AV

Files at this revision

API Documentation at this revision

Revision 1:b9485bb1ca69, committed 2015-11-18

Comitter:
mochaMan
Date:
Wed Nov 18 19:15:26 2015 -0800
Parent:
0:99dd5499928a
Commit message:
a

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- 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)