2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/MotorControl/MotorControl.cpp@92:4a1225fbb146, 2013-10-15 (annotated)
- Committer:
- rsavitski
- Date:
- Tue Oct 15 12:19:32 2013 +0000
- Revision:
- 92:4a1225fbb146
- Parent:
- 88:8850373c3f0d
touch: ripped out 2013-specific bits. Need to address "2014" comments. Rewrite AI layer and other deleted parts.
Who changed what in which revision?
User | Revision | Line number | New 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 | 62:78d99b781f02 | 7 | #include "supportfuncs.h" |
madcowswe | 22:6e3218cf75f8 | 8 | |
madcowswe | 25:b16f1045108f | 9 | namespace MotorControl |
madcowswe | 25:b16f1045108f | 10 | { |
madcowswe | 22:6e3218cf75f8 | 11 | |
madcowswe | 73:265d3cc6b0b1 | 12 | volatile bool motorsenabled = 0; |
madcowswe | 73:265d3cc6b0b1 | 13 | |
madcowswe | 60:5058465904e0 | 14 | volatile float fwdcmd = 0; |
madcowswe | 60:5058465904e0 | 15 | volatile float omegacmd = 0; |
madcowswe | 22:6e3218cf75f8 | 16 | |
madcowswe | 62:78d99b781f02 | 17 | volatile float mfwdpowdbg = 0; |
madcowswe | 62:78d99b781f02 | 18 | volatile float mrotpowdbg = 0; |
madcowswe | 62:78d99b781f02 | 19 | |
madcowswe | 62:78d99b781f02 | 20 | MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); |
madcowswe | 62:78d99b781f02 | 21 | |
madcowswe | 25:b16f1045108f | 22 | void motor_control_isr() |
madcowswe | 25:b16f1045108f | 23 | { |
madcowswe | 62:78d99b781f02 | 24 | const float power_per_dc_m_per_s = 1.64f; |
madcowswe | 62:78d99b781f02 | 25 | const float hysteresis_pwr = 0.16f; |
madcowswe | 25:b16f1045108f | 26 | |
madcowswe | 25:b16f1045108f | 27 | float testspeed = 0.2; |
madcowswe | 33:a49197572737 | 28 | float Fcrit = 1.75; |
madcowswe | 88:8850373c3f0d | 29 | float Pcrit = 8; |
madcowswe | 33:a49197572737 | 30 | float Pgain = Pcrit*0.45; |
madcowswe | 63:c2c6269767b8 | 31 | float Igain = 1.2f*Pgain*Fcrit*0.05; |
madcowswe | 33:a49197572737 | 32 | |
madcowswe | 33:a49197572737 | 33 | float testrot = 0.5*PI; |
madcowswe | 63:c2c6269767b8 | 34 | float Pcrit_rot = 20; |
madcowswe | 33:a49197572737 | 35 | float Pgain_rot = Pcrit_rot*0.45f; |
madcowswe | 33:a49197572737 | 36 | float Fcrit_rot = 1.75f; |
madcowswe | 34:e1678450feec | 37 | float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.1; |
madcowswe | 33:a49197572737 | 38 | |
madcowswe | 33:a49197572737 | 39 | //float Dgain = |
madcowswe | 25:b16f1045108f | 40 | static float lastT = SystemTime.read(); |
madcowswe | 22:6e3218cf75f8 | 41 | static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK; |
madcowswe | 22:6e3218cf75f8 | 42 | static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK; |
madcowswe | 25:b16f1045108f | 43 | |
madcowswe | 25:b16f1045108f | 44 | static float thetafiltstate = 0; |
madcowswe | 25:b16f1045108f | 45 | static float fwdfiltstate = 0; |
madcowswe | 25:b16f1045108f | 46 | |
madcowswe | 25:b16f1045108f | 47 | float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK; |
madcowswe | 25:b16f1045108f | 48 | float dright = currright - lastright; |
madcowswe | 25:b16f1045108f | 49 | lastright = currright; |
madcowswe | 25:b16f1045108f | 50 | |
madcowswe | 25:b16f1045108f | 51 | float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK; |
madcowswe | 25:b16f1045108f | 52 | float dleft = currleft - lastleft; |
madcowswe | 25:b16f1045108f | 53 | lastleft = currleft; |
madcowswe | 25:b16f1045108f | 54 | |
madcowswe | 25:b16f1045108f | 55 | float currtime = SystemTime.read(); |
madcowswe | 25:b16f1045108f | 56 | float dt = currtime - lastT; |
madcowswe | 63:c2c6269767b8 | 57 | //dt = 0.05; //TODO: HACK! |
madcowswe | 25:b16f1045108f | 58 | lastT = currtime; |
madcowswe | 25:b16f1045108f | 59 | |
madcowswe | 25:b16f1045108f | 60 | thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE)); |
madcowswe | 25:b16f1045108f | 61 | fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt)); |
madcowswe | 25:b16f1045108f | 62 | |
madcowswe | 60:5058465904e0 | 63 | float errfwd = fwdcmd - fwdfiltstate; |
madcowswe | 60:5058465904e0 | 64 | float errtheta = omegacmd - thetafiltstate; |
madcowswe | 33:a49197572737 | 65 | |
madcowswe | 33:a49197572737 | 66 | static float fwdIstate = 0; |
madcowswe | 33:a49197572737 | 67 | static float rotIstate = 0; |
madcowswe | 33:a49197572737 | 68 | |
madcowswe | 63:c2c6269767b8 | 69 | float actuatefwd = errfwd*Pgain + fwdIstate*Igain;// + max(power_per_dc_m_per_s*abs(fwdcmd), hysteresis_pwr)*sgn(fwdcmd); |
madcowswe | 33:a49197572737 | 70 | float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot; |
madcowswe | 25:b16f1045108f | 71 | |
madcowswe | 33:a49197572737 | 72 | float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f); |
madcowswe | 33:a49197572737 | 73 | float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f); |
madcowswe | 63:c2c6269767b8 | 74 | |
madcowswe | 63:c2c6269767b8 | 75 | float lff = fwdcmd - omegacmd*ENCODER_WHEELBASE/2.0f; |
madcowswe | 63:c2c6269767b8 | 76 | float rff = fwdcmd + omegacmd*ENCODER_WHEELBASE/2.0f; |
madcowswe | 62:78d99b781f02 | 77 | |
madcowswe | 73:265d3cc6b0b1 | 78 | if(motorsenabled){ |
madcowswe | 73:265d3cc6b0b1 | 79 | |
madcowswe | 73:265d3cc6b0b1 | 80 | mleft(max(min(actuateleft + max(power_per_dc_m_per_s*abs(lff), min(hysteresis_pwr, 3.0f*abs(lff)))*sgn(lff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); |
madcowswe | 73:265d3cc6b0b1 | 81 | mright(max(min(actuateright + max(power_per_dc_m_per_s*abs(rff), min(hysteresis_pwr, 3.0f*abs(rff)))*sgn(rff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); |
madcowswe | 73:265d3cc6b0b1 | 82 | |
madcowswe | 73:265d3cc6b0b1 | 83 | if (!(abs(actuateleft) > MOTOR_MAX_POWER || abs(actuateright) > MOTOR_MAX_POWER)){ |
madcowswe | 73:265d3cc6b0b1 | 84 | fwdIstate += errfwd; |
madcowswe | 73:265d3cc6b0b1 | 85 | rotIstate += errtheta; |
madcowswe | 73:265d3cc6b0b1 | 86 | } |
madcowswe | 73:265d3cc6b0b1 | 87 | |
madcowswe | 73:265d3cc6b0b1 | 88 | } else { |
madcowswe | 75:283283604485 | 89 | mleft(0); |
madcowswe | 75:283283604485 | 90 | mright(0); |
madcowswe | 73:265d3cc6b0b1 | 91 | fwdIstate = 0; |
madcowswe | 73:265d3cc6b0b1 | 92 | rotIstate = 0; |
madcowswe | 63:c2c6269767b8 | 93 | } |
madcowswe | 63:c2c6269767b8 | 94 | |
madcowswe | 63:c2c6269767b8 | 95 | //mfwdpowdbg = 0;//fwdfiltstate;//; |
madcowswe | 63:c2c6269767b8 | 96 | //mrotpowdbg = rotIstate*Igain_rot;//thetafiltstate;//; |
madcowswe | 25:b16f1045108f | 97 | |
madcowswe | 22:6e3218cf75f8 | 98 | } |
madcowswe | 22:6e3218cf75f8 | 99 | |
madcowswe | 22:6e3218cf75f8 | 100 | } |