MotorControl.cpp

00001 
00002 #include "MainMotor.h"
00003 #include "Encoder.h"
00004 #include "globals.h"
00005 #include <algorithm>
00006 #include "system.h"
00007 #include "supportfuncs.h"
00008 
00009 namespace MotorControl
00010 {
00011 
00012 volatile bool motorsenabled = 0;
00013 
00014 volatile float fwdcmd = 0;
00015 volatile float omegacmd = 0;
00016 
00017 volatile float mfwdpowdbg = 0;
00018 volatile float mrotpowdbg = 0;
00019 
00020 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
00021 
00022 void motor_control_isr()
00023 {
00024     const float power_per_dc_m_per_s = 1.64f;
00025     const float hysteresis_pwr = 0.16f;
00026 
00027     float testspeed = 0.2;
00028     float Fcrit = 1.75;
00029     float Pcrit = 8;
00030     float Pgain = Pcrit*0.45;
00031     float Igain = 1.2f*Pgain*Fcrit*0.05;
00032     
00033     float testrot = 0.5*PI;
00034     float Pcrit_rot = 20;
00035     float Pgain_rot = Pcrit_rot*0.45f;
00036     float Fcrit_rot = 1.75f;
00037     float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.1;
00038     
00039     //float Dgain = 
00040     static float lastT = SystemTime.read();
00041     static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
00042     static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
00043 
00044     static float thetafiltstate = 0;
00045     static float fwdfiltstate = 0;
00046 
00047     float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
00048     float dright = currright - lastright;
00049     lastright = currright;
00050 
00051     float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
00052     float dleft = currleft - lastleft;
00053     lastleft = currleft;
00054 
00055     float currtime = SystemTime.read();
00056     float dt = currtime - lastT;
00057     //dt = 0.05; //TODO: HACK!
00058     lastT = currtime;
00059 
00060     thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
00061     fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt));
00062 
00063     float errfwd = fwdcmd - fwdfiltstate;
00064     float errtheta = omegacmd - thetafiltstate;
00065     
00066     static float fwdIstate = 0;
00067     static float rotIstate = 0;
00068     
00069     float actuatefwd = errfwd*Pgain + fwdIstate*Igain;// + max(power_per_dc_m_per_s*abs(fwdcmd), hysteresis_pwr)*sgn(fwdcmd);
00070     float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;
00071 
00072     float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
00073     float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f);
00074     
00075     float lff = fwdcmd - omegacmd*ENCODER_WHEELBASE/2.0f;
00076     float rff = fwdcmd + omegacmd*ENCODER_WHEELBASE/2.0f;
00077     
00078     if(motorsenabled){
00079 
00080         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));
00081         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));
00082         
00083         if (!(abs(actuateleft) > MOTOR_MAX_POWER || abs(actuateright) > MOTOR_MAX_POWER)){
00084             fwdIstate += errfwd;
00085             rotIstate += errtheta;
00086         }
00087     
00088     } else {
00089         mleft(0);
00090         mright(0);
00091         fwdIstate = 0;
00092         rotIstate = 0;
00093     }
00094     
00095     //mfwdpowdbg = 0;//fwdfiltstate;//;
00096     //mrotpowdbg = rotIstate*Igain_rot;//thetafiltstate;//;
00097 
00098 }
00099 
00100 }