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?
User | Revision | Line number | New 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 | } |