File content as of revision 1:c97f8bcd6c0f:

#include "motor.h"

Motor::Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dir1Pin, unsigned int dir2Pin, PinName encA, PinName encB)
    : pwm(PWMpin), extIO(ioExt), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) {
 
    PwmOut pwm(PWMpin);
    pwm.period_ms(5);
    setPoint = 0;
    pMulti = 0.2;
    iMulti = 0.01;
    error = 0;
    prevError = 0;
    P = 0.0;
    I = 0.0;
    
    currentSpeed = 0;
}

void Motor::setPWM(float newPWM) {
    if (newPWM < 0) {
        pwm = -1 * newPWM;
        extIO->setPin(dir1);
        extIO->clearPin(dir2);
    } else {
        pwm = newPWM;
        extIO->clearPin(dir1);
        extIO->setPin(dir2);
    }    
}

int Motor::getSpeed() {
    return currentSpeed;
}

int Motor::getDecoderCount() {
    currentSpeed = qed.read();
    return currentSpeed;
}

void Motor::setSpeed(int newSpeed) {
    setPoint = newSpeed;
}

void Motor::pid() {
    /*int setPoint = 0;
    float pMulti = 0.1;
    float iMulti = 0.001;
    int error = 0;
    int prevError = 0;
    float P = 0.0;
    float I = 0.0;*/
    
    prevError = error;
    error = setPoint - getDecoderCount();
    
    float err = (float)error / 250.0;
    
    I += err * iMulti;
    //constrain integral
    if (I < -1.0) I = -1.0;
    if (I > 1.0) I = 1.0;
    
    float newPWMvalue = err * pMulti + I;
    //constrain pwm
    if (newPWMvalue < -1.0) newPWMvalue = -1.0;
    if (newPWMvalue > 1.0) newPWMvalue = 1.0;
    
    setPWM(newPWMvalue);
}