Ian McCulloch / VNH5019

Library for the VNH5019 Motor Driver, with a helper class for the Pololu Dual VNH5019 Dual Motor Driver Shield http://www.pololu.com/product/2502

Dependents:   VNH5019_second VNH5019_second1

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?

UserRevisionLine numberNew 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 }