File content as of revision 23:6e3218cf75f8:

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

namespace MotorControl{

float fwdcmd = 0;
float thetacmd = 0;

void motor_control_loop(){
    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
    
    float Pgain = -0.01;
    static float lastT = SystemTime;
    static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
    static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
    
    while(true){
        
        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;
        currleft = 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 - thetacmd;
        
        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));
    }
}

}