Diff: a3930.cpp

Revision:

0:a6123d12f08b

Child:

1:464cd2cb852e

--- /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