File content as of revision 27:7cb3a21d9a2e:

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

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 Pgain = 10;
    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;

    float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f);
    float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f);

    mleft(max(min(errleft*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
    mright(max(min(errright*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));

}

}