Diff: Processes/MotorControl/MotorControl.cpp

Revision:

23:6e3218cf75f8

Child:

26:b16f1045108f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Tue Apr 09 20:41:22 2013 +0000
@@ -0,0 +1,51 @@
+
+#include "MainMotor.h"
+#include "Encoder.h"
+#include "globals.h"
+#include <algorithm>
+
+namespace MotorControl{
+
+float fwdcmd = 0;
+float thetacmd = 0;
+
+void motor_control_loop(){
+    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
+    
+    float Pgain = -0.01;
+    static float lastT = SystemTime;
+    static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
+    static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
+    
+    while(true){
+        
+        static float thetafiltstate = 0;
+        static float fwdfiltstate = 0;
+        
+        float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
+        float dright = currright - lastright;
+        lastright = currright;
+        
+        float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
+        float dleft = currleft - lastleft;
+        currleft = currleft;
+        
+        float currtime = SystemTime.read();
+        float dt = currtime - lastT;
+        lastT = currtime;
+        
+        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 - thetacmd;
+        
+        float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f);
+        float errright = errfwd + (errtheta*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));
+    }
+}
+
+}
\ No newline at end of file