Diff: Processes/MotorControl/MotorControl.cpp

Revision:

63:c2c6269767b8

Parent:

62:78d99b781f02

Child:

64:c979fb1cd3b5

--- a/Processes/MotorControl/MotorControl.cpp	Sun Apr 14 12:57:04 2013 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Sun Apr 14 14:32:43 2013 +0000
@@ -25,12 +25,12 @@
 
     float testspeed = 0.2;
     float Fcrit = 1.75;
-    float Pcrit = 10*0.5;
+    float Pcrit = 10;
     float Pgain = Pcrit*0.45;
-    float Igain = 1.2f*Pgain*Fcrit*0.2;
+    float Igain = 1.2f*Pgain*Fcrit*0.05;
     
     float testrot = 0.5*PI;
-    float Pcrit_rot = 10;
+    float Pcrit_rot = 20;
     float Pgain_rot = Pcrit_rot*0.45f;
     float Fcrit_rot = 1.75f;
     float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.1;
@@ -53,7 +53,7 @@
 
     float currtime = SystemTime.read();
     float dt = currtime - lastT;
-    dt = 0.05; //TODO: HACK!
+    //dt = 0.05; //TODO: HACK!
     lastT = currtime;
 
     thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
@@ -64,20 +64,26 @@
     
     static float fwdIstate = 0;
     static float rotIstate = 0;
-    fwdIstate += errfwd;
-    rotIstate += errtheta;
     
-    float actuatefwd = errfwd*Pgain + fwdIstate*Igain + max(power_per_dc_m_per_s*abs(fwdcmd), hysteresis_pwr)*sgn(fwdcmd);
+    float actuatefwd = errfwd*Pgain + fwdIstate*Igain;// + max(power_per_dc_m_per_s*abs(fwdcmd), hysteresis_pwr)*sgn(fwdcmd);
     float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;
 
     float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
     float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f);
+    
+    float lff = fwdcmd - omegacmd*ENCODER_WHEELBASE/2.0f;
+    float rff = fwdcmd + omegacmd*ENCODER_WHEELBASE/2.0f;
 
-    mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
-    mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    mleft(max(min(actuateleft + max(power_per_dc_m_per_s*abs(lff), hysteresis_pwr)*sgn(lff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    mright(max(min(actuateright + max(power_per_dc_m_per_s*abs(rff), hysteresis_pwr)*sgn(rff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
     
-    mfwdpowdbg = fwdIstate*Igain;
-    mrotpowdbg = rotIstate*Igain_rot;
+    if (!(abs(actuateleft) > MOTOR_MAX_POWER || abs(actuateright) > MOTOR_MAX_POWER)){
+        fwdIstate += errfwd;
+        rotIstate += errtheta;
+    }
+    
+    //mfwdpowdbg = 0;//fwdfiltstate;//;
+    //mrotpowdbg = rotIstate*Igain_rot;//thetafiltstate;//;
 
 }