Revision 1:5e8d9ed18f0f, committed 2014-02-01

Comitter:

ianmcc

Date:

Sat Feb 01 14:46:45 2014 +0000

Parent:

0:5d3ab0ea7f27

Child:

2:d670a4b999ab

Commit message:

Refactor into a VNH5019 class to control a single motor, and a helper class for the dual shield.

Changed in this revision

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

--- a/VNH5019.h	Sat Feb 01 13:48:28 2014 +0000
+++ b/VNH5019.h	Sat Feb 01 14:46:45 2014 +0000
@@ -3,6 +3,49 @@
 
 #include <mbed.h>
 
+class VNH5019
+{
+    public:
+        VNH5019(PinName INA_, PinName INB_, PinName ENDIAG_, PinName CS_, PinName PWM_);
+        
+        // set motor speed from -1.0 to +1.0
+        void speed(float Speed);
+
+        // stop (no current to the motors)
+        void stop();        
+    
+        // Brake, with strength 0..1
+        void brake(float Brake);
+
+        // returns the current through the motor, in mA
+        float get_current_mA();
+
+        // returns true if there has been a fault
+        bool is_fault();
+
+        // Clears the fault condition
+        // PRECONDITION: is_fault()
+        void clear_fault();
+
+        // disable the motor, and set outputs to zero.  This is a low power mode.
+        void disable();
+
+        // enable the motor.        
+        void enable();
+
+    private:
+        void init(); // Initialize TIMER 1, set the PWM to 20kHZ. 
+
+        DigitalOut   INA;
+        DigitalOut   INB;
+        DigitalInOut ENDIAG;
+        AnalogIn     CS;
+        PwmOut       PWM;
+};        
+
+// Helper class for the Pololu dual VNH5019 motor shield.
+// The default constructor uses the default arduino pins.
+// The motors can be accessed either by .m1 or .m2, or by operator()(i) where i is 1 or 2.
 class DualVNH5019MotorShield
 {
    public:
@@ -13,54 +56,11 @@
       DualVNH5019MotorShield(PinName INA1_, PinName INB1_, PinName ENDIAG1_, PinName CS1_, PinName PWM1_,
                              PinName INA2_, PinName INB2_, PinName ENDIAG2_, PinName CS2_, PinName PWM2_);
     
-      // Set motor speeds, from -1.0 to +1.0
-      void setM1Speed(float Speed);
-      void setM2Speed(float Speed);
-      void setSpeeds(float m1Speed, float m2Speed);
-
-      // stop (no current to the motors)
-      void setM1Stop();
-      void setM2Stop();
-
-      // brake, with strength 0..1
-      void setM1Brake(float Brake);
-      void setM2Brake(float Brake);
-      void setBrakes(float m1Brake, float m2Brake);
-      
-      // return the current supplied to the motors
-      float getM1CurrentMA();
-      float getM2CurrentMA();
-      
-      // fault
-      bool isM1Fault();
-      bool isM2Fault();
+      // returns the given motor object, 1 or 2.
+      VNH5019& operator()(int m);
 
-      // If a fault occurs, then we follow the procedure from the data sheet to clear it
-      void clearM1Fault();
-      void clearM2Fault();
-      
-      // disable / enable (isFault() will return true if the motor is disabled).
-      // Disabling the motor also sets the speed to 0 and brakes to LOW.
-      void disableM1();
-      void disableM2();
-
-      void enableM1(bool Enable = true);
-      void enableM2(bool Enable = true);
-    
-   private:
-      void init(); // Initialize TIMER 1, set the PWM to 20kHZ. 
-
-      DigitalOut   INA1;
-      DigitalOut   INB1;
-      DigitalInOut ENDIAG1;
-      AnalogIn     CS1;
-      PwmOut       PWM1;
-
-      DigitalOut   INA2;
-      DigitalOut   INB2;
-      DigitalInOut ENDIAG2;
-      AnalogIn     CS2;
-      PwmOut       PWM2;
+      VNH5019 m1;
+      VNH5019 m2;
 };
 
 #endif