Reiko Randoja / Motor1Pwm1Dir

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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 }