Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/MotorControl/MotorControl.cpp@60:5058465904e0, 2013-04-13 (annotated)

Committer:
madcowswe
Date:
Sat Apr 13 22:41:00 2013 +0000
Revision:
60:5058465904e0
Parent:
34:e1678450feec
Child:
62:78d99b781f02
Added feed forward. Maybe positive feedback

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 60:5058465904e0 11 volatile float fwdcmd = 0;
madcowswe 60:5058465904e0 12 volatile 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 60:5058465904e0 18
madcowswe 60:5058465904e0 19 const float power_per_dc_m_per_s = 1.0f/0.6f;
madcowswe 25:b16f1045108f 20
madcowswe 25:b16f1045108f 21 float testspeed = 0.2;
madcowswe 33:a49197572737 22 float Fcrit = 1.75;
madcowswe 34:e1678450feec 23 float Pcrit = 10*0.5;
madcowswe 33:a49197572737 24 float Pgain = Pcrit*0.45;
madcowswe 33:a49197572737 25 float Igain = 1.2f*Pgain*Fcrit*0.2;
madcowswe 33:a49197572737 26
madcowswe 33:a49197572737 27 float testrot = 0.5*PI;
madcowswe 33:a49197572737 28 float Pcrit_rot = 10;
madcowswe 33:a49197572737 29 float Pgain_rot = Pcrit_rot*0.45f;
madcowswe 33:a49197572737 30 float Fcrit_rot = 1.75f;
madcowswe 34:e1678450feec 31 float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.1;
madcowswe 33:a49197572737 32
madcowswe 33:a49197572737 33 //float Dgain =
madcowswe 25:b16f1045108f 34 static float lastT = SystemTime.read();
madcowswe 22:6e3218cf75f8 35 static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 22:6e3218cf75f8 36 static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 25:b16f1045108f 37
madcowswe 25:b16f1045108f 38 static float thetafiltstate = 0;
madcowswe 25:b16f1045108f 39 static float fwdfiltstate = 0;
madcowswe 25:b16f1045108f 40
madcowswe 25:b16f1045108f 41 float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 25:b16f1045108f 42 float dright = currright - lastright;
madcowswe 25:b16f1045108f 43 lastright = currright;
madcowswe 25:b16f1045108f 44
madcowswe 25:b16f1045108f 45 float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 25:b16f1045108f 46 float dleft = currleft - lastleft;
madcowswe 25:b16f1045108f 47 lastleft = currleft;
madcowswe 25:b16f1045108f 48
madcowswe 25:b16f1045108f 49 float currtime = SystemTime.read();
madcowswe 25:b16f1045108f 50 float dt = currtime - lastT;
madcowswe 25:b16f1045108f 51 lastT = currtime;
madcowswe 25:b16f1045108f 52
madcowswe 25:b16f1045108f 53 thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
madcowswe 25:b16f1045108f 54 fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt));
madcowswe 25:b16f1045108f 55
madcowswe 60:5058465904e0 56 float errfwd = fwdcmd - fwdfiltstate;
madcowswe 60:5058465904e0 57 float errtheta = omegacmd - thetafiltstate;
madcowswe 33:a49197572737 58
madcowswe 33:a49197572737 59 static float fwdIstate = 0;
madcowswe 33:a49197572737 60 static float rotIstate = 0;
madcowswe 33:a49197572737 61 fwdIstate += errfwd;
madcowswe 33:a49197572737 62 rotIstate += errtheta;
madcowswe 33:a49197572737 63
madcowswe 60:5058465904e0 64 float actuatefwd = errfwd*Pgain + fwdIstate*Igain + power_per_dc_m_per_s*fwdcmd;
madcowswe 33:a49197572737 65 float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;
madcowswe 25:b16f1045108f 66
madcowswe 33:a49197572737 67 float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
madcowswe 33:a49197572737 68 float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f);
madcowswe 25:b16f1045108f 69
madcowswe 33:a49197572737 70 mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
madcowswe 33:a49197572737 71 mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
madcowswe 25:b16f1045108f 72
madcowswe 22:6e3218cf75f8 73 }
madcowswe 22:6e3218cf75f8 74
madcowswe 22:6e3218cf75f8 75 }