Reiko Randoja / A3930

Allegro A3930 BLDC driver

a3930.cpp@1:464cd2cb852e, 2013-11-10 (annotated)

Committer:
Reiko
Date:
Sun Nov 10 15:36:47 2013 +0000
Revision:
1:464cd2cb852e
Parent:
0:a6123d12f08b 
Added debounce to DIRO

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reiko 0:a6123d12f08b 1 #include "a3930.h"
Reiko 0:a6123d12f08b 2
Reiko 0:a6123d12f08b 3 A3930::A3930(PinName PWMpin, PCA9555 *ioExt, unsigned int dirPin, unsigned int brakePin, unsigned int coastPin, PinName tachoPin, PinName diroPin)
Reiko 0:a6123d12f08b 4 : pwm(PWMpin), extIO(ioExt), dirPinNumber(dirPin), brakePinNumber(brakePin), coastPinNumber(coastPin), interruptTacho(tachoPin), interruptDiro(diroPin) {
Reiko 0:a6123d12f08b 5
Reiko 0:a6123d12f08b 6 pwmPeriod = 1.0f / 50000.0f;
Reiko 0:a6123d12f08b 7 PwmOut pwm(PWMpin);
Reiko 0:a6123d12f08b 8 pwm.period(pwmPeriod);
Reiko 0:a6123d12f08b 9 setPoint = 0;
Reiko 0:a6123d12f08b 10 pMulti = 0.8;
Reiko 0:a6123d12f08b 11 iMulti = 0.1;
Reiko 0:a6123d12f08b 12 dMulti = 0.01;
Reiko 0:a6123d12f08b 13 error = 0;
Reiko 0:a6123d12f08b 14 prevError = 0;
Reiko 0:a6123d12f08b 15 P = 0;
Reiko 0:a6123d12f08b 16 I = 0;
Reiko 0:a6123d12f08b 17 minPwm = 0.5;
Reiko 0:a6123d12f08b 18
Reiko 0:a6123d12f08b 19 currentSpeed = 0;
Reiko 0:a6123d12f08b 20
Reiko 0:a6123d12f08b 21 currentPWM = 0;
Reiko 0:a6123d12f08b 22 stallCount = 0;
Reiko 0:a6123d12f08b 23 prevStallCount = 0;
Reiko 0:a6123d12f08b 24 stallWarningLimit = 60;
Reiko 0:a6123d12f08b 25 stallErrorLimit = 300;
Reiko 0:a6123d12f08b 26 stallLevel = 0;
Reiko 0:a6123d12f08b 27
Reiko 0:a6123d12f08b 28 extIO->setPin(brakePinNumber);
Reiko 0:a6123d12f08b 29 extIO->setPin(coastPinNumber);
Reiko 0:a6123d12f08b 30 extIO->clearPin(dirPinNumber);
Reiko 0:a6123d12f08b 31
Reiko 1:464cd2cb852e 32 /*interruptDiro.rise(this, &A3930::diroRise);
Reiko 1:464cd2cb852e 33 interruptDiro.fall(this, &A3930::diroFall);*/
Reiko 0:a6123d12f08b 34 interruptTacho.rise(this, &A3930::tachoChanged);
Reiko 0:a6123d12f08b 35 interruptTacho.fall(this, &A3930::tachoChanged);
Reiko 1:464cd2cb852e 36
Reiko 1:464cd2cb852e 37 interruptDiro.attach_asserted(this, &A3930::diroRise);
Reiko 1:464cd2cb852e 38 interruptDiro.attach_deasserted(this, &A3930::diroFall);
Reiko 1:464cd2cb852e 39 /*interruptTacho.attach_asserted(this, &A3930::tachoChanged);
Reiko 1:464cd2cb852e 40 interruptTacho.attach_deasserted(this, &A3930::tachoChanged);*/
Reiko 1:464cd2cb852e 41
Reiko 1:464cd2cb852e 42 interruptDiro.setSamplesTillAssert(3);
Reiko 1:464cd2cb852e 43 //interruptTacho.setSamplesTillAssert(2);
Reiko 1:464cd2cb852e 44
Reiko 1:464cd2cb852e 45 interruptDiro.setSampleFrequency(500);
Reiko 1:464cd2cb852e 46 //interruptTacho.setSampleFrequency(1000);
Reiko 0:a6123d12f08b 47 }
Reiko 0:a6123d12f08b 48
Reiko 0:a6123d12f08b 49 void A3930::setPWM(float newPWM) {
Reiko 0:a6123d12f08b 50 currentPWM = newPWM;
Reiko 0:a6123d12f08b 51 if (newPWM < -0.5) {
Reiko 0:a6123d12f08b 52 pwm = -1 * newPWM;
Reiko 0:a6123d12f08b 53 extIO->setPin(dirPinNumber);
Reiko 1:464cd2cb852e 54 //extIO->setPin(coastPinNumber);
Reiko 1:464cd2cb852e 55 extIO->setPin(brakePinNumber);
Reiko 0:a6123d12f08b 56 } else if (newPWM > 0.5) {
Reiko 0:a6123d12f08b 57 pwm = newPWM;
Reiko 0:a6123d12f08b 58 extIO->clearPin(dirPinNumber);
Reiko 1:464cd2cb852e 59 //extIO->setPin(coastPinNumber);
Reiko 1:464cd2cb852e 60 extIO->setPin(brakePinNumber);
Reiko 0:a6123d12f08b 61 } else {
Reiko 1:464cd2cb852e 62 extIO->clearPin(brakePinNumber);
Reiko 0:a6123d12f08b 63 }
Reiko 0:a6123d12f08b 64 }
Reiko 0:a6123d12f08b 65
Reiko 1:464cd2cb852e 66 void A3930::setRawPWM(float newPWM) {
Reiko 1:464cd2cb852e 67 currentPWM = newPWM;
Reiko 1:464cd2cb852e 68 pwm = newPWM;
Reiko 1:464cd2cb852e 69 }
Reiko 1:464cd2cb852e 70
Reiko 0:a6123d12f08b 71 int A3930::getSpeed() {
Reiko 0:a6123d12f08b 72 return currentSpeed;
Reiko 0:a6123d12f08b 73 }
Reiko 0:a6123d12f08b 74
Reiko 0:a6123d12f08b 75 int A3930::getDecoderCount() {
Reiko 0:a6123d12f08b 76 currentSpeed = pulses;
Reiko 0:a6123d12f08b 77 pulses = 0;
Reiko 0:a6123d12f08b 78 return currentSpeed;
Reiko 0:a6123d12f08b 79 }
Reiko 0:a6123d12f08b 80
Reiko 0:a6123d12f08b 81 void A3930::diroRise() {
Reiko 0:a6123d12f08b 82 diro = 1;
Reiko 0:a6123d12f08b 83 }
Reiko 0:a6123d12f08b 84
Reiko 0:a6123d12f08b 85 void A3930::diroFall() {
Reiko 0:a6123d12f08b 86 diro = 0;
Reiko 0:a6123d12f08b 87 }
Reiko 0:a6123d12f08b 88
Reiko 0:a6123d12f08b 89 void A3930::tachoChanged() {
Reiko 0:a6123d12f08b 90 if (diro) {
Reiko 0:a6123d12f08b 91 pulses++;
Reiko 0:a6123d12f08b 92 } else {
Reiko 0:a6123d12f08b 93 pulses--;
Reiko 0:a6123d12f08b 94 }
Reiko 0:a6123d12f08b 95 }
Reiko 0:a6123d12f08b 96
Reiko 0:a6123d12f08b 97 void A3930::setSpeed(int newSpeed) {
Reiko 0:a6123d12f08b 98 setPoint = newSpeed;
Reiko 0:a6123d12f08b 99 if (newSpeed == 0) {
Reiko 0:a6123d12f08b 100 resetPID();
Reiko 0:a6123d12f08b 101 }
Reiko 0:a6123d12f08b 102 }
Reiko 0:a6123d12f08b 103
Reiko 0:a6123d12f08b 104 void A3930::pid() {
Reiko 0:a6123d12f08b 105 prevError = error;
Reiko 0:a6123d12f08b 106 error = setPoint - getDecoderCount();
Reiko 0:a6123d12f08b 107
Reiko 0:a6123d12f08b 108 if (stallLevel != 2) {
Reiko 0:a6123d12f08b 109
Reiko 0:a6123d12f08b 110 float err = (float)error / 100.0;
Reiko 0:a6123d12f08b 111 float minPwmValue = minPwm;
Reiko 0:a6123d12f08b 112
Reiko 0:a6123d12f08b 113 if (setPoint == 0) {
Reiko 0:a6123d12f08b 114 minPwmValue = 0;
Reiko 0:a6123d12f08b 115 } else if (setPoint < 0) {
Reiko 0:a6123d12f08b 116 minPwmValue = -minPwm;
Reiko 0:a6123d12f08b 117 }
Reiko 0:a6123d12f08b 118
Reiko 0:a6123d12f08b 119 I += err * iMulti;
Reiko 0:a6123d12f08b 120 //constrain integral
Reiko 0:a6123d12f08b 121 if (I < -1) I = -1;
Reiko 0:a6123d12f08b 122 if (I > 1) I = 1;
Reiko 0:a6123d12f08b 123
Reiko 0:a6123d12f08b 124 float newPWMvalue = minPwmValue + err * pMulti + I;
Reiko 0:a6123d12f08b 125
Reiko 0:a6123d12f08b 126 //constrain pwm
Reiko 0:a6123d12f08b 127 if (newPWMvalue < -1) newPWMvalue = -1;
Reiko 0:a6123d12f08b 128 if (newPWMvalue > 1) newPWMvalue = 1;
Reiko 0:a6123d12f08b 129
Reiko 0:a6123d12f08b 130 prevStallCount = stallCount;
Reiko 0:a6123d12f08b 131 if ((currentSpeed < 5 && currentPWM == 1 || currentSpeed > -5 && currentPWM == -1) && stallCount < stallErrorLimit) {
Reiko 0:a6123d12f08b 132 stallCount++;
Reiko 0:a6123d12f08b 133 } else if (stallCount > 0) {
Reiko 0:a6123d12f08b 134 stallCount--;
Reiko 0:a6123d12f08b 135 }
Reiko 0:a6123d12f08b 136
Reiko 0:a6123d12f08b 137 setPWM(newPWMvalue);
Reiko 0:a6123d12f08b 138
Reiko 0:a6123d12f08b 139 if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
Reiko 0:a6123d12f08b 140 stallLevel = 0;
Reiko 0:a6123d12f08b 141 stallEndCallback.call();
Reiko 0:a6123d12f08b 142 stallChangeCallback.call();
Reiko 0:a6123d12f08b 143 } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
Reiko 0:a6123d12f08b 144 stallLevel = 1;
Reiko 0:a6123d12f08b 145 stallWarningCallback.call();
Reiko 0:a6123d12f08b 146 stallChangeCallback.call();
Reiko 0:a6123d12f08b 147 } else if (stallCount == stallErrorLimit) {
Reiko 0:a6123d12f08b 148 stallLevel = 2;
Reiko 0:a6123d12f08b 149 stallErrorCallback.call();
Reiko 0:a6123d12f08b 150 stallChangeCallback.call();
Reiko 0:a6123d12f08b 151 resetPID();
Reiko 0:a6123d12f08b 152 }
Reiko 0:a6123d12f08b 153 } else {
Reiko 0:a6123d12f08b 154 stallCount--;
Reiko 0:a6123d12f08b 155 if (stallCount == 0) {
Reiko 0:a6123d12f08b 156 stallLevel = 0;
Reiko 0:a6123d12f08b 157 stallEndCallback.call();
Reiko 0:a6123d12f08b 158 stallChangeCallback.call();
Reiko 0:a6123d12f08b 159 }
Reiko 0:a6123d12f08b 160 }
Reiko 0:a6123d12f08b 161 }
Reiko 0:a6123d12f08b 162
Reiko 0:a6123d12f08b 163 void A3930::resetPID() {
Reiko 0:a6123d12f08b 164 error = 0;
Reiko 0:a6123d12f08b 165 prevError = 0;
Reiko 0:a6123d12f08b 166 P = 0;
Reiko 0:a6123d12f08b 167 I = 0;
Reiko 0:a6123d12f08b 168 setPoint = 0;
Reiko 0:a6123d12f08b 169 setPWM(0);
Reiko 0:a6123d12f08b 170 }
Reiko 0:a6123d12f08b 171
Reiko 0:a6123d12f08b 172 int A3930::getStallLevel() {
Reiko 0:a6123d12f08b 173 return stallLevel;
Reiko 0:a6123d12f08b 174 }
Reiko 0:a6123d12f08b 175
Reiko 0:a6123d12f08b 176 void A3930::stallChange(void (*function)(void)) {
Reiko 0:a6123d12f08b 177 stallChangeCallback.attach(function);
Reiko 0:a6123d12f08b 178 }
Reiko 0:a6123d12f08b 179
Reiko 0:a6123d12f08b 180 void A3930::stallEnd(void (*function)(void)) {
Reiko 0:a6123d12f08b 181 stallEndCallback.attach(function);
Reiko 0:a6123d12f08b 182 }
Reiko 0:a6123d12f08b 183
Reiko 0:a6123d12f08b 184 void A3930::stallWarning(void (*function)(void)) {
Reiko 0:a6123d12f08b 185 stallWarningCallback.attach(function);
Reiko 0:a6123d12f08b 186 }
Reiko 0:a6123d12f08b 187
Reiko 0:a6123d12f08b 188 void A3930::stallError(void (*function)(void)) {
Reiko 0:a6123d12f08b 189 stallErrorCallback.attach(function);
Reiko 0:a6123d12f08b 190 }