motor.cpp

00001 #include "motor.h"
00002 
00003 Motor::Motor(PinName PWMpin, PinName dir1Pin, PinName dir2Pin, PinName encA, PinName encB)
00004     : pwm(PWMpin), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) {
00005  
00006     pwmPeriod = 4000;
00007     PwmOut pwm(PWMpin);
00008     pwm.period_us(pwmPeriod);
00009     setPoint = 0;
00010     pMulti = 64;
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         dir1 = 0;
00036         dir2 = 1;
00037     } else {
00038         pwm.pulsewidth_us(newPWM);
00039         dir1 = 1;
00040         dir2 = 0;
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     
00056     if (newSpeed == 0) {
00057         resetPID();
00058     }
00059 }
00060 
00061 void Motor::pid() {
00062     prevError = error;
00063     error = setPoint - getDecoderCount();
00064 
00065     if (stallLevel != 2) {        
00066         float minPwmValue = minPwm;
00067         
00068         if (setPoint == 0) {
00069             minPwmValue = 0;
00070         } else if (setPoint < 0) {
00071             minPwmValue = -minPwm;
00072         }
00073         
00074         I += error * pidMulti * iMulti;
00075         //constrain integral
00076         if (I < -iMax) I = -iMax;
00077         if (I > iMax) I = iMax;
00078         
00079         //D = error - prevError;
00080         
00081         int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
00082     
00083         //constrain pwm
00084         if (newPWMvalue < -pwmPeriod) newPWMvalue = -pwmPeriod;
00085         if (newPWMvalue > pwmPeriod) newPWMvalue = pwmPeriod;    
00086         
00087         prevStallCount = stallCount;
00088         if ((currentSpeed < 5 && currentPWM == pwmPeriod || currentSpeed > -5 && currentPWM == -pwmPeriod) && stallCount < stallErrorLimit) {
00089             stallCount++;
00090         } else if (stallCount > 0) {
00091             stallCount--;
00092         }
00093         
00094         setPWM(newPWMvalue);
00095         
00096         if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
00097             stallLevel = 0;
00098             stallEndCallback.call();
00099             stallChangeCallback.call();
00100         } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
00101             stallLevel = 1;
00102             stallWarningCallback.call();
00103             stallChangeCallback.call();
00104         } else if (stallCount == stallErrorLimit) {
00105             stallLevel = 2;
00106             stallErrorCallback.call();
00107             stallChangeCallback.call();
00108             resetPID();
00109         }
00110     } else {
00111         stallCount--;
00112         if (stallCount == 0) {
00113             stallLevel = 0;
00114             stallEndCallback.call();
00115             stallChangeCallback.call();
00116         }
00117     }
00118 }
00119 
00120 void Motor::resetPID() {
00121     error = 0;
00122     prevError = 0;
00123     P = 0;
00124     I = 0;
00125     setPoint = 0;
00126     setPWM(0);
00127 }
00128 
00129 int Motor::getStallLevel() {
00130     return stallLevel;
00131 }
00132 
00133 void Motor::stallChange(void (*function)(void)) { 
00134     stallChangeCallback.attach(function);
00135 }
00136 
00137 void Motor::stallEnd(void (*function)(void)) { 
00138     stallEndCallback.attach(function);
00139 }
00140 
00141 void Motor::stallWarning(void (*function)(void)) { 
00142     stallWarningCallback.attach(function);
00143 }
00144 
00145 void Motor::stallError(void (*function)(void)) { 
00146     stallErrorCallback.attach(function);
00147 }