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     PwmOut pwm(PWMpin);
00007     pwm.period_ms(1);
00008     setPoint = 0;
00009     pMulti = 16;
00010     iDiv = 1;
00011     dMulti = 1;
00012     error = 0;
00013     prevError = 0;
00014     P = 0;
00015     I = 0;
00016     minPwm = 100;
00017     pidMulti = 128;
00018     iMax = 512 * pidMulti;
00019     
00020     currentSpeed = 0;
00021     
00022     currentPWM = 0;
00023     stallCounter = 0;
00024     stallCounterLimit = 60;
00025 }
00026 
00027 void Motor::setPWM(int newPWM) {
00028     currentPWM = newPWM;
00029     if (newPWM < 0) {
00030         pwm.pulsewidth_us(-1 * newPWM);
00031         extIO->setPin(dir1);
00032         extIO->clearPin(dir2);
00033     } else {
00034         pwm.pulsewidth_us(newPWM);
00035         extIO->clearPin(dir1);
00036         extIO->setPin(dir2);
00037     }    
00038 }
00039 
00040 int Motor::getSpeed() {
00041     return currentSpeed;
00042 }
00043 
00044 int Motor::getDecoderCount() {
00045     currentSpeed = qed.read();
00046     return currentSpeed;
00047 }
00048 
00049 void Motor::setSpeed(int newSpeed) {
00050     setPoint = newSpeed;
00051     if (newSpeed == 0) {
00052         resetPID();
00053     }
00054 }
00055 
00056 void Motor::pid() {    
00057     prevError = error;
00058     error = setPoint - getDecoderCount();
00059     
00060     //float err = (float)error / 250.0;
00061     float minPwmValue = minPwm;
00062     
00063     if (setPoint == 0) {
00064         minPwmValue = 0;
00065     } else if (setPoint < 0) {
00066         minPwmValue = -minPwm;
00067     }
00068     
00069     I += error * pidMulti / iDiv;
00070     //constrain integral
00071     if (I < -iMax) I = -iMax;
00072     if (I > iMax) I = iMax;
00073     
00074     //D = error - prevError;
00075     
00076     //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
00077     int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
00078 
00079     //constrain pwm
00080     if (newPWMvalue < -1000) newPWMvalue = -1000;
00081     if (newPWMvalue > 1000) newPWMvalue = 1000;
00082     
00083     /*if ((currentSpeed < 5 && currentPWM == 1000 || currentSpeed > -5 && currentPWM == -1000) && stallCounter < stallCounterLimit) {
00084         stallCounter++;
00085     } else if (stallCounter > 0) {
00086         stallCounter++;
00087     }
00088     
00089     if (stallCounter == stallCounterLimit) {
00090         
00091     }*/
00092     
00093     setPWM(newPWMvalue);
00094 }
00095 
00096 void Motor::resetPID() {
00097     error = 0;
00098     prevError = 0;
00099     P = 0;
00100     I = 0;
00101     setPoint = 0;
00102     setPWM(0);
00103 }
00104 
00105 void Motor::stallWarning(void (*function)(void)) { 
00106     stallWarningCallback.attach(function);
00107 }
00108 
00109 void Motor::stallError(void (*function)(void)) { 
00110     stallErrorCallback.attach(function);
00111 }