Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/MotorControl/MotorControl.cpp@25:b16f1045108f, 2013-04-10 (annotated)

Committer:
madcowswe
Date:
Wed Apr 10 02:01:51 2013 +0000
Revision:
25:b16f1045108f
Parent:
22:6e3218cf75f8
Child:
28:4e20b44251c6
Motion and motor works, but needs tuning

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 22:6e3218cf75f8 1
madcowswe 22:6e3218cf75f8 2 #include "MainMotor.h"
madcowswe 22:6e3218cf75f8 3 #include "Encoder.h"
madcowswe 22:6e3218cf75f8 4 #include "globals.h"
madcowswe 22:6e3218cf75f8 5 #include <algorithm>
madcowswe 22:6e3218cf75f8 6
madcowswe 25:b16f1045108f 7 namespace MotorControl
madcowswe 25:b16f1045108f 8 {
madcowswe 22:6e3218cf75f8 9
madcowswe 22:6e3218cf75f8 10 float fwdcmd = 0;
madcowswe 25:b16f1045108f 11 float omegacmd = 0;
madcowswe 22:6e3218cf75f8 12
madcowswe 25:b16f1045108f 13 void motor_control_isr()
madcowswe 25:b16f1045108f 14 {
madcowswe 25:b16f1045108f 15
madcowswe 22:6e3218cf75f8 16 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
madcowswe 25:b16f1045108f 17
madcowswe 25:b16f1045108f 18 float testspeed = 0.2;
madcowswe 25:b16f1045108f 19 float Pgain = 10;
madcowswe 25:b16f1045108f 20 static float lastT = SystemTime.read();
madcowswe 22:6e3218cf75f8 21 static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 22:6e3218cf75f8 22 static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 25:b16f1045108f 23
madcowswe 25:b16f1045108f 24 static float thetafiltstate = 0;
madcowswe 25:b16f1045108f 25 static float fwdfiltstate = 0;
madcowswe 25:b16f1045108f 26
madcowswe 25:b16f1045108f 27 float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 25:b16f1045108f 28 float dright = currright - lastright;
madcowswe 25:b16f1045108f 29 lastright = currright;
madcowswe 25:b16f1045108f 30
madcowswe 25:b16f1045108f 31 float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 25:b16f1045108f 32 float dleft = currleft - lastleft;
madcowswe 25:b16f1045108f 33 lastleft = currleft;
madcowswe 25:b16f1045108f 34
madcowswe 25:b16f1045108f 35 float currtime = SystemTime.read();
madcowswe 25:b16f1045108f 36 float dt = currtime - lastT;
madcowswe 25:b16f1045108f 37 lastT = currtime;
madcowswe 25:b16f1045108f 38
madcowswe 25:b16f1045108f 39 thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
madcowswe 25:b16f1045108f 40 fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt));
madcowswe 25:b16f1045108f 41
madcowswe 25:b16f1045108f 42 float errfwd = fwdfiltstate - fwdcmd;
madcowswe 25:b16f1045108f 43 float errtheta = thetafiltstate - omegacmd;
madcowswe 25:b16f1045108f 44
madcowswe 25:b16f1045108f 45 float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f);
madcowswe 25:b16f1045108f 46 float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f);
madcowswe 25:b16f1045108f 47
madcowswe 25:b16f1045108f 48 mleft(max(min(errleft*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
madcowswe 25:b16f1045108f 49 mright(max(min(errright*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
madcowswe 25:b16f1045108f 50
madcowswe 22:6e3218cf75f8 51 }
madcowswe 22:6e3218cf75f8 52
madcowswe 22:6e3218cf75f8 53 }