Reiko Randoja / A3930

Allegro A3930 BLDC driver

a3930.cpp@0:a6123d12f08b, 2013-11-04 (annotated)

Committer:
Reiko
Date:
Mon Nov 04 17:04:34 2013 +0000
Revision:
0:a6123d12f08b
Child:
1:464cd2cb852e
Initial functionality

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 0:a6123d12f08b 32 interruptDiro.rise(this, &A3930::diroRise);
Reiko 0:a6123d12f08b 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 0:a6123d12f08b 36 }
Reiko 0:a6123d12f08b 37
Reiko 0:a6123d12f08b 38 void A3930::setPWM(float newPWM) {
Reiko 0:a6123d12f08b 39 currentPWM = newPWM;
Reiko 0:a6123d12f08b 40 if (newPWM < -0.5) {
Reiko 0:a6123d12f08b 41 pwm = -1 * newPWM;
Reiko 0:a6123d12f08b 42 extIO->setPin(dirPinNumber);
Reiko 0:a6123d12f08b 43 extIO->setPin(coastPinNumber);
Reiko 0:a6123d12f08b 44 } else if (newPWM > 0.5) {
Reiko 0:a6123d12f08b 45 pwm = newPWM;
Reiko 0:a6123d12f08b 46 extIO->clearPin(dirPinNumber);
Reiko 0:a6123d12f08b 47 extIO->setPin(coastPinNumber);
Reiko 0:a6123d12f08b 48 } else {
Reiko 0:a6123d12f08b 49 extIO->clearPin(coastPinNumber);
Reiko 0:a6123d12f08b 50 }
Reiko 0:a6123d12f08b 51 }
Reiko 0:a6123d12f08b 52
Reiko 0:a6123d12f08b 53 int A3930::getSpeed() {
Reiko 0:a6123d12f08b 54 return currentSpeed;
Reiko 0:a6123d12f08b 55 }
Reiko 0:a6123d12f08b 56
Reiko 0:a6123d12f08b 57 int A3930::getDecoderCount() {
Reiko 0:a6123d12f08b 58 currentSpeed = pulses;
Reiko 0:a6123d12f08b 59 pulses = 0;
Reiko 0:a6123d12f08b 60 return currentSpeed;
Reiko 0:a6123d12f08b 61 }
Reiko 0:a6123d12f08b 62
Reiko 0:a6123d12f08b 63 void A3930::diroRise() {
Reiko 0:a6123d12f08b 64 diro = 1;
Reiko 0:a6123d12f08b 65 }
Reiko 0:a6123d12f08b 66
Reiko 0:a6123d12f08b 67 void A3930::diroFall() {
Reiko 0:a6123d12f08b 68 diro = 0;
Reiko 0:a6123d12f08b 69 }
Reiko 0:a6123d12f08b 70
Reiko 0:a6123d12f08b 71 void A3930::tachoChanged() {
Reiko 0:a6123d12f08b 72 if (diro) {
Reiko 0:a6123d12f08b 73 pulses++;
Reiko 0:a6123d12f08b 74 } else {
Reiko 0:a6123d12f08b 75 pulses--;
Reiko 0:a6123d12f08b 76 }
Reiko 0:a6123d12f08b 77 }
Reiko 0:a6123d12f08b 78
Reiko 0:a6123d12f08b 79 void A3930::setSpeed(int newSpeed) {
Reiko 0:a6123d12f08b 80 setPoint = newSpeed;
Reiko 0:a6123d12f08b 81 if (newSpeed == 0) {
Reiko 0:a6123d12f08b 82 resetPID();
Reiko 0:a6123d12f08b 83 }
Reiko 0:a6123d12f08b 84 }
Reiko 0:a6123d12f08b 85
Reiko 0:a6123d12f08b 86 void A3930::pid() {
Reiko 0:a6123d12f08b 87 prevError = error;
Reiko 0:a6123d12f08b 88 error = setPoint - getDecoderCount();
Reiko 0:a6123d12f08b 89
Reiko 0:a6123d12f08b 90 if (stallLevel != 2) {
Reiko 0:a6123d12f08b 91
Reiko 0:a6123d12f08b 92 float err = (float)error / 100.0;
Reiko 0:a6123d12f08b 93 float minPwmValue = minPwm;
Reiko 0:a6123d12f08b 94
Reiko 0:a6123d12f08b 95 if (setPoint == 0) {
Reiko 0:a6123d12f08b 96 minPwmValue = 0;
Reiko 0:a6123d12f08b 97 } else if (setPoint < 0) {
Reiko 0:a6123d12f08b 98 minPwmValue = -minPwm;
Reiko 0:a6123d12f08b 99 }
Reiko 0:a6123d12f08b 100
Reiko 0:a6123d12f08b 101 I += err * iMulti;
Reiko 0:a6123d12f08b 102 //constrain integral
Reiko 0:a6123d12f08b 103 if (I < -1) I = -1;
Reiko 0:a6123d12f08b 104 if (I > 1) I = 1;
Reiko 0:a6123d12f08b 105
Reiko 0:a6123d12f08b 106 float newPWMvalue = minPwmValue + err * pMulti + I;
Reiko 0:a6123d12f08b 107
Reiko 0:a6123d12f08b 108 //constrain pwm
Reiko 0:a6123d12f08b 109 if (newPWMvalue < -1) newPWMvalue = -1;
Reiko 0:a6123d12f08b 110 if (newPWMvalue > 1) newPWMvalue = 1;
Reiko 0:a6123d12f08b 111
Reiko 0:a6123d12f08b 112 prevStallCount = stallCount;
Reiko 0:a6123d12f08b 113 if ((currentSpeed < 5 && currentPWM == 1 || currentSpeed > -5 && currentPWM == -1) && stallCount < stallErrorLimit) {
Reiko 0:a6123d12f08b 114 stallCount++;
Reiko 0:a6123d12f08b 115 } else if (stallCount > 0) {
Reiko 0:a6123d12f08b 116 stallCount--;
Reiko 0:a6123d12f08b 117 }
Reiko 0:a6123d12f08b 118
Reiko 0:a6123d12f08b 119 setPWM(newPWMvalue);
Reiko 0:a6123d12f08b 120
Reiko 0:a6123d12f08b 121 if ((stallCount == stallWarningLimit - 1) && (prevStallCount == stallWarningLimit)) {
Reiko 0:a6123d12f08b 122 stallLevel = 0;
Reiko 0:a6123d12f08b 123 stallEndCallback.call();
Reiko 0:a6123d12f08b 124 stallChangeCallback.call();
Reiko 0:a6123d12f08b 125 } else if ((stallCount == stallWarningLimit) && (prevStallCount == stallWarningLimit - 1)) {
Reiko 0:a6123d12f08b 126 stallLevel = 1;
Reiko 0:a6123d12f08b 127 stallWarningCallback.call();
Reiko 0:a6123d12f08b 128 stallChangeCallback.call();
Reiko 0:a6123d12f08b 129 } else if (stallCount == stallErrorLimit) {
Reiko 0:a6123d12f08b 130 stallLevel = 2;
Reiko 0:a6123d12f08b 131 stallErrorCallback.call();
Reiko 0:a6123d12f08b 132 stallChangeCallback.call();
Reiko 0:a6123d12f08b 133 resetPID();
Reiko 0:a6123d12f08b 134 }
Reiko 0:a6123d12f08b 135 } else {
Reiko 0:a6123d12f08b 136 stallCount--;
Reiko 0:a6123d12f08b 137 if (stallCount == 0) {
Reiko 0:a6123d12f08b 138 stallLevel = 0;
Reiko 0:a6123d12f08b 139 stallEndCallback.call();
Reiko 0:a6123d12f08b 140 stallChangeCallback.call();
Reiko 0:a6123d12f08b 141 }
Reiko 0:a6123d12f08b 142 }
Reiko 0:a6123d12f08b 143 }
Reiko 0:a6123d12f08b 144
Reiko 0:a6123d12f08b 145 void A3930::resetPID() {
Reiko 0:a6123d12f08b 146 error = 0;
Reiko 0:a6123d12f08b 147 prevError = 0;
Reiko 0:a6123d12f08b 148 P = 0;
Reiko 0:a6123d12f08b 149 I = 0;
Reiko 0:a6123d12f08b 150 setPoint = 0;
Reiko 0:a6123d12f08b 151 setPWM(0);
Reiko 0:a6123d12f08b 152 }
Reiko 0:a6123d12f08b 153
Reiko 0:a6123d12f08b 154 int A3930::getStallLevel() {
Reiko 0:a6123d12f08b 155 return stallLevel;
Reiko 0:a6123d12f08b 156 }
Reiko 0:a6123d12f08b 157
Reiko 0:a6123d12f08b 158 void A3930::stallChange(void (*function)(void)) {
Reiko 0:a6123d12f08b 159 stallChangeCallback.attach(function);
Reiko 0:a6123d12f08b 160 }
Reiko 0:a6123d12f08b 161
Reiko 0:a6123d12f08b 162 void A3930::stallEnd(void (*function)(void)) {
Reiko 0:a6123d12f08b 163 stallEndCallback.attach(function);
Reiko 0:a6123d12f08b 164 }
Reiko 0:a6123d12f08b 165
Reiko 0:a6123d12f08b 166 void A3930::stallWarning(void (*function)(void)) {
Reiko 0:a6123d12f08b 167 stallWarningCallback.attach(function);
Reiko 0:a6123d12f08b 168 }
Reiko 0:a6123d12f08b 169
Reiko 0:a6123d12f08b 170 void A3930::stallError(void (*function)(void)) {
Reiko 0:a6123d12f08b 171 stallErrorCallback.attach(function);
Reiko 0:a6123d12f08b 172 }