a3930.cpp

00001 #include "a3930.h"
00002 
00003 A3930::A3930(PinName PWMpin, PCA9555 *ioExt, unsigned int dirPin, unsigned int brakePin, unsigned int coastPin, PinName tachoPin, PinName diroPin)
00004     : pwm(PWMpin), extIO(ioExt), dirPinNumber(dirPin), brakePinNumber(brakePin), coastPinNumber(coastPin), interruptTacho(tachoPin), interruptDiro(diroPin) {
00005  
00006     pwmPeriod = 1.0f / 50000.0f;
00007     PwmOut pwm(PWMpin);
00008     pwm.period(pwmPeriod);
00009     setPoint = 0;
00010     pMulti = 0.8;
00011     iMulti = 0.1;
00012     dMulti = 0.01;
00013     error = 0;
00014     prevError = 0;
00015     P = 0;
00016     I = 0;
00017     minPwm = 0.5;
00018     
00019     currentSpeed = 0;
00020     
00021     currentPWM = 0;
00022     stallCount = 0;
00023     prevStallCount = 0;
00024     stallWarningLimit = 60;
00025     stallErrorLimit = 300;
00026     stallLevel = 0;
00027     
00028     extIO->setPin(brakePinNumber);    
00029     extIO->setPin(coastPinNumber);
00030     extIO->clearPin(dirPinNumber);
00031     
00032     /*interruptDiro.rise(this, &A3930::diroRise);
00033     interruptDiro.fall(this, &A3930::diroFall);*/
00034     interruptTacho.rise(this, &A3930::tachoChanged);
00035     interruptTacho.fall(this, &A3930::tachoChanged);
00036     
00037     interruptDiro.attach_asserted(this, &A3930::diroRise);
00038     interruptDiro.attach_deasserted(this, &A3930::diroFall);
00039     /*interruptTacho.attach_asserted(this, &A3930::tachoChanged);
00040     interruptTacho.attach_deasserted(this, &A3930::tachoChanged);*/
00041     
00042     interruptDiro.setSamplesTillAssert(3);
00043     //interruptTacho.setSamplesTillAssert(2);
00044     
00045     interruptDiro.setSampleFrequency(500);
00046     //interruptTacho.setSampleFrequency(1000);
00047 }
00048 
00049 void A3930::setPWM(float newPWM) {
00050     currentPWM = newPWM;
00051     if (newPWM < -0.5) {
00052         pwm = -1 * newPWM;
00053         extIO->setPin(dirPinNumber);
00054         //extIO->setPin(coastPinNumber);
00055         extIO->setPin(brakePinNumber);
00056     } else if (newPWM > 0.5) {
00057         pwm = newPWM;
00058         extIO->clearPin(dirPinNumber);
00059         //extIO->setPin(coastPinNumber);
00060         extIO->setPin(brakePinNumber);
00061     } else {
00062         extIO->clearPin(brakePinNumber);
00063     }
00064 }
00065 
00066 void A3930::setRawPWM(float newPWM) {
00067     currentPWM = newPWM;
00068     pwm = newPWM;
00069 }
00070 
00071 int A3930::getSpeed() {
00072     return currentSpeed;
00073 }
00074 
00075 int A3930::getDecoderCount() {
00076     currentSpeed = pulses;
00077     pulses = 0;
00078     return currentSpeed;
00079 }
00080 
00081 void A3930::diroRise() {
00082     diro = 1;
00083 }
00084 
00085 void A3930::diroFall() {
00086     diro = 0;
00087 }
00088 
00089 void A3930::tachoChanged() {
00090     if (diro) {
00091         pulses++;
00092     } else {
00093         pulses--;
00094     }
00095 }
00096 
00097 void A3930::setSpeed(int newSpeed) {
00098     setPoint = newSpeed;
00099     if (newSpeed == 0) {
00100         resetPID();
00101     }
00102 }
00103 
00104 void A3930::pid() {
00105     prevError = error;
00106     error = setPoint - getDecoderCount();
00107 
00108     if (stallLevel != 2) {
00109         
00110         float err = (float)error / 100.0;
00111         float minPwmValue = minPwm;
00112         
00113         if (setPoint == 0) {
00114             minPwmValue = 0;
00115         } else if (setPoint < 0) {
00116             minPwmValue = -minPwm;
00117         }
00118         
00119         I += err * iMulti;
00120         //constrain integral
00121         if (I < -1) I = -1;
00122         if (I > 1) I = 1;
00123         
00124         float newPWMvalue = minPwmValue + err * pMulti + I;
00125     
00126         //constrain pwm
00127         if (newPWMvalue < -1) newPWMvalue = -1;
00128         if (newPWMvalue > 1) newPWMvalue = 1;    
00129         
00130         prevStallCount = stallCount;
00131         if ((currentSpeed < 5 && currentPWM == 1 || currentSpeed > -5 && currentPWM == -1) && stallCount < stallErrorLimit) {
00132             stallCount++;
00133         } else if (stallCount > 0) {
00134             stallCount--;
00135         }
00136         
00137         setPWM(newPWMvalue);
00138         
00139         if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
00140             stallLevel = 0;
00141             stallEndCallback.call();
00142             stallChangeCallback.call();
00143         } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
00144             stallLevel = 1;
00145             stallWarningCallback.call();
00146             stallChangeCallback.call();
00147         } else if (stallCount == stallErrorLimit) {
00148             stallLevel = 2;
00149             stallErrorCallback.call();
00150             stallChangeCallback.call();
00151             resetPID();
00152         }
00153     } else {
00154         stallCount--;
00155         if (stallCount == 0) {
00156             stallLevel = 0;
00157             stallEndCallback.call();
00158             stallChangeCallback.call();
00159         }
00160     }
00161 }
00162 
00163 void A3930::resetPID() {
00164     error = 0;
00165     prevError = 0;
00166     P = 0;
00167     I = 0;
00168     setPoint = 0;
00169     setPWM(0);
00170 }
00171 
00172 int A3930::getStallLevel() {
00173     return stallLevel;
00174 }
00175 
00176 void A3930::stallChange(void (*function)(void)) { 
00177     stallChangeCallback.attach(function);
00178 }
00179 
00180 void A3930::stallEnd(void (*function)(void)) { 
00181     stallEndCallback.attach(function);
00182 }
00183 
00184 void A3930::stallWarning(void (*function)(void)) { 
00185     stallWarningCallback.attach(function);
00186 }
00187 
00188 void A3930::stallError(void (*function)(void)) { 
00189     stallErrorCallback.attach(function);
00190 }