Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/MotorControl/MotorControl.cpp@28:4e20b44251c6, 2013-04-10 (annotated)

Committer:
madcowswe
Date:
Wed Apr 10 04:20:40 2013 +0000
Revision:
28:4e20b44251c6
Parent:
25:b16f1045108f
Child:
33:a49197572737
Idle CPU measure implemented, not tested

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