Reiko Randoja / Motor1Pwm1Dir

Library for driving a motor with one PWM and one DIR signal using PI control.

Fork of Motor2 by Reiko Randoja

motor.cpp@12:1d3fb22bbdf9, 2014-11-21 (annotated)

Committer:
Reiko
Date:
Fri Nov 21 18:22:54 2014 +0000
Revision:
12:1d3fb22bbdf9
Parent:
11:87a73d40ce5b 
Shorter stall detection times

Who changed what in which revision?

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