Diff: Processes/MotorControl/MotorControl.cpp

Revision:

25:b16f1045108f

Parent:

22:6e3218cf75f8

Child:

28:4e20b44251c6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -0,0 +1,53 @@
+
+#include "MainMotor.h"
+#include "Encoder.h"
+#include "globals.h"
+#include <algorithm>
+
+namespace MotorControl
+{
+
+float fwdcmd = 0;
+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);
+
+    float testspeed = 0.2;
+    float Pgain = 10;
+    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;
+
+    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;
+    lastleft = 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 - omegacmd;
+
+    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