Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

Processes/MotorControl/MotorControl.cpp@23:6e3218cf75f8, 2013-04-09 (annotated)

Committer:
madcowswe
Date:
Tue Apr 09 20:41:22 2013 +0000
Revision:
23:6e3218cf75f8
Child:
26:b16f1045108f
MotorControl compiles

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 23:6e3218cf75f8 1
madcowswe 23:6e3218cf75f8 2 #include "MainMotor.h"
madcowswe 23:6e3218cf75f8 3 #include "Encoder.h"
madcowswe 23:6e3218cf75f8 4 #include "globals.h"
madcowswe 23:6e3218cf75f8 5 #include <algorithm>
madcowswe 23:6e3218cf75f8 6
madcowswe 23:6e3218cf75f8 7 namespace MotorControl{
madcowswe 23:6e3218cf75f8 8
madcowswe 23:6e3218cf75f8 9 float fwdcmd = 0;
madcowswe 23:6e3218cf75f8 10 float thetacmd = 0;
madcowswe 23:6e3218cf75f8 11
madcowswe 23:6e3218cf75f8 12 void motor_control_loop(){
madcowswe 23:6e3218cf75f8 13 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
madcowswe 23:6e3218cf75f8 14
madcowswe 23:6e3218cf75f8 15 float Pgain = -0.01;
madcowswe 23:6e3218cf75f8 16 static float lastT = SystemTime;
madcowswe 23:6e3218cf75f8 17 static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 23:6e3218cf75f8 18 static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 23:6e3218cf75f8 19
madcowswe 23:6e3218cf75f8 20 while(true){
madcowswe 23:6e3218cf75f8 21
madcowswe 23:6e3218cf75f8 22 static float thetafiltstate = 0;
madcowswe 23:6e3218cf75f8 23 static float fwdfiltstate = 0;
madcowswe 23:6e3218cf75f8 24
madcowswe 23:6e3218cf75f8 25 float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 23:6e3218cf75f8 26 float dright = currright - lastright;
madcowswe 23:6e3218cf75f8 27 lastright = currright;
madcowswe 23:6e3218cf75f8 28
madcowswe 23:6e3218cf75f8 29 float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
madcowswe 23:6e3218cf75f8 30 float dleft = currleft - lastleft;
madcowswe 23:6e3218cf75f8 31 currleft = currleft;
madcowswe 23:6e3218cf75f8 32
madcowswe 23:6e3218cf75f8 33 float currtime = SystemTime.read();
madcowswe 23:6e3218cf75f8 34 float dt = currtime - lastT;
madcowswe 23:6e3218cf75f8 35 lastT = currtime;
madcowswe 23:6e3218cf75f8 36
madcowswe 23:6e3218cf75f8 37 thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
madcowswe 23:6e3218cf75f8 38 fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt));
madcowswe 23:6e3218cf75f8 39
madcowswe 23:6e3218cf75f8 40 float errfwd = fwdfiltstate - fwdcmd;
madcowswe 23:6e3218cf75f8 41 float errtheta = thetafiltstate - thetacmd;
madcowswe 23:6e3218cf75f8 42
madcowswe 23:6e3218cf75f8 43 float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f);
madcowswe 23:6e3218cf75f8 44 float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f);
madcowswe 23:6e3218cf75f8 45
madcowswe 23:6e3218cf75f8 46 mleft(max(min(errleft*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
madcowswe 23:6e3218cf75f8 47 mright(max(min(errright*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
madcowswe 23:6e3218cf75f8 48 }
madcowswe 23:6e3218cf75f8 49 }
madcowswe 23:6e3218cf75f8 50
madcowswe 23:6e3218cf75f8 51 }