File content as of revision 8:63a67086a1b5:

#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(1);
    setPoint = 0;
    pMulti = 16;
    iDiv = 2;
    dMulti = 1;
    error = 0;
    prevError = 0;
    P = 0;
    I = 0;
    minPwm = 100;
    pidMulti = 128;
    iMax = 1024 * pidMulti;
    
    currentSpeed = 0;
    
    currentPWM = 0;
    stallCount = 0;
    prevStallCount = 0;
    stallWarningLimit = 60;
    stallErrorLimit = 300;
    stallLevel = 0;
}

void Motor::setPWM(int newPWM) {
    currentPWM = newPWM;
    if (newPWM < 0) {
        pwm.pulsewidth_us(-1 * newPWM);
        extIO->setPin(dir1);
        extIO->clearPin(dir2);
    } else {
        pwm.pulsewidth_us(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;
    if (newSpeed == 0) {
        resetPID();
    }
}

void Motor::pid() {
    prevError = error;
    error = setPoint - getDecoderCount();

    if (stallLevel != 2) {
        
        //float err = (float)error / 250.0;
        float minPwmValue = minPwm;
        
        if (setPoint == 0) {
            minPwmValue = 0;
        } else if (setPoint < 0) {
            minPwmValue = -minPwm;
        }
        
        I += error * pidMulti / iDiv;
        //constrain integral
        if (I < -iMax) I = -iMax;
        if (I > iMax) I = iMax;
        
        //D = error - prevError;
        
        //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
        int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
    
        //constrain pwm
        if (newPWMvalue < -1000) newPWMvalue = -1000;
        if (newPWMvalue > 1000) newPWMvalue = 1000;    
        
        prevStallCount = stallCount;
        if ((currentSpeed < 5 && currentPWM == 1000 || currentSpeed > -5 && currentPWM == -1000) && stallCount < stallErrorLimit) {
            stallCount++;
        } else if (stallCount > 0) {
            stallCount--;
        }
        
        setPWM(newPWMvalue);
        
        if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
            stallLevel = 0;
            stallEndCallback.call();
            stallChangeCallback.call();
        } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
            stallLevel = 1;
            stallWarningCallback.call();
            stallChangeCallback.call();
        } else if (stallCount == stallErrorLimit) {
            stallLevel = 2;
            stallErrorCallback.call();
            stallChangeCallback.call();
            resetPID();
        }
    } else {
        stallCount--;
        if (stallCount == 0) {
            stallLevel = 0;
            stallEndCallback.call();
            stallChangeCallback.call();
        }
    }
}

void Motor::resetPID() {
    error = 0;
    prevError = 0;
    P = 0;
    I = 0;
    setPoint = 0;
    setPWM(0);
}

int Motor::getStallLevel() {
    return stallLevel;
}

void Motor::stallChange(void (*function)(void)) { 
    stallChangeCallback.attach(function);
}

void Motor::stallEnd(void (*function)(void)) { 
    stallEndCallback.attach(function);
}

void Motor::stallWarning(void (*function)(void)) { 
    stallWarningCallback.attach(function);
}

void Motor::stallError(void (*function)(void)) { 
    stallErrorCallback.attach(function);
}