Diff: Processes/MotorControl/MotorControl.cpp

Revision:

62:78d99b781f02

Parent:

60:5058465904e0

Child:

63:c2c6269767b8

--- a/Processes/MotorControl/MotorControl.cpp	Sun Apr 14 10:49:51 2013 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Sun Apr 14 12:57:04 2013 +0000
@@ -4,6 +4,7 @@
 #include "globals.h"
 #include <algorithm>
 #include "system.h"
+#include "supportfuncs.h"
 
 namespace MotorControl
 {
@@ -11,12 +12,16 @@
 volatile float fwdcmd = 0;
 volatile float omegacmd = 0;
 
+volatile float mfwdpowdbg = 0;
+volatile float mrotpowdbg = 0;
+
+MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
+
 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;
+    const float power_per_dc_m_per_s = 1.64f;
+    const float hysteresis_pwr = 0.16f;
 
     float testspeed = 0.2;
     float Fcrit = 1.75;
@@ -48,6 +53,7 @@
 
     float currtime = SystemTime.read();
     float dt = currtime - lastT;
+    dt = 0.05; //TODO: HACK!
     lastT = currtime;
 
     thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
@@ -61,7 +67,7 @@
     fwdIstate += errfwd;
     rotIstate += errtheta;
     
-    float actuatefwd = errfwd*Pgain + fwdIstate*Igain + power_per_dc_m_per_s*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);
@@ -69,6 +75,9 @@
 
     mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
     mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    
+    mfwdpowdbg = fwdIstate*Igain;
+    mrotpowdbg = rotIstate*Igain_rot;
 
 }