File content as of revision 0:5d3ab0ea7f27:

#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();
    }
}