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@3:2b139675f60d, 2014-02-09 (annotated)
- Committer:
- ianmcc
- Date:
- Sun Feb 09 11:17:06 2014 +0000
- Revision:
- 3:2b139675f60d
- Parent:
- 1:5e8d9ed18f0f
- Child:
- 4:3802325cf6e1
Comment fix
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 | 3:2b139675f60d | 50 | void VNH5019::stop() |
ianmcc | 3:2b139675f60d | 51 | { |
ianmcc | 3:2b139675f60d | 52 | INA = 0; |
ianmcc | 3:2b139675f60d | 53 | INB = 0; |
ianmcc | 3:2b139675f60d | 54 | PWM = 0.0; |
ianmcc | 3:2b139675f60d | 55 | } |
ianmcc | 3:2b139675f60d | 56 | |
ianmcc | 1:5e8d9ed18f0f | 57 | void VNH5019::brake(float Brake) |
ianmcc | 0:5d3ab0ea7f27 | 58 | { |
ianmcc | 0:5d3ab0ea7f27 | 59 | // normalize Brake to 0..1 |
ianmcc | 0:5d3ab0ea7f27 | 60 | if (Brake < 0) |
ianmcc | 1:5e8d9ed18f0f | 61 | Brake = -Brake; |
ianmcc | 0:5d3ab0ea7f27 | 62 | if (Brake > 1.0) |
ianmcc | 0:5d3ab0ea7f27 | 63 | Brake = 1.0; |
ianmcc | 0:5d3ab0ea7f27 | 64 | |
ianmcc | 1:5e8d9ed18f0f | 65 | INA = 0; |
ianmcc | 1:5e8d9ed18f0f | 66 | INB = 0; |
ianmcc | 1:5e8d9ed18f0f | 67 | PWM = Brake; |
ianmcc | 0:5d3ab0ea7f27 | 68 | } |
ianmcc | 0:5d3ab0ea7f27 | 69 | |
ianmcc | 1:5e8d9ed18f0f | 70 | float VNH5019::get_current_mA() |
ianmcc | 0:5d3ab0ea7f27 | 71 | { |
ianmcc | 0:5d3ab0ea7f27 | 72 | // Scale is 144mV per A |
ianmcc | 0:5d3ab0ea7f27 | 73 | // Scale factor is 3.3 / 0.144 = 22.916667 |
ianmcc | 1:5e8d9ed18f0f | 74 | return CS.read() * 22.916667; |
ianmcc | 0:5d3ab0ea7f27 | 75 | } |
ianmcc | 0:5d3ab0ea7f27 | 76 | |
ianmcc | 1:5e8d9ed18f0f | 77 | bool VNH5019::is_fault() |
ianmcc | 0:5d3ab0ea7f27 | 78 | { |
ianmcc | 1:5e8d9ed18f0f | 79 | return !ENDIAG; |
ianmcc | 0:5d3ab0ea7f27 | 80 | } |
ianmcc | 0:5d3ab0ea7f27 | 81 | |
ianmcc | 1:5e8d9ed18f0f | 82 | void VNH5019::clear_fault() |
ianmcc | 0:5d3ab0ea7f27 | 83 | { |
ianmcc | 0:5d3ab0ea7f27 | 84 | // if ENDIAG is high, then there is no fault |
ianmcc | 1:5e8d9ed18f0f | 85 | if (ENDIAG.read()) |
ianmcc | 0:5d3ab0ea7f27 | 86 | return; |
ianmcc | 0:5d3ab0ea7f27 | 87 | |
ianmcc | 0:5d3ab0ea7f27 | 88 | // toggle the inputs |
ianmcc | 1:5e8d9ed18f0f | 89 | INA = 0; |
ianmcc | 1:5e8d9ed18f0f | 90 | INB = 0; |
ianmcc | 0:5d3ab0ea7f27 | 91 | wait_us(250); |
ianmcc | 1:5e8d9ed18f0f | 92 | INA = 1; |
ianmcc | 1:5e8d9ed18f0f | 93 | INB = 1; |
ianmcc | 0:5d3ab0ea7f27 | 94 | wait_us(250); |
ianmcc | 0:5d3ab0ea7f27 | 95 | |
ianmcc | 0:5d3ab0ea7f27 | 96 | // pull low all inputs and wait 1600us for t_DEL |
ianmcc | 1:5e8d9ed18f0f | 97 | INA = 0; |
ianmcc | 1:5e8d9ed18f0f | 98 | INB = 0; |
ianmcc | 1:5e8d9ed18f0f | 99 | PWM = 0; |
ianmcc | 1:5e8d9ed18f0f | 100 | ENDIAG.output(); |
ianmcc | 1:5e8d9ed18f0f | 101 | ENDIAG = 0; |
ianmcc | 0:5d3ab0ea7f27 | 102 | wait_us(1600); |
ianmcc | 0:5d3ab0ea7f27 | 103 | |
ianmcc | 0:5d3ab0ea7f27 | 104 | // and finally re-enable the motor |
ianmcc | 1:5e8d9ed18f0f | 105 | ENDIAG.input(); |
ianmcc | 0:5d3ab0ea7f27 | 106 | } |
ianmcc | 1:5e8d9ed18f0f | 107 | |
ianmcc | 1:5e8d9ed18f0f | 108 | void VNH5019::disable() |
ianmcc | 0:5d3ab0ea7f27 | 109 | { |
ianmcc | 1:5e8d9ed18f0f | 110 | ENDIAG.output(); |
ianmcc | 1:5e8d9ed18f0f | 111 | ENDIAG.write(0); |
ianmcc | 0:5d3ab0ea7f27 | 112 | } |
ianmcc | 0:5d3ab0ea7f27 | 113 | |
ianmcc | 1:5e8d9ed18f0f | 114 | void VNH5019::enable() |
ianmcc | 0:5d3ab0ea7f27 | 115 | { |
ianmcc | 1:5e8d9ed18f0f | 116 | ENDIAG.input(); |
ianmcc | 0:5d3ab0ea7f27 | 117 | } |
ianmcc | 0:5d3ab0ea7f27 | 118 | |
ianmcc | 1:5e8d9ed18f0f | 119 | DualVNH5019MotorShield::DualVNH5019MotorShield() |
ianmcc | 1:5e8d9ed18f0f | 120 | : m1(PTD4, PTA4, PTC8, PTB0, PTD5), |
ianmcc | 1:5e8d9ed18f0f | 121 | m2(PTC9, PTA13, PTD3, PTB1, PTD0) |
ianmcc | 0:5d3ab0ea7f27 | 122 | { |
ianmcc | 0:5d3ab0ea7f27 | 123 | } |
ianmcc | 0:5d3ab0ea7f27 | 124 | |
ianmcc | 1:5e8d9ed18f0f | 125 | DualVNH5019MotorShield::DualVNH5019MotorShield(PinName INA1_, PinName INB1_, PinName ENDIAG1_, PinName CS1_, PinName PWM1_, |
ianmcc | 1:5e8d9ed18f0f | 126 | PinName INA2_, PinName INB2_, PinName ENDIAG2_, PinName CS2_, PinName PWM2_) |
ianmcc | 1:5e8d9ed18f0f | 127 | : m1(INA1_, INB1_, ENDIAG1_, CS1_, PWM1_), |
ianmcc | 1:5e8d9ed18f0f | 128 | m2(INA2_, INB2_, ENDIAG2_, CS2_, PWM2_) |
ianmcc | 0:5d3ab0ea7f27 | 129 | { |
ianmcc | 0:5d3ab0ea7f27 | 130 | } |
ianmcc | 1:5e8d9ed18f0f | 131 | |
ianmcc | 1:5e8d9ed18f0f | 132 | VNH5019& |
ianmcc | 1:5e8d9ed18f0f | 133 | DualVNH5019MotorShield::operator()(int m) |
ianmcc | 1:5e8d9ed18f0f | 134 | { |
ianmcc | 1:5e8d9ed18f0f | 135 | return m == 1 ? m1 : m2; |
ianmcc | 1:5e8d9ed18f0f | 136 | } |