Default mbed pwm doesn't have enough resolution at high frequencies, thats why I implemented VNH5019 Motor carrier with FastPWM.
Fork of VNH5019 by
Diff: VNH5019.cpp
- Revision:
- 0:5d3ab0ea7f27
- Child:
- 1:5e8d9ed18f0f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/VNH5019.cpp Sat Feb 01 13:48:28 2014 +0000 @@ -0,0 +1,255 @@ +#include "DualVNH5019MotorShield.h" + +// Constructors //////////////////////////////////////////////////////////////// + +DualVNH5019MotorShield::DualVNH5019MotorShield() + : INA1(PTD4), + INB1(PTA4), + ENDIAG1(PTC8), + CS1(PTB0), + PWM1(PTD5), + INA2(PTC9), + INB2(PTA13), + ENDIAG2(PTD3), + CS2(PTB1), + PWM2(PTD0) +{ + this->init(); +} + +DualVNH5019MotorShield::DualVNH5019MotorShield(inName INA1_, PinName INB1_, PinName ENDIAG1_, PinName CS1_, PinName PWM1_, + PinName INA2_, PinName INB2_, PinName ENDIAG2_, PinName CS2_, PinName PWM2_) + : INA1(INA1_), + INB1(INB1_), + ENDIAG1(ENDIAG1_), + CS1(CS1_), + PWM1(PWM1_), + INA2(INA2_), + INB2(INB2_), + ENDIAG2(ENDIAG2_), + CS2(CS2_), + PWM2(PWM2_) +{ + this->init(); +} + +void DualVNH5019MotorShield::init() +{ + ENDIAG1.input(); + ENDIAG1.mode(PullUp); + PWM1.period(0.00025); // 4 kHz (valid 0 - 20 kHz) + PWM1.write(0); + + ENDIAG2.input(); + ENDIAG2.mode(PullUp); + PWM2.write(0); + + INA1 = 0; + INB1 = 0; + INA2 = 0; + INB2 = 0; +} + +void DualVNH5019MotorShield::setM1Speed(float Speed) +{ + bool Reverse = 0; + + if (Speed < 0) + { + Speed = -Speed; // Make speed a positive quantity + Reverse = 1; // Preserve the direction + } + + // clamp the speed at maximum + if (Speed > 1.0) + Speed = 1.0; + + if (Speed == 0.0) + { + INA1 = 0; + INB1 = 0; + } + else + { + INA1 = !Reverse; + INB1 = Reverse; + PWM1 = Speed; + } +} + +void DualVNH5019MotorShield::setM2Speed(float Speed) +{ + bool Reverse = 0; + + if (Speed < 0) + { + Speed = -Speed; // Make speed a positive quantity + Reverse = 1; // Preserve the direction + } + + // clamp the speed at maximum + if (Speed > 1.0) + Speed = 1.0; + + if (Speed == 0.0) + { + INA2 = 0; + INB2 = 0; + } + else + { + INA2 = !Reverse; + INB2 = Reverse; + PWM2 = Speed; + } +} + +void DualVNH5019MotorShield::setSpeeds(float m1Speed, float m2Speed) +{ + this->setM1Speed(m1Speed); + this->setM2Speed(m2Speed); +} + +void DualVNH5019MotorShield::setM1Brake(float Brake) +{ + // normalize Brake to 0..1 + if (Brake < 0) + Brake = -Brakel + if (Brake > 1.0) + Brake = 1.0; + + INA1 = 0; + INB1 = 0; + PWM1 = Brake; +} + +void DualVNH5019MotorShield::setM2Brake(float Brake) +{ + // normalize Brake to 0..1 + if (Brake < 0) + Brake = -Brakel; + if (Brake > 1.0) + Brake = 1.0; + + INA2 = 0; + INB2 = 0; + PWM2 = Brake; +} + +void DualVNH5019MotorShield::setBrakes(float m1Brake, float m2Brake) +{ + this->setM1Brake(m1Brake); + this->setM2Brake(m2Brake); +} + +float DualVNH5019MotorShield::getM1CurrentMA() +{ + // Scale is 144mV per A + // Scale factor is 3.3 / 0.144 = 22.916667 + return CS1.read() * 22.916667; +} + +float DualVNH5019MotorShield::getM1CurrentMA() +{ + // Scale is 144mV per A + // Scale factor is 3.3 / 0.144 = 22.916667 + return CS2.read() * 22.916667; +} + +bool DualVNH5019MotorShield::getM1Fault() +{ + return !ENDIAG1; +} + +bool DualVNH5019MotorShield::getM2Fault() +{ + return !ENDIAG2; +} + +void DualVNH5019MotorShield::clearM1Fault() +{ + // if ENDIAG is high, then there is no fault + if (ENDIAG1.read()) + return; + + // toggle the inputs + INA1 = 0; + INB1 = 0; + wait_us(250); + INA1 = 1; + INB1 = 1; + wait_us(250); + + // pull low all inputs and wait 1600us for t_DEL + INA1 = 0; + INB1 = 0; + PWM1 = 0; + ENDIAG1.output(); + ENDIAG1 = 0; + wait_us(1600); + + // and finally re-enable the motor + ENDIAG1.input(); +} + +void DualVNH5019MotorShield::clearM2Fault() +{ + // if ENDIAG is high, then there is no fault + if (ENDIAG2.read()) + return; + + // toggle the inputs + INA2 = 0; + INB2 = 0; + wait_us(250); + INA2 = 1; + INB2 = 1; + wait_us(250); + + // pull low all inputs and wait 1600us for t_DEL + INA2 = 0; + INB2 = 0; + PWM2 = 0; + ENDIAG2.output(); + ENDIAG2 = 0; + wait_us(1600); + + // and finally re-enable the motor + ENDIAG2.input(); +} + +void DualVNH5019MotorShield::disableM1() +{ + ENDIAG1.output(); + ENDIAG1.write(0); +} + +void DualVNH5019MotorShield::disableM2() +{ + ENDIAG2.output(); + ENDIAG2.write(0); +} + +void DualVNH5019MotorShield::enableM1(bool Enable) +{ + if (Enable) + { + ENDIAG1.input(); + } + else + { + this->disableM1(); + } +} + +void DualVNH5019MotorShield::enableM2(bool Enable) +{ + if (Enable) + { + ENDIAG2.input(); + } + else + { + this->disableM2(); + } +}