Diff: Processes/MotorControl/MotorControl.cpp

Revision:

34:a49197572737

Parent:

29:4e20b44251c6

Child:

35:e1678450feec

--- a/Processes/MotorControl/MotorControl.cpp	Wed Apr 10 18:25:16 2013 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Wed Apr 10 19:52:19 2013 +0000
@@ -17,7 +17,18 @@
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
 
     float testspeed = 0.2;
-    float Pgain = 10;
+    float Fcrit = 1.75;
+    float Pcrit = 10;
+    float Pgain = Pcrit*0.45;
+    float Igain = 1.2f*Pgain*Fcrit*0.2;
+    
+    float testrot = 0.5*PI;
+    float Pcrit_rot = 10;
+    float Pgain_rot = Pcrit_rot*0.45f;
+    float Fcrit_rot = 1.75f;
+    float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.07;
+    
+    //float Dgain = 
     static float lastT = SystemTime.read();
     static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
     static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
@@ -42,12 +53,20 @@
 
     float errfwd = fwdfiltstate - fwdcmd;
     float errtheta = thetafiltstate - omegacmd;
+    
+    static float fwdIstate = 0;
+    static float rotIstate = 0;
+    fwdIstate += errfwd;
+    rotIstate += errtheta;
+    
+    float actuatefwd = errfwd*Pgain + fwdIstate*Igain;
+    float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;
 
-    float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f);
-    float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f);
+    float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
+    float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f);
 
-    mleft(max(min(errleft*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
-    mright(max(min(errright*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
 
 }