Diff: Processes/MotorControl/MotorControl.cpp

Revision:

60:5058465904e0

Parent:

34:e1678450feec

Child:

62:78d99b781f02

--- a/Processes/MotorControl/MotorControl.cpp	Fri Apr 12 22:00:32 2013 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Sat Apr 13 22:41:00 2013 +0000
@@ -8,13 +8,15 @@
 namespace MotorControl
 {
 
-float fwdcmd = 0;
-float omegacmd = 0;
+volatile float fwdcmd = 0;
+volatile float omegacmd = 0;
 
 void motor_control_isr()
 {
 
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
+    
+    const float power_per_dc_m_per_s = 1.0f/0.6f;
 
     float testspeed = 0.2;
     float Fcrit = 1.75;
@@ -51,15 +53,15 @@
     thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
     fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt));
 
-    float errfwd = fwdfiltstate - fwdcmd;
-    float errtheta = thetafiltstate - omegacmd;
+    float errfwd = fwdcmd - fwdfiltstate;
+    float errtheta = omegacmd - thetafiltstate;
     
     static float fwdIstate = 0;
     static float rotIstate = 0;
     fwdIstate += errfwd;
     rotIstate += errtheta;
     
-    float actuatefwd = errfwd*Pgain + fwdIstate*Igain;
+    float actuatefwd = errfwd*Pgain + fwdIstate*Igain + power_per_dc_m_per_s*fwdcmd;
     float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;
 
     float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);