File content as of revision 14:d0e5894aa352:

#include "motor.h"

motor::motor(PinName p_pwm, PinName p_dir, PinName p_senseA, PinName p_senseB, double p, double i, double d, bool flipped): 
    pwm(p_pwm),
    dir(p_dir),
    sense(p_senseA, p_senseB, NC, 8400, QEI::X4_ENCODING),
    bq1(2.533015E-01, 5.066030E-01, 2.533015E-01, 1.000000E+00, -4.531195E-01, 4.663256E-01),
    bq2(1.839030E-01, 3.678060E-01, 1.839030E-01, 1.000000E+00, -3.289757E-01, 6.458765E-02),
    P(p), I(i), D(d){
        flip = flipped;
        integral = 0;
        olderr = 0;
        bqc.add(&bq1).add(&bq2);
        pwm.period_ms(2);
        sense.reset();
}

void motor::setSpeed(float val){
    val = flip?-val:val;
    dir.write(val>=0?1:0);
    pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75)); // Use a root to make the motor give more power in low end.
}

double motor::getPosition(void){
    return(flip?-sense.getPulses()*(2*M_PI/8400.0):sense.getPulses()*(2*M_PI/8400.0));
}

void motor::PID(double setpoint, double dt){
    double error;
    double derivative;
    double position;
    double pid;
    
    position = this->getPosition();
    error = setpoint - position;
    integral += error * dt;
    derivative = bqc.step((error - olderr) / dt);
    olderr = error;
    pid = (P * error + I * integral + D * derivative);
    
    this->setSpeed(pid);
}

motor::~motor(){
}