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