Diff: VNH5019.cpp

Revision:

1:5e8d9ed18f0f

Parent:

0:5d3ab0ea7f27

Child:

3:2b139675f60d

--- a/VNH5019.cpp	Sat Feb 01 13:48:28 2014 +0000
+++ b/VNH5019.cpp	Sat Feb 01 14:46:45 2014 +0000
@@ -1,83 +1,26 @@
-#include "DualVNH5019MotorShield.h"
-
-// Constructors ////////////////////////////////////////////////////////////////
+#include "VNH5019.h"
 
-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_)
+VNH5019::VNH5019(PinName INA_, PinName INB_, PinName ENDIAG_, PinName CS_, PinName PWM_)
+ : INA(INA_),
+   INB(INB_),
+   ENDIAG(ENDIAG_),
+   CS(CS_),
+   PWM(PWM_)
 {
     this->init();
 }
 
-void DualVNH5019MotorShield::init()
+void VNH5019::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;
+   ENDIAG.input();
+   ENDIAG.mode(PullUp);
+   PWM.period(0.00025);   // 4 kHz (valid 0 - 20 kHz)
+   PWM.write(0);
+   INA = 0;
+   INB = 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)
+void VNH5019::speed(float Speed)
 {
    bool Reverse = 0;
   
@@ -93,163 +36,94 @@
     
    if (Speed == 0.0)
    {
-       INA2 = 0;
-       INB2 = 0;
+       INA = 0;
+       INB = 0;
     }
     else
     {
-      INA2 = !Reverse;
-      INB2 = Reverse;
-      PWM2 = Speed;
+      INA = !Reverse;
+      INB = Reverse;
+      PWM = Speed;
     }
 }
 
-void DualVNH5019MotorShield::setSpeeds(float m1Speed, float m2Speed)
-{
-  this->setM1Speed(m1Speed);
-  this->setM2Speed(m2Speed);
-}
-
-void DualVNH5019MotorShield::setM1Brake(float Brake)
+void VNH5019::brake(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;
+      Brake = -Brake;
    if (Brake > 1.0)
       Brake = 1.0;
 
-   INA2 = 0;
-   INB2 = 0;
-   PWM2 = Brake;
+   INA = 0;
+   INB = 0;
+   PWM = Brake;
 }
 
-void DualVNH5019MotorShield::setBrakes(float m1Brake, float m2Brake)
-{
-  this->setM1Brake(m1Brake);
-  this->setM2Brake(m2Brake);
-}
-
-float DualVNH5019MotorShield::getM1CurrentMA()
+float VNH5019::get_current_mA()
 {
    // 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;
+   return CS.read() * 22.916667;
 }
 
-bool DualVNH5019MotorShield::getM1Fault()
+bool VNH5019::is_fault()
 {
-  return !ENDIAG1;
+  return !ENDIAG;
 }
 
-bool DualVNH5019MotorShield::getM2Fault()
-{
-  return !ENDIAG2;
-}
-
-void DualVNH5019MotorShield::clearM1Fault()
+void VNH5019::clear_fault()
 {
     // if ENDIAG is high, then there is no fault
-    if (ENDIAG1.read())
+    if (ENDIAG.read())
        return;
 
    // toggle the inputs
-   INA1 = 0;
-   INB1 = 0;
-   wait_us(250);
-   INA1 = 1;
-   INB1 = 1;
+   INA = 0;
+   INB = 0;
    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;
+   INA = 1;
+   INB = 1;
    wait_us(250);
    
    // pull low all inputs and wait 1600us for t_DEL
-   INA2 = 0;
-   INB2 = 0;
-   PWM2 = 0;
-   ENDIAG2.output();
-   ENDIAG2 = 0;
+   INA = 0;
+   INB = 0;
+   PWM = 0;
+   ENDIAG.output();
+   ENDIAG = 0;
    wait_us(1600);
    
    // and finally re-enable the motor
-   ENDIAG2.input();
+   ENDIAG.input();
 }
-      
-void DualVNH5019MotorShield::disableM1()
+          
+void VNH5019::disable()
 {
-    ENDIAG1.output();
-    ENDIAG1.write(0);
+    ENDIAG.output();
+    ENDIAG.write(0);
 }
     
-void DualVNH5019MotorShield::disableM2()
+void VNH5019::enable()
 {
-    ENDIAG2.output();
-    ENDIAG2.write(0);
+    ENDIAG.input();
 }
 
-void DualVNH5019MotorShield::enableM1(bool Enable)
+DualVNH5019MotorShield::DualVNH5019MotorShield()
+: m1(PTD4, PTA4, PTC8, PTB0, PTD5),
+  m2(PTC9, PTA13, PTD3, PTB1, PTD0)
 {
-    if (Enable)
-    {
-        ENDIAG1.input();
-    }
-    else
-    {
-        this->disableM1();
-    }
 }
 
-void DualVNH5019MotorShield::enableM2(bool Enable)
+DualVNH5019MotorShield::DualVNH5019MotorShield(PinName INA1_, PinName INB1_, PinName ENDIAG1_, PinName CS1_, PinName PWM1_,
+                                               PinName INA2_, PinName INB2_, PinName ENDIAG2_, PinName CS2_, PinName PWM2_)
+ : m1(INA1_, INB1_, ENDIAG1_, CS1_, PWM1_),
+   m2(INA2_, INB2_, ENDIAG2_, CS2_, PWM2_)
 {
-    if (Enable)
-    {
-        ENDIAG2.input();
-    }
-    else
-    {
-        this->disableM2();
-    }
 }
+
+VNH5019& 
+DualVNH5019MotorShield::operator()(int m)
+{
+    return m == 1 ? m1 : m2;
+}