Reiko Randoja / Motor2

Motor

Fork of Motor by Reiko Randoja

motor.cpp@8:63a67086a1b5, 2013-09-19 (annotated)

Committer:
Reiko
Date:
Thu Sep 19 13:08:56 2013 +0000
Revision:
8:63a67086a1b5
Parent:
5:99fa6dffea40
Child:
9:bec5a728405f
Added stall detection

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