Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/MotorControl/MotorControl.cpp@88:8850373c3f0d, 2013-04-16 (annotated)

Committer:
madcowswe
Date:
Tue Apr 16 12:55:10 2013 +0000
Revision:
88:8850373c3f0d
Parent:
75:283283604485 
prelim final RED

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