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@1:5e8d9ed18f0f, 2014-02-01 (annotated)
- Committer:
- ianmcc
- Date:
- Sat Feb 01 14:46:45 2014 +0000
- Revision:
- 1:5e8d9ed18f0f
- Parent:
- 0:5d3ab0ea7f27
- Child:
- 3:2b139675f60d
Refactor into a VNH5019 class to control a single motor, and a helper class for the dual shield.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ianmcc | 1:5e8d9ed18f0f | 1 | #include "VNH5019.h" |
ianmcc | 0:5d3ab0ea7f27 | 2 | |
ianmcc | 1:5e8d9ed18f0f | 3 | VNH5019::VNH5019(PinName INA_, PinName INB_, PinName ENDIAG_, PinName CS_, PinName PWM_) |
ianmcc | 1:5e8d9ed18f0f | 4 | : INA(INA_), |
ianmcc | 1:5e8d9ed18f0f | 5 | INB(INB_), |
ianmcc | 1:5e8d9ed18f0f | 6 | ENDIAG(ENDIAG_), |
ianmcc | 1:5e8d9ed18f0f | 7 | CS(CS_), |
ianmcc | 1:5e8d9ed18f0f | 8 | PWM(PWM_) |
ianmcc | 0:5d3ab0ea7f27 | 9 | { |
ianmcc | 0:5d3ab0ea7f27 | 10 | this->init(); |
ianmcc | 0:5d3ab0ea7f27 | 11 | } |
ianmcc | 0:5d3ab0ea7f27 | 12 | |
ianmcc | 1:5e8d9ed18f0f | 13 | void VNH5019::init() |
ianmcc | 0:5d3ab0ea7f27 | 14 | { |
ianmcc | 1:5e8d9ed18f0f | 15 | ENDIAG.input(); |
ianmcc | 1:5e8d9ed18f0f | 16 | ENDIAG.mode(PullUp); |
ianmcc | 1:5e8d9ed18f0f | 17 | PWM.period(0.00025); // 4 kHz (valid 0 - 20 kHz) |
ianmcc | 1:5e8d9ed18f0f | 18 | PWM.write(0); |
ianmcc | 1:5e8d9ed18f0f | 19 | INA = 0; |
ianmcc | 1:5e8d9ed18f0f | 20 | INB = 0; |
ianmcc | 0:5d3ab0ea7f27 | 21 | } |
ianmcc | 0:5d3ab0ea7f27 | 22 | |
ianmcc | 1:5e8d9ed18f0f | 23 | void VNH5019::speed(float Speed) |
ianmcc | 0:5d3ab0ea7f27 | 24 | { |
ianmcc | 0:5d3ab0ea7f27 | 25 | bool Reverse = 0; |
ianmcc | 0:5d3ab0ea7f27 | 26 | |
ianmcc | 0:5d3ab0ea7f27 | 27 | if (Speed < 0) |
ianmcc | 0:5d3ab0ea7f27 | 28 | { |
ianmcc | 0:5d3ab0ea7f27 | 29 | Speed = -Speed; // Make speed a positive quantity |
ianmcc | 0:5d3ab0ea7f27 | 30 | Reverse = 1; // Preserve the direction |
ianmcc | 0:5d3ab0ea7f27 | 31 | } |
ianmcc | 0:5d3ab0ea7f27 | 32 | |
ianmcc | 0:5d3ab0ea7f27 | 33 | // clamp the speed at maximum |
ianmcc | 0:5d3ab0ea7f27 | 34 | if (Speed > 1.0) |
ianmcc | 0:5d3ab0ea7f27 | 35 | Speed = 1.0; |
ianmcc | 0:5d3ab0ea7f27 | 36 | |
ianmcc | 0:5d3ab0ea7f27 | 37 | if (Speed == 0.0) |
ianmcc | 0:5d3ab0ea7f27 | 38 | { |
ianmcc | 1:5e8d9ed18f0f | 39 | INA = 0; |
ianmcc | 1:5e8d9ed18f0f | 40 | INB = 0; |
ianmcc | 0:5d3ab0ea7f27 | 41 | } |
ianmcc | 0:5d3ab0ea7f27 | 42 | else |
ianmcc | 0:5d3ab0ea7f27 | 43 | { |
ianmcc | 1:5e8d9ed18f0f | 44 | INA = !Reverse; |
ianmcc | 1:5e8d9ed18f0f | 45 | INB = Reverse; |
ianmcc | 1:5e8d9ed18f0f | 46 | PWM = Speed; |
ianmcc | 0:5d3ab0ea7f27 | 47 | } |
ianmcc | 0:5d3ab0ea7f27 | 48 | } |
ianmcc | 0:5d3ab0ea7f27 | 49 | |
ianmcc | 1:5e8d9ed18f0f | 50 | void VNH5019::brake(float Brake) |
ianmcc | 0:5d3ab0ea7f27 | 51 | { |
ianmcc | 0:5d3ab0ea7f27 | 52 | // normalize Brake to 0..1 |
ianmcc | 0:5d3ab0ea7f27 | 53 | if (Brake < 0) |
ianmcc | 1:5e8d9ed18f0f | 54 | Brake = -Brake; |
ianmcc | 0:5d3ab0ea7f27 | 55 | if (Brake > 1.0) |
ianmcc | 0:5d3ab0ea7f27 | 56 | Brake = 1.0; |
ianmcc | 0:5d3ab0ea7f27 | 57 | |
ianmcc | 1:5e8d9ed18f0f | 58 | INA = 0; |
ianmcc | 1:5e8d9ed18f0f | 59 | INB = 0; |
ianmcc | 1:5e8d9ed18f0f | 60 | PWM = Brake; |
ianmcc | 0:5d3ab0ea7f27 | 61 | } |
ianmcc | 0:5d3ab0ea7f27 | 62 | |
ianmcc | 1:5e8d9ed18f0f | 63 | float VNH5019::get_current_mA() |
ianmcc | 0:5d3ab0ea7f27 | 64 | { |
ianmcc | 0:5d3ab0ea7f27 | 65 | // Scale is 144mV per A |
ianmcc | 0:5d3ab0ea7f27 | 66 | // Scale factor is 3.3 / 0.144 = 22.916667 |
ianmcc | 1:5e8d9ed18f0f | 67 | return CS.read() * 22.916667; |
ianmcc | 0:5d3ab0ea7f27 | 68 | } |
ianmcc | 0:5d3ab0ea7f27 | 69 | |
ianmcc | 1:5e8d9ed18f0f | 70 | bool VNH5019::is_fault() |
ianmcc | 0:5d3ab0ea7f27 | 71 | { |
ianmcc | 1:5e8d9ed18f0f | 72 | return !ENDIAG; |
ianmcc | 0:5d3ab0ea7f27 | 73 | } |
ianmcc | 0:5d3ab0ea7f27 | 74 | |
ianmcc | 1:5e8d9ed18f0f | 75 | void VNH5019::clear_fault() |
ianmcc | 0:5d3ab0ea7f27 | 76 | { |
ianmcc | 0:5d3ab0ea7f27 | 77 | // if ENDIAG is high, then there is no fault |
ianmcc | 1:5e8d9ed18f0f | 78 | if (ENDIAG.read()) |
ianmcc | 0:5d3ab0ea7f27 | 79 | return; |
ianmcc | 0:5d3ab0ea7f27 | 80 | |
ianmcc | 0:5d3ab0ea7f27 | 81 | // toggle the inputs |
ianmcc | 1:5e8d9ed18f0f | 82 | INA = 0; |
ianmcc | 1:5e8d9ed18f0f | 83 | INB = 0; |
ianmcc | 0:5d3ab0ea7f27 | 84 | wait_us(250); |
ianmcc | 1:5e8d9ed18f0f | 85 | INA = 1; |
ianmcc | 1:5e8d9ed18f0f | 86 | INB = 1; |
ianmcc | 0:5d3ab0ea7f27 | 87 | wait_us(250); |
ianmcc | 0:5d3ab0ea7f27 | 88 | |
ianmcc | 0:5d3ab0ea7f27 | 89 | // pull low all inputs and wait 1600us for t_DEL |
ianmcc | 1:5e8d9ed18f0f | 90 | INA = 0; |
ianmcc | 1:5e8d9ed18f0f | 91 | INB = 0; |
ianmcc | 1:5e8d9ed18f0f | 92 | PWM = 0; |
ianmcc | 1:5e8d9ed18f0f | 93 | ENDIAG.output(); |
ianmcc | 1:5e8d9ed18f0f | 94 | ENDIAG = 0; |
ianmcc | 0:5d3ab0ea7f27 | 95 | wait_us(1600); |
ianmcc | 0:5d3ab0ea7f27 | 96 | |
ianmcc | 0:5d3ab0ea7f27 | 97 | // and finally re-enable the motor |
ianmcc | 1:5e8d9ed18f0f | 98 | ENDIAG.input(); |
ianmcc | 0:5d3ab0ea7f27 | 99 | } |
ianmcc | 1:5e8d9ed18f0f | 100 | |
ianmcc | 1:5e8d9ed18f0f | 101 | void VNH5019::disable() |
ianmcc | 0:5d3ab0ea7f27 | 102 | { |
ianmcc | 1:5e8d9ed18f0f | 103 | ENDIAG.output(); |
ianmcc | 1:5e8d9ed18f0f | 104 | ENDIAG.write(0); |
ianmcc | 0:5d3ab0ea7f27 | 105 | } |
ianmcc | 0:5d3ab0ea7f27 | 106 | |
ianmcc | 1:5e8d9ed18f0f | 107 | void VNH5019::enable() |
ianmcc | 0:5d3ab0ea7f27 | 108 | { |
ianmcc | 1:5e8d9ed18f0f | 109 | ENDIAG.input(); |
ianmcc | 0:5d3ab0ea7f27 | 110 | } |
ianmcc | 0:5d3ab0ea7f27 | 111 | |
ianmcc | 1:5e8d9ed18f0f | 112 | DualVNH5019MotorShield::DualVNH5019MotorShield() |
ianmcc | 1:5e8d9ed18f0f | 113 | : m1(PTD4, PTA4, PTC8, PTB0, PTD5), |
ianmcc | 1:5e8d9ed18f0f | 114 | m2(PTC9, PTA13, PTD3, PTB1, PTD0) |
ianmcc | 0:5d3ab0ea7f27 | 115 | { |
ianmcc | 0:5d3ab0ea7f27 | 116 | } |
ianmcc | 0:5d3ab0ea7f27 | 117 | |
ianmcc | 1:5e8d9ed18f0f | 118 | DualVNH5019MotorShield::DualVNH5019MotorShield(PinName INA1_, PinName INB1_, PinName ENDIAG1_, PinName CS1_, PinName PWM1_, |
ianmcc | 1:5e8d9ed18f0f | 119 | PinName INA2_, PinName INB2_, PinName ENDIAG2_, PinName CS2_, PinName PWM2_) |
ianmcc | 1:5e8d9ed18f0f | 120 | : m1(INA1_, INB1_, ENDIAG1_, CS1_, PWM1_), |
ianmcc | 1:5e8d9ed18f0f | 121 | m2(INA2_, INB2_, ENDIAG2_, CS2_, PWM2_) |
ianmcc | 0:5d3ab0ea7f27 | 122 | { |
ianmcc | 0:5d3ab0ea7f27 | 123 | } |
ianmcc | 1:5e8d9ed18f0f | 124 | |
ianmcc | 1:5e8d9ed18f0f | 125 | VNH5019& |
ianmcc | 1:5e8d9ed18f0f | 126 | DualVNH5019MotorShield::operator()(int m) |
ianmcc | 1:5e8d9ed18f0f | 127 | { |
ianmcc | 1:5e8d9ed18f0f | 128 | return m == 1 ? m1 : m2; |
ianmcc | 1:5e8d9ed18f0f | 129 | } |