Allegro A3930 BLDC driver
Diff: a3930.cpp
- Revision:
- 0:a6123d12f08b
- Child:
- 1:464cd2cb852e
diff -r 000000000000 -r a6123d12f08b a3930.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/a3930.cpp Mon Nov 04 17:04:34 2013 +0000 @@ -0,0 +1,172 @@ +#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); +} + +void A3930::setPWM(float newPWM) { + currentPWM = newPWM; + if (newPWM < -0.5) { + pwm = -1 * newPWM; + extIO->setPin(dirPinNumber); + extIO->setPin(coastPinNumber); + } else if (newPWM > 0.5) { + pwm = newPWM; + extIO->clearPin(dirPinNumber); + extIO->setPin(coastPinNumber); + } else { + extIO->clearPin(coastPinNumber); + } +} + +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); +} \ No newline at end of file