Default mbed pwm doesn't have enough resolution at high frequencies, thats why I implemented VNH5019 Motor carrier with FastPWM.
Fork of VNH5019 by
VNH5019.cpp@0:5d3ab0ea7f27, 2014-02-01 (annotated)
- Committer:
- ianmcc
- Date:
- Sat Feb 01 13:48:28 2014 +0000
- Revision:
- 0:5d3ab0ea7f27
- Child:
- 1:5e8d9ed18f0f
Initial checkin
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ianmcc | 0:5d3ab0ea7f27 | 1 | #include "DualVNH5019MotorShield.h" |
ianmcc | 0:5d3ab0ea7f27 | 2 | |
ianmcc | 0:5d3ab0ea7f27 | 3 | // Constructors //////////////////////////////////////////////////////////////// |
ianmcc | 0:5d3ab0ea7f27 | 4 | |
ianmcc | 0:5d3ab0ea7f27 | 5 | DualVNH5019MotorShield::DualVNH5019MotorShield() |
ianmcc | 0:5d3ab0ea7f27 | 6 | : INA1(PTD4), |
ianmcc | 0:5d3ab0ea7f27 | 7 | INB1(PTA4), |
ianmcc | 0:5d3ab0ea7f27 | 8 | ENDIAG1(PTC8), |
ianmcc | 0:5d3ab0ea7f27 | 9 | CS1(PTB0), |
ianmcc | 0:5d3ab0ea7f27 | 10 | PWM1(PTD5), |
ianmcc | 0:5d3ab0ea7f27 | 11 | INA2(PTC9), |
ianmcc | 0:5d3ab0ea7f27 | 12 | INB2(PTA13), |
ianmcc | 0:5d3ab0ea7f27 | 13 | ENDIAG2(PTD3), |
ianmcc | 0:5d3ab0ea7f27 | 14 | CS2(PTB1), |
ianmcc | 0:5d3ab0ea7f27 | 15 | PWM2(PTD0) |
ianmcc | 0:5d3ab0ea7f27 | 16 | { |
ianmcc | 0:5d3ab0ea7f27 | 17 | this->init(); |
ianmcc | 0:5d3ab0ea7f27 | 18 | } |
ianmcc | 0:5d3ab0ea7f27 | 19 | |
ianmcc | 0:5d3ab0ea7f27 | 20 | DualVNH5019MotorShield::DualVNH5019MotorShield(inName INA1_, PinName INB1_, PinName ENDIAG1_, PinName CS1_, PinName PWM1_, |
ianmcc | 0:5d3ab0ea7f27 | 21 | PinName INA2_, PinName INB2_, PinName ENDIAG2_, PinName CS2_, PinName PWM2_) |
ianmcc | 0:5d3ab0ea7f27 | 22 | : INA1(INA1_), |
ianmcc | 0:5d3ab0ea7f27 | 23 | INB1(INB1_), |
ianmcc | 0:5d3ab0ea7f27 | 24 | ENDIAG1(ENDIAG1_), |
ianmcc | 0:5d3ab0ea7f27 | 25 | CS1(CS1_), |
ianmcc | 0:5d3ab0ea7f27 | 26 | PWM1(PWM1_), |
ianmcc | 0:5d3ab0ea7f27 | 27 | INA2(INA2_), |
ianmcc | 0:5d3ab0ea7f27 | 28 | INB2(INB2_), |
ianmcc | 0:5d3ab0ea7f27 | 29 | ENDIAG2(ENDIAG2_), |
ianmcc | 0:5d3ab0ea7f27 | 30 | CS2(CS2_), |
ianmcc | 0:5d3ab0ea7f27 | 31 | PWM2(PWM2_) |
ianmcc | 0:5d3ab0ea7f27 | 32 | { |
ianmcc | 0:5d3ab0ea7f27 | 33 | this->init(); |
ianmcc | 0:5d3ab0ea7f27 | 34 | } |
ianmcc | 0:5d3ab0ea7f27 | 35 | |
ianmcc | 0:5d3ab0ea7f27 | 36 | void DualVNH5019MotorShield::init() |
ianmcc | 0:5d3ab0ea7f27 | 37 | { |
ianmcc | 0:5d3ab0ea7f27 | 38 | ENDIAG1.input(); |
ianmcc | 0:5d3ab0ea7f27 | 39 | ENDIAG1.mode(PullUp); |
ianmcc | 0:5d3ab0ea7f27 | 40 | PWM1.period(0.00025); // 4 kHz (valid 0 - 20 kHz) |
ianmcc | 0:5d3ab0ea7f27 | 41 | PWM1.write(0); |
ianmcc | 0:5d3ab0ea7f27 | 42 | |
ianmcc | 0:5d3ab0ea7f27 | 43 | ENDIAG2.input(); |
ianmcc | 0:5d3ab0ea7f27 | 44 | ENDIAG2.mode(PullUp); |
ianmcc | 0:5d3ab0ea7f27 | 45 | PWM2.write(0); |
ianmcc | 0:5d3ab0ea7f27 | 46 | |
ianmcc | 0:5d3ab0ea7f27 | 47 | INA1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 48 | INB1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 49 | INA2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 50 | INB2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 51 | } |
ianmcc | 0:5d3ab0ea7f27 | 52 | |
ianmcc | 0:5d3ab0ea7f27 | 53 | void DualVNH5019MotorShield::setM1Speed(float Speed) |
ianmcc | 0:5d3ab0ea7f27 | 54 | { |
ianmcc | 0:5d3ab0ea7f27 | 55 | bool Reverse = 0; |
ianmcc | 0:5d3ab0ea7f27 | 56 | |
ianmcc | 0:5d3ab0ea7f27 | 57 | if (Speed < 0) |
ianmcc | 0:5d3ab0ea7f27 | 58 | { |
ianmcc | 0:5d3ab0ea7f27 | 59 | Speed = -Speed; // Make speed a positive quantity |
ianmcc | 0:5d3ab0ea7f27 | 60 | Reverse = 1; // Preserve the direction |
ianmcc | 0:5d3ab0ea7f27 | 61 | } |
ianmcc | 0:5d3ab0ea7f27 | 62 | |
ianmcc | 0:5d3ab0ea7f27 | 63 | // clamp the speed at maximum |
ianmcc | 0:5d3ab0ea7f27 | 64 | if (Speed > 1.0) |
ianmcc | 0:5d3ab0ea7f27 | 65 | Speed = 1.0; |
ianmcc | 0:5d3ab0ea7f27 | 66 | |
ianmcc | 0:5d3ab0ea7f27 | 67 | if (Speed == 0.0) |
ianmcc | 0:5d3ab0ea7f27 | 68 | { |
ianmcc | 0:5d3ab0ea7f27 | 69 | INA1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 70 | INB1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 71 | } |
ianmcc | 0:5d3ab0ea7f27 | 72 | else |
ianmcc | 0:5d3ab0ea7f27 | 73 | { |
ianmcc | 0:5d3ab0ea7f27 | 74 | INA1 = !Reverse; |
ianmcc | 0:5d3ab0ea7f27 | 75 | INB1 = Reverse; |
ianmcc | 0:5d3ab0ea7f27 | 76 | PWM1 = Speed; |
ianmcc | 0:5d3ab0ea7f27 | 77 | } |
ianmcc | 0:5d3ab0ea7f27 | 78 | } |
ianmcc | 0:5d3ab0ea7f27 | 79 | |
ianmcc | 0:5d3ab0ea7f27 | 80 | void DualVNH5019MotorShield::setM2Speed(float Speed) |
ianmcc | 0:5d3ab0ea7f27 | 81 | { |
ianmcc | 0:5d3ab0ea7f27 | 82 | bool Reverse = 0; |
ianmcc | 0:5d3ab0ea7f27 | 83 | |
ianmcc | 0:5d3ab0ea7f27 | 84 | if (Speed < 0) |
ianmcc | 0:5d3ab0ea7f27 | 85 | { |
ianmcc | 0:5d3ab0ea7f27 | 86 | Speed = -Speed; // Make speed a positive quantity |
ianmcc | 0:5d3ab0ea7f27 | 87 | Reverse = 1; // Preserve the direction |
ianmcc | 0:5d3ab0ea7f27 | 88 | } |
ianmcc | 0:5d3ab0ea7f27 | 89 | |
ianmcc | 0:5d3ab0ea7f27 | 90 | // clamp the speed at maximum |
ianmcc | 0:5d3ab0ea7f27 | 91 | if (Speed > 1.0) |
ianmcc | 0:5d3ab0ea7f27 | 92 | Speed = 1.0; |
ianmcc | 0:5d3ab0ea7f27 | 93 | |
ianmcc | 0:5d3ab0ea7f27 | 94 | if (Speed == 0.0) |
ianmcc | 0:5d3ab0ea7f27 | 95 | { |
ianmcc | 0:5d3ab0ea7f27 | 96 | INA2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 97 | INB2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 98 | } |
ianmcc | 0:5d3ab0ea7f27 | 99 | else |
ianmcc | 0:5d3ab0ea7f27 | 100 | { |
ianmcc | 0:5d3ab0ea7f27 | 101 | INA2 = !Reverse; |
ianmcc | 0:5d3ab0ea7f27 | 102 | INB2 = Reverse; |
ianmcc | 0:5d3ab0ea7f27 | 103 | PWM2 = Speed; |
ianmcc | 0:5d3ab0ea7f27 | 104 | } |
ianmcc | 0:5d3ab0ea7f27 | 105 | } |
ianmcc | 0:5d3ab0ea7f27 | 106 | |
ianmcc | 0:5d3ab0ea7f27 | 107 | void DualVNH5019MotorShield::setSpeeds(float m1Speed, float m2Speed) |
ianmcc | 0:5d3ab0ea7f27 | 108 | { |
ianmcc | 0:5d3ab0ea7f27 | 109 | this->setM1Speed(m1Speed); |
ianmcc | 0:5d3ab0ea7f27 | 110 | this->setM2Speed(m2Speed); |
ianmcc | 0:5d3ab0ea7f27 | 111 | } |
ianmcc | 0:5d3ab0ea7f27 | 112 | |
ianmcc | 0:5d3ab0ea7f27 | 113 | void DualVNH5019MotorShield::setM1Brake(float Brake) |
ianmcc | 0:5d3ab0ea7f27 | 114 | { |
ianmcc | 0:5d3ab0ea7f27 | 115 | // normalize Brake to 0..1 |
ianmcc | 0:5d3ab0ea7f27 | 116 | if (Brake < 0) |
ianmcc | 0:5d3ab0ea7f27 | 117 | Brake = -Brakel |
ianmcc | 0:5d3ab0ea7f27 | 118 | if (Brake > 1.0) |
ianmcc | 0:5d3ab0ea7f27 | 119 | Brake = 1.0; |
ianmcc | 0:5d3ab0ea7f27 | 120 | |
ianmcc | 0:5d3ab0ea7f27 | 121 | INA1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 122 | INB1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 123 | PWM1 = Brake; |
ianmcc | 0:5d3ab0ea7f27 | 124 | } |
ianmcc | 0:5d3ab0ea7f27 | 125 | |
ianmcc | 0:5d3ab0ea7f27 | 126 | void DualVNH5019MotorShield::setM2Brake(float Brake) |
ianmcc | 0:5d3ab0ea7f27 | 127 | { |
ianmcc | 0:5d3ab0ea7f27 | 128 | // normalize Brake to 0..1 |
ianmcc | 0:5d3ab0ea7f27 | 129 | if (Brake < 0) |
ianmcc | 0:5d3ab0ea7f27 | 130 | Brake = -Brakel; |
ianmcc | 0:5d3ab0ea7f27 | 131 | if (Brake > 1.0) |
ianmcc | 0:5d3ab0ea7f27 | 132 | Brake = 1.0; |
ianmcc | 0:5d3ab0ea7f27 | 133 | |
ianmcc | 0:5d3ab0ea7f27 | 134 | INA2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 135 | INB2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 136 | PWM2 = Brake; |
ianmcc | 0:5d3ab0ea7f27 | 137 | } |
ianmcc | 0:5d3ab0ea7f27 | 138 | |
ianmcc | 0:5d3ab0ea7f27 | 139 | void DualVNH5019MotorShield::setBrakes(float m1Brake, float m2Brake) |
ianmcc | 0:5d3ab0ea7f27 | 140 | { |
ianmcc | 0:5d3ab0ea7f27 | 141 | this->setM1Brake(m1Brake); |
ianmcc | 0:5d3ab0ea7f27 | 142 | this->setM2Brake(m2Brake); |
ianmcc | 0:5d3ab0ea7f27 | 143 | } |
ianmcc | 0:5d3ab0ea7f27 | 144 | |
ianmcc | 0:5d3ab0ea7f27 | 145 | float DualVNH5019MotorShield::getM1CurrentMA() |
ianmcc | 0:5d3ab0ea7f27 | 146 | { |
ianmcc | 0:5d3ab0ea7f27 | 147 | // Scale is 144mV per A |
ianmcc | 0:5d3ab0ea7f27 | 148 | // Scale factor is 3.3 / 0.144 = 22.916667 |
ianmcc | 0:5d3ab0ea7f27 | 149 | return CS1.read() * 22.916667; |
ianmcc | 0:5d3ab0ea7f27 | 150 | } |
ianmcc | 0:5d3ab0ea7f27 | 151 | |
ianmcc | 0:5d3ab0ea7f27 | 152 | float DualVNH5019MotorShield::getM1CurrentMA() |
ianmcc | 0:5d3ab0ea7f27 | 153 | { |
ianmcc | 0:5d3ab0ea7f27 | 154 | // Scale is 144mV per A |
ianmcc | 0:5d3ab0ea7f27 | 155 | // Scale factor is 3.3 / 0.144 = 22.916667 |
ianmcc | 0:5d3ab0ea7f27 | 156 | return CS2.read() * 22.916667; |
ianmcc | 0:5d3ab0ea7f27 | 157 | } |
ianmcc | 0:5d3ab0ea7f27 | 158 | |
ianmcc | 0:5d3ab0ea7f27 | 159 | bool DualVNH5019MotorShield::getM1Fault() |
ianmcc | 0:5d3ab0ea7f27 | 160 | { |
ianmcc | 0:5d3ab0ea7f27 | 161 | return !ENDIAG1; |
ianmcc | 0:5d3ab0ea7f27 | 162 | } |
ianmcc | 0:5d3ab0ea7f27 | 163 | |
ianmcc | 0:5d3ab0ea7f27 | 164 | bool DualVNH5019MotorShield::getM2Fault() |
ianmcc | 0:5d3ab0ea7f27 | 165 | { |
ianmcc | 0:5d3ab0ea7f27 | 166 | return !ENDIAG2; |
ianmcc | 0:5d3ab0ea7f27 | 167 | } |
ianmcc | 0:5d3ab0ea7f27 | 168 | |
ianmcc | 0:5d3ab0ea7f27 | 169 | void DualVNH5019MotorShield::clearM1Fault() |
ianmcc | 0:5d3ab0ea7f27 | 170 | { |
ianmcc | 0:5d3ab0ea7f27 | 171 | // if ENDIAG is high, then there is no fault |
ianmcc | 0:5d3ab0ea7f27 | 172 | if (ENDIAG1.read()) |
ianmcc | 0:5d3ab0ea7f27 | 173 | return; |
ianmcc | 0:5d3ab0ea7f27 | 174 | |
ianmcc | 0:5d3ab0ea7f27 | 175 | // toggle the inputs |
ianmcc | 0:5d3ab0ea7f27 | 176 | INA1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 177 | INB1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 178 | wait_us(250); |
ianmcc | 0:5d3ab0ea7f27 | 179 | INA1 = 1; |
ianmcc | 0:5d3ab0ea7f27 | 180 | INB1 = 1; |
ianmcc | 0:5d3ab0ea7f27 | 181 | wait_us(250); |
ianmcc | 0:5d3ab0ea7f27 | 182 | |
ianmcc | 0:5d3ab0ea7f27 | 183 | // pull low all inputs and wait 1600us for t_DEL |
ianmcc | 0:5d3ab0ea7f27 | 184 | INA1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 185 | INB1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 186 | PWM1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 187 | ENDIAG1.output(); |
ianmcc | 0:5d3ab0ea7f27 | 188 | ENDIAG1 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 189 | wait_us(1600); |
ianmcc | 0:5d3ab0ea7f27 | 190 | |
ianmcc | 0:5d3ab0ea7f27 | 191 | // and finally re-enable the motor |
ianmcc | 0:5d3ab0ea7f27 | 192 | ENDIAG1.input(); |
ianmcc | 0:5d3ab0ea7f27 | 193 | } |
ianmcc | 0:5d3ab0ea7f27 | 194 | |
ianmcc | 0:5d3ab0ea7f27 | 195 | void DualVNH5019MotorShield::clearM2Fault() |
ianmcc | 0:5d3ab0ea7f27 | 196 | { |
ianmcc | 0:5d3ab0ea7f27 | 197 | // if ENDIAG is high, then there is no fault |
ianmcc | 0:5d3ab0ea7f27 | 198 | if (ENDIAG2.read()) |
ianmcc | 0:5d3ab0ea7f27 | 199 | return; |
ianmcc | 0:5d3ab0ea7f27 | 200 | |
ianmcc | 0:5d3ab0ea7f27 | 201 | // toggle the inputs |
ianmcc | 0:5d3ab0ea7f27 | 202 | INA2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 203 | INB2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 204 | wait_us(250); |
ianmcc | 0:5d3ab0ea7f27 | 205 | INA2 = 1; |
ianmcc | 0:5d3ab0ea7f27 | 206 | INB2 = 1; |
ianmcc | 0:5d3ab0ea7f27 | 207 | wait_us(250); |
ianmcc | 0:5d3ab0ea7f27 | 208 | |
ianmcc | 0:5d3ab0ea7f27 | 209 | // pull low all inputs and wait 1600us for t_DEL |
ianmcc | 0:5d3ab0ea7f27 | 210 | INA2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 211 | INB2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 212 | PWM2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 213 | ENDIAG2.output(); |
ianmcc | 0:5d3ab0ea7f27 | 214 | ENDIAG2 = 0; |
ianmcc | 0:5d3ab0ea7f27 | 215 | wait_us(1600); |
ianmcc | 0:5d3ab0ea7f27 | 216 | |
ianmcc | 0:5d3ab0ea7f27 | 217 | // and finally re-enable the motor |
ianmcc | 0:5d3ab0ea7f27 | 218 | ENDIAG2.input(); |
ianmcc | 0:5d3ab0ea7f27 | 219 | } |
ianmcc | 0:5d3ab0ea7f27 | 220 | |
ianmcc | 0:5d3ab0ea7f27 | 221 | void DualVNH5019MotorShield::disableM1() |
ianmcc | 0:5d3ab0ea7f27 | 222 | { |
ianmcc | 0:5d3ab0ea7f27 | 223 | ENDIAG1.output(); |
ianmcc | 0:5d3ab0ea7f27 | 224 | ENDIAG1.write(0); |
ianmcc | 0:5d3ab0ea7f27 | 225 | } |
ianmcc | 0:5d3ab0ea7f27 | 226 | |
ianmcc | 0:5d3ab0ea7f27 | 227 | void DualVNH5019MotorShield::disableM2() |
ianmcc | 0:5d3ab0ea7f27 | 228 | { |
ianmcc | 0:5d3ab0ea7f27 | 229 | ENDIAG2.output(); |
ianmcc | 0:5d3ab0ea7f27 | 230 | ENDIAG2.write(0); |
ianmcc | 0:5d3ab0ea7f27 | 231 | } |
ianmcc | 0:5d3ab0ea7f27 | 232 | |
ianmcc | 0:5d3ab0ea7f27 | 233 | void DualVNH5019MotorShield::enableM1(bool Enable) |
ianmcc | 0:5d3ab0ea7f27 | 234 | { |
ianmcc | 0:5d3ab0ea7f27 | 235 | if (Enable) |
ianmcc | 0:5d3ab0ea7f27 | 236 | { |
ianmcc | 0:5d3ab0ea7f27 | 237 | ENDIAG1.input(); |
ianmcc | 0:5d3ab0ea7f27 | 238 | } |
ianmcc | 0:5d3ab0ea7f27 | 239 | else |
ianmcc | 0:5d3ab0ea7f27 | 240 | { |
ianmcc | 0:5d3ab0ea7f27 | 241 | this->disableM1(); |
ianmcc | 0:5d3ab0ea7f27 | 242 | } |
ianmcc | 0:5d3ab0ea7f27 | 243 | } |
ianmcc | 0:5d3ab0ea7f27 | 244 | |
ianmcc | 0:5d3ab0ea7f27 | 245 | void DualVNH5019MotorShield::enableM2(bool Enable) |
ianmcc | 0:5d3ab0ea7f27 | 246 | { |
ianmcc | 0:5d3ab0ea7f27 | 247 | if (Enable) |
ianmcc | 0:5d3ab0ea7f27 | 248 | { |
ianmcc | 0:5d3ab0ea7f27 | 249 | ENDIAG2.input(); |
ianmcc | 0:5d3ab0ea7f27 | 250 | } |
ianmcc | 0:5d3ab0ea7f27 | 251 | else |
ianmcc | 0:5d3ab0ea7f27 | 252 | { |
ianmcc | 0:5d3ab0ea7f27 | 253 | this->disableM2(); |
ianmcc | 0:5d3ab0ea7f27 | 254 | } |
ianmcc | 0:5d3ab0ea7f27 | 255 | } |