motor.cpp

00001 #include "motor.h"
00002 
00003 Motor::Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dir1Pin, unsigned int dir2Pin, PinName encA, PinName encB)
00004     : pwm(PWMpin), extIO(ioExt), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) {
00005  
00006     pwmPeriod = 2500;
00007     PwmOut pwm(PWMpin);
00008     pwm.period_us(pwmPeriod);
00009     setPoint = 0;
00010     pMulti = 48;
00011     iMulti = 2;
00012     dMulti = 1;
00013     error = 0;
00014     prevError = 0;
00015     P = 0;
00016     I = 0;
00017     minPwm = 100;
00018     pidMulti = 256;
00019     iMax = 4096 * pidMulti;
00020     
00021     currentSpeed = 0;
00022     
00023     currentPWM = 0;
00024     stallCount = 0;
00025     prevStallCount = 0;
00026     stallWarningLimit = 60;
00027     stallErrorLimit = 300;
00028     stallLevel = 0;
00029 }
00030 
00031 void Motor::setPWM(int newPWM) {
00032     currentPWM = newPWM;
00033     if (newPWM < 0) {
00034         pwm.pulsewidth_us(-1 * newPWM);
00035         extIO->setPin(dir1);
00036         extIO->clearPin(dir2);
00037     } else {
00038         pwm.pulsewidth_us(newPWM);
00039         extIO->clearPin(dir1);
00040         extIO->setPin(dir2);
00041     }    
00042 }
00043 
00044 int Motor::getSpeed() {
00045     return currentSpeed;
00046 }
00047 
00048 int Motor::getDecoderCount() {
00049     currentSpeed = qed.read();
00050     return currentSpeed;
00051 }
00052 
00053 void Motor::setSpeed(int newSpeed) {
00054     setPoint = newSpeed;
00055     if (newSpeed == 0) {
00056         resetPID();
00057     }
00058 }
00059 
00060 void Motor::pid() {
00061     prevError = error;
00062     error = setPoint - getDecoderCount();
00063 
00064     if (stallLevel != 2) {
00065         
00066         //float err = (float)error / 250.0;
00067         float minPwmValue = minPwm;
00068         
00069         if (setPoint == 0) {
00070             minPwmValue = 0;
00071         } else if (setPoint < 0) {
00072             minPwmValue = -minPwm;
00073         }
00074         
00075         I += error * pidMulti * iMulti;
00076         //constrain integral
00077         if (I < -iMax) I = -iMax;
00078         if (I > iMax) I = iMax;
00079         
00080         //D = error - prevError;
00081         
00082         //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
00083         int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
00084     
00085         //constrain pwm
00086         if (newPWMvalue < -pwmPeriod) newPWMvalue = -pwmPeriod;
00087         if (newPWMvalue > pwmPeriod) newPWMvalue = pwmPeriod;    
00088         
00089         prevStallCount = stallCount;
00090         if ((currentSpeed < 5 && currentPWM == pwmPeriod || currentSpeed > -5 && currentPWM == -pwmPeriod) && stallCount < stallErrorLimit) {
00091             stallCount++;
00092         } else if (stallCount > 0) {
00093             stallCount--;
00094         }
00095         
00096         setPWM(newPWMvalue);
00097         
00098         if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
00099             stallLevel = 0;
00100             stallEndCallback.call();
00101             stallChangeCallback.call();
00102         } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
00103             stallLevel = 1;
00104             stallWarningCallback.call();
00105             stallChangeCallback.call();
00106         } else if (stallCount == stallErrorLimit) {
00107             stallLevel = 2;
00108             stallErrorCallback.call();
00109             stallChangeCallback.call();
00110             resetPID();
00111         }
00112     } else {
00113         stallCount--;
00114         if (stallCount == 0) {
00115             stallLevel = 0;
00116             stallEndCallback.call();
00117             stallChangeCallback.call();
00118         }
00119     }
00120 }
00121 
00122 void Motor::resetPID() {
00123     error = 0;
00124     prevError = 0;
00125     P = 0;
00126     I = 0;
00127     setPoint = 0;
00128     setPWM(0);
00129 }
00130 
00131 int Motor::getStallLevel() {
00132     return stallLevel;
00133 }
00134 
00135 void Motor::stallChange(void (*function)(void)) { 
00136     stallChangeCallback.attach(function);
00137 }
00138 
00139 void Motor::stallEnd(void (*function)(void)) { 
00140     stallEndCallback.attach(function);
00141 }
00142 
00143 void Motor::stallWarning(void (*function)(void)) { 
00144     stallWarningCallback.attach(function);
00145 }
00146 
00147 void Motor::stallError(void (*function)(void)) { 
00148     stallErrorCallback.attach(function);
00149 }