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?
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 | 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 | } |