File content as of revision 73:265d3cc6b0b1:

#include "MainMotor.h"
#include "Encoder.h"
#include "globals.h"
#include <algorithm>
#include "system.h"
#include "supportfuncs.h"

namespace MotorControl
{

volatile bool motorsenabled = 0;

volatile float fwdcmd = 0;
volatile float omegacmd = 0;

volatile float mfwdpowdbg = 0;
volatile float mrotpowdbg = 0;

MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);

void motor_control_isr()
{
    const float power_per_dc_m_per_s = 1.64f;
    const float hysteresis_pwr = 0.16f;

    float testspeed = 0.2;
    float Fcrit = 1.75;
    float Pcrit = 10;
    float Pgain = Pcrit*0.45;
    float Igain = 1.2f*Pgain*Fcrit*0.05;
    
    float testrot = 0.5*PI;
    float Pcrit_rot = 20;
    float Pgain_rot = Pcrit_rot*0.45f;
    float Fcrit_rot = 1.75f;
    float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.1;
    
    //float Dgain = 
    static float lastT = SystemTime.read();
    static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
    static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;

    static float thetafiltstate = 0;
    static float fwdfiltstate = 0;

    float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
    float dright = currright - lastright;
    lastright = currright;

    float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
    float dleft = currleft - lastleft;
    lastleft = currleft;

    float currtime = SystemTime.read();
    float dt = currtime - lastT;
    //dt = 0.05; //TODO: HACK!
    lastT = currtime;

    thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
    fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt));

    float errfwd = fwdcmd - fwdfiltstate;
    float errtheta = omegacmd - thetafiltstate;
    
    static float fwdIstate = 0;
    static float rotIstate = 0;
    
    float actuatefwd = errfwd*Pgain + fwdIstate*Igain;// + max(power_per_dc_m_per_s*abs(fwdcmd), hysteresis_pwr)*sgn(fwdcmd);
    float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;

    float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
    float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f);
    
    float lff = fwdcmd - omegacmd*ENCODER_WHEELBASE/2.0f;
    float rff = fwdcmd + omegacmd*ENCODER_WHEELBASE/2.0f;
    
    if(motorsenabled){

        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));
        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));
        
        if (!(abs(actuateleft) > MOTOR_MAX_POWER || abs(actuateright) > MOTOR_MAX_POWER)){
            fwdIstate += errfwd;
            rotIstate += errtheta;
        }
    
    } else {
        fwdIstate = 0;
        rotIstate = 0;
    }
    
    //mfwdpowdbg = 0;//fwdfiltstate;//;
    //mrotpowdbg = rotIstate*Igain_rot;//thetafiltstate;//;

}

}