File content as of revision 1:464cd2cb852e:

#include "a3930.h"

A3930::A3930(PinName PWMpin, PCA9555 *ioExt, unsigned int dirPin, unsigned int brakePin, unsigned int coastPin, PinName tachoPin, PinName diroPin)
    : pwm(PWMpin), extIO(ioExt), dirPinNumber(dirPin), brakePinNumber(brakePin), coastPinNumber(coastPin), interruptTacho(tachoPin), interruptDiro(diroPin) {
 
    pwmPeriod = 1.0f / 50000.0f;
    PwmOut pwm(PWMpin);
    pwm.period(pwmPeriod);
    setPoint = 0;
    pMulti = 0.8;
    iMulti = 0.1;
    dMulti = 0.01;
    error = 0;
    prevError = 0;
    P = 0;
    I = 0;
    minPwm = 0.5;
    
    currentSpeed = 0;
    
    currentPWM = 0;
    stallCount = 0;
    prevStallCount = 0;
    stallWarningLimit = 60;
    stallErrorLimit = 300;
    stallLevel = 0;
    
    extIO->setPin(brakePinNumber);    
    extIO->setPin(coastPinNumber);
    extIO->clearPin(dirPinNumber);
    
    /*interruptDiro.rise(this, &A3930::diroRise);
    interruptDiro.fall(this, &A3930::diroFall);*/
    interruptTacho.rise(this, &A3930::tachoChanged);
    interruptTacho.fall(this, &A3930::tachoChanged);
    
    interruptDiro.attach_asserted(this, &A3930::diroRise);
    interruptDiro.attach_deasserted(this, &A3930::diroFall);
    /*interruptTacho.attach_asserted(this, &A3930::tachoChanged);
    interruptTacho.attach_deasserted(this, &A3930::tachoChanged);*/
    
    interruptDiro.setSamplesTillAssert(3);
    //interruptTacho.setSamplesTillAssert(2);
    
    interruptDiro.setSampleFrequency(500);
    //interruptTacho.setSampleFrequency(1000);
}

void A3930::setPWM(float newPWM) {
    currentPWM = newPWM;
    if (newPWM < -0.5) {
        pwm = -1 * newPWM;
        extIO->setPin(dirPinNumber);
        //extIO->setPin(coastPinNumber);
        extIO->setPin(brakePinNumber);
    } else if (newPWM > 0.5) {
        pwm = newPWM;
        extIO->clearPin(dirPinNumber);
        //extIO->setPin(coastPinNumber);
        extIO->setPin(brakePinNumber);
    } else {
        extIO->clearPin(brakePinNumber);
    }
}

void A3930::setRawPWM(float newPWM) {
    currentPWM = newPWM;
    pwm = newPWM;
}

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

int A3930::getDecoderCount() {
    currentSpeed = pulses;
    pulses = 0;
    return currentSpeed;
}

void A3930::diroRise() {
    diro = 1;
}

void A3930::diroFall() {
    diro = 0;
}

void A3930::tachoChanged() {
    if (diro) {
        pulses++;
    } else {
        pulses--;
    }
}

void A3930::setSpeed(int newSpeed) {
    setPoint = newSpeed;
    if (newSpeed == 0) {
        resetPID();
    }
}

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

    if (stallLevel != 2) {
        
        float err = (float)error / 100.0;
        float minPwmValue = minPwm;
        
        if (setPoint == 0) {
            minPwmValue = 0;
        } else if (setPoint < 0) {
            minPwmValue = -minPwm;
        }
        
        I += err * iMulti;
        //constrain integral
        if (I < -1) I = -1;
        if (I > 1) I = 1;
        
        float newPWMvalue = minPwmValue + err * pMulti + I;
    
        //constrain pwm
        if (newPWMvalue < -1) newPWMvalue = -1;
        if (newPWMvalue > 1) newPWMvalue = 1;    
        
        prevStallCount = stallCount;
        if ((currentSpeed < 5 && currentPWM == 1 || currentSpeed > -5 && currentPWM == -1) && 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 A3930::resetPID() {
    error = 0;
    prevError = 0;
    P = 0;
    I = 0;
    setPoint = 0;
    setPWM(0);
}

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

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

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

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

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