File content as of revision 46:adcd57a5e402:

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

namespace MotorControl
{

float fwdcmd = 0;
float omegacmd = 0;

void motor_control_isr()
{

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

    float testspeed = 0.2;
    float Fcrit = 1.75;
    float Pcrit = 10*0.5;
    float Pgain = Pcrit*0.45;
    float Igain = 1.2f*Pgain*Fcrit*0.2;
    
    float testrot = 0.5*PI;
    float Pcrit_rot = 10;
    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;
    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 = fwdfiltstate - fwdcmd;
    float errtheta = thetafiltstate - omegacmd;
    
    static float fwdIstate = 0;
    static float rotIstate = 0;
    fwdIstate += errfwd;
    rotIstate += errtheta;
    
    float actuatefwd = errfwd*Pgain + fwdIstate*Igain;
    float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;

    float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
    float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f);

    mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
    mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));

}

}