Reiko Randoja / Motor2

Motor

Fork of Motor by Reiko Randoja

motor.cpp@10:b0e6051a6109, 2013-11-03 (annotated)

Committer:
Reiko
Date:
Sun Nov 03 11:41:40 2013 +0000
Revision:
10:b0e6051a6109
Parent:
9:bec5a728405f 
Increased PI controller parameters

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reiko 1:c97f8bcd6c0f 1 #include "motor.h"
Reiko 1:c97f8bcd6c0f 2
Reiko 1:c97f8bcd6c0f 3 Motor::Motor(PinName PWMpin, PCA9555 *ioExt, unsigned int dir1Pin, unsigned int dir2Pin, PinName encA, PinName encB)
Reiko 1:c97f8bcd6c0f 4 : pwm(PWMpin), extIO(ioExt), dir1(dir1Pin), dir2(dir2Pin), qed(encA, encB) {
Reiko 1:c97f8bcd6c0f 5
Reiko 9:bec5a728405f 6 pwmPeriod = 2500;
Reiko 1:c97f8bcd6c0f 7 PwmOut pwm(PWMpin);
Reiko 9:bec5a728405f 8 pwm.period_us(pwmPeriod);
Reiko 1:c97f8bcd6c0f 9 setPoint = 0;
Reiko 10:b0e6051a6109 10 pMulti = 48;
Reiko 10:b0e6051a6109 11 iMulti = 2;
Reiko 2:3faf5dcde08f 12 dMulti = 1;
Reiko 3:94db629c0a83 13 error = 0;
Reiko 3:94db629c0a83 14 prevError = 0;
Reiko 3:94db629c0a83 15 P = 0;
Reiko 3:94db629c0a83 16 I = 0;
Reiko 3:94db629c0a83 17 minPwm = 100;
Reiko 10:b0e6051a6109 18 pidMulti = 256;
Reiko 9:bec5a728405f 19 iMax = 4096 * pidMulti;
Reiko 1:c97f8bcd6c0f 20
Reiko 1:c97f8bcd6c0f 21 currentSpeed = 0;
Reiko 5:99fa6dffea40 22
Reiko 5:99fa6dffea40 23 currentPWM = 0;
Reiko 8:63a67086a1b5 24 stallCount = 0;
Reiko 8:63a67086a1b5 25 prevStallCount = 0;
Reiko 8:63a67086a1b5 26 stallWarningLimit = 60;
Reiko 8:63a67086a1b5 27 stallErrorLimit = 300;
Reiko 8:63a67086a1b5 28 stallLevel = 0;
Reiko 1:c97f8bcd6c0f 29 }
Reiko 1:c97f8bcd6c0f 30
Reiko 3:94db629c0a83 31 void Motor::setPWM(int newPWM) {
Reiko 5:99fa6dffea40 32 currentPWM = newPWM;
Reiko 1:c97f8bcd6c0f 33 if (newPWM < 0) {
Reiko 3:94db629c0a83 34 pwm.pulsewidth_us(-1 * newPWM);
Reiko 5:99fa6dffea40 35 extIO->setPin(dir1);
Reiko 5:99fa6dffea40 36 extIO->clearPin(dir2);
Reiko 1:c97f8bcd6c0f 37 } else {
Reiko 3:94db629c0a83 38 pwm.pulsewidth_us(newPWM);
Reiko 5:99fa6dffea40 39 extIO->clearPin(dir1);
Reiko 5:99fa6dffea40 40 extIO->setPin(dir2);
Reiko 1:c97f8bcd6c0f 41 }
Reiko 1:c97f8bcd6c0f 42 }
Reiko 1:c97f8bcd6c0f 43
Reiko 1:c97f8bcd6c0f 44 int Motor::getSpeed() {
Reiko 1:c97f8bcd6c0f 45 return currentSpeed;
Reiko 1:c97f8bcd6c0f 46 }
Reiko 1:c97f8bcd6c0f 47
Reiko 1:c97f8bcd6c0f 48 int Motor::getDecoderCount() {
Reiko 1:c97f8bcd6c0f 49 currentSpeed = qed.read();
Reiko 1:c97f8bcd6c0f 50 return currentSpeed;
Reiko 1:c97f8bcd6c0f 51 }
Reiko 1:c97f8bcd6c0f 52
Reiko 1:c97f8bcd6c0f 53 void Motor::setSpeed(int newSpeed) {
Reiko 1:c97f8bcd6c0f 54 setPoint = newSpeed;
Reiko 2:3faf5dcde08f 55 if (newSpeed == 0) {
Reiko 2:3faf5dcde08f 56 resetPID();
Reiko 2:3faf5dcde08f 57 }
Reiko 1:c97f8bcd6c0f 58 }
Reiko 1:c97f8bcd6c0f 59
Reiko 8:63a67086a1b5 60 void Motor::pid() {
Reiko 1:c97f8bcd6c0f 61 prevError = error;
Reiko 3:94db629c0a83 62 error = setPoint - getDecoderCount();
Reiko 8:63a67086a1b5 63
Reiko 8:63a67086a1b5 64 if (stallLevel != 2) {
Reiko 8:63a67086a1b5 65
Reiko 8:63a67086a1b5 66 //float err = (float)error / 250.0;
Reiko 8:63a67086a1b5 67 float minPwmValue = minPwm;
Reiko 8:63a67086a1b5 68
Reiko 8:63a67086a1b5 69 if (setPoint == 0) {
Reiko 8:63a67086a1b5 70 minPwmValue = 0;
Reiko 8:63a67086a1b5 71 } else if (setPoint < 0) {
Reiko 8:63a67086a1b5 72 minPwmValue = -minPwm;
Reiko 8:63a67086a1b5 73 }
Reiko 8:63a67086a1b5 74
Reiko 10:b0e6051a6109 75 I += error * pidMulti * iMulti;
Reiko 8:63a67086a1b5 76 //constrain integral
Reiko 8:63a67086a1b5 77 if (I < -iMax) I = -iMax;
Reiko 8:63a67086a1b5 78 if (I > iMax) I = iMax;
Reiko 8:63a67086a1b5 79
Reiko 8:63a67086a1b5 80 //D = error - prevError;
Reiko 8:63a67086a1b5 81
Reiko 8:63a67086a1b5 82 //float newPWMvalue = minPwmValue + error * pMulti + I + dMulti * D;
Reiko 8:63a67086a1b5 83 int newPWMvalue = minPwmValue + error * pMulti + I / pidMulti;
Reiko 2:3faf5dcde08f 84
Reiko 8:63a67086a1b5 85 //constrain pwm
Reiko 9:bec5a728405f 86 if (newPWMvalue < -pwmPeriod) newPWMvalue = -pwmPeriod;
Reiko 9:bec5a728405f 87 if (newPWMvalue > pwmPeriod) newPWMvalue = pwmPeriod;
Reiko 8:63a67086a1b5 88
Reiko 8:63a67086a1b5 89 prevStallCount = stallCount;
Reiko 9:bec5a728405f 90 if ((currentSpeed < 5 && currentPWM == pwmPeriod || currentSpeed > -5 && currentPWM == -pwmPeriod) && stallCount < stallErrorLimit) {
Reiko 8:63a67086a1b5 91 stallCount++;
Reiko 8:63a67086a1b5 92 } else if (stallCount > 0) {
Reiko 8:63a67086a1b5 93 stallCount--;
Reiko 8:63a67086a1b5 94 }
Reiko 8:63a67086a1b5 95
Reiko 8:63a67086a1b5 96 setPWM(newPWMvalue);
Reiko 8:63a67086a1b5 97
Reiko 8:63a67086a1b5 98 if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
Reiko 8:63a67086a1b5 99 stallLevel = 0;
Reiko 8:63a67086a1b5 100 stallEndCallback.call();
Reiko 8:63a67086a1b5 101 stallChangeCallback.call();
Reiko 8:63a67086a1b5 102 } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
Reiko 8:63a67086a1b5 103 stallLevel = 1;
Reiko 8:63a67086a1b5 104 stallWarningCallback.call();
Reiko 8:63a67086a1b5 105 stallChangeCallback.call();
Reiko 8:63a67086a1b5 106 } else if (stallCount == stallErrorLimit) {
Reiko 8:63a67086a1b5 107 stallLevel = 2;
Reiko 8:63a67086a1b5 108 stallErrorCallback.call();
Reiko 8:63a67086a1b5 109 stallChangeCallback.call();
Reiko 8:63a67086a1b5 110 resetPID();
Reiko 8:63a67086a1b5 111 }
Reiko 8:63a67086a1b5 112 } else {
Reiko 8:63a67086a1b5 113 stallCount--;
Reiko 8:63a67086a1b5 114 if (stallCount == 0) {
Reiko 8:63a67086a1b5 115 stallLevel = 0;
Reiko 8:63a67086a1b5 116 stallEndCallback.call();
Reiko 8:63a67086a1b5 117 stallChangeCallback.call();
Reiko 8:63a67086a1b5 118 }
Reiko 5:99fa6dffea40 119 }
Reiko 2:3faf5dcde08f 120 }
Reiko 2:3faf5dcde08f 121
Reiko 2:3faf5dcde08f 122 void Motor::resetPID() {
Reiko 2:3faf5dcde08f 123 error = 0;
Reiko 2:3faf5dcde08f 124 prevError = 0;
Reiko 2:3faf5dcde08f 125 P = 0;
Reiko 2:3faf5dcde08f 126 I = 0;
Reiko 2:3faf5dcde08f 127 setPoint = 0;
Reiko 5:99fa6dffea40 128 setPWM(0);
Reiko 5:99fa6dffea40 129 }
Reiko 5:99fa6dffea40 130
Reiko 8:63a67086a1b5 131 int Motor::getStallLevel() {
Reiko 8:63a67086a1b5 132 return stallLevel;
Reiko 8:63a67086a1b5 133 }
Reiko 8:63a67086a1b5 134
Reiko 8:63a67086a1b5 135 void Motor::stallChange(void (*function)(void)) {
Reiko 8:63a67086a1b5 136 stallChangeCallback.attach(function);
Reiko 8:63a67086a1b5 137 }
Reiko 8:63a67086a1b5 138
Reiko 8:63a67086a1b5 139 void Motor::stallEnd(void (*function)(void)) {
Reiko 8:63a67086a1b5 140 stallEndCallback.attach(function);
Reiko 8:63a67086a1b5 141 }
Reiko 8:63a67086a1b5 142
Reiko 5:99fa6dffea40 143 void Motor::stallWarning(void (*function)(void)) {
Reiko 5:99fa6dffea40 144 stallWarningCallback.attach(function);
Reiko 5:99fa6dffea40 145 }
Reiko 5:99fa6dffea40 146
Reiko 5:99fa6dffea40 147 void Motor::stallError(void (*function)(void)) {
Reiko 5:99fa6dffea40 148 stallErrorCallback.attach(function);
Reiko 1:c97f8bcd6c0f 149 }