Revision 5:b5f360a16354, committed 2014-08-07

Comitter:

ianmcc

Date:

Thu Aug 07 12:23:30 2014 +0000

Parent:

4:3802325cf6e1

Commit message:

Added VNH5019Accel for a drop-in replacement for the VNH5019 but has a built-in acceleration limiter. Default maximum acceleration will result in ramping up to maximum speed in 1/4 second.

Changed in this revision

--- a/VNH5019.cpp	Tue Jul 29 14:25:40 2014 +0000
+++ b/VNH5019.cpp	Thu Aug 07 12:23:30 2014 +0000
@@ -48,38 +48,6 @@
     }
 }
 
-void VNH5019::stop()
-{
-    INA = 0;
-    INB = 0;
-    PWM = 0.0;
-}
-
-void VNH5019::brake(float Brake)
-{
-   // normalize Brake to 0..1
-   if (Brake < 0)
-      Brake = -Brake;
-   if (Brake > 1.0)
-      Brake = 1.0;
-
-   INA = 0;
-   INB = 0;
-   PWM = Brake;
-}
-
-float VNH5019::get_current_mA()
-{
-   // Scale is 144mV per A
-   // Scale factor is 3.3 / 0.144 = 22.916667
-   return CS.read() * 22.916667;
-}
-
-bool VNH5019::is_fault()
-{
-  return !ENDIAG;
-}
-
 void VNH5019::clear_fault()
 {
     // if ENDIAG is high, then there is no fault
@@ -105,33 +73,3 @@
    // and finally re-enable the motor
    ENDIAG.input();
 }
-          
-void VNH5019::disable()
-{
-    ENDIAG.output();
-    ENDIAG.write(0);
-}
-    
-void VNH5019::enable()
-{
-    ENDIAG.input();
-}
-
-DualVNH5019MotorShield::DualVNH5019MotorShield()
-: m1(PTD4, PTA4, PTC8, PTB0, PTD5),
-  m2(PTC9, PTA13, PTD3, PTB1, PTD0)
-{
-}
-
-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_)
-{
-}
-
-VNH5019& 
-DualVNH5019MotorShield::operator()(int m)
-{
-    return m == 1 ? m1 : m2;
-}

--- a/VNH5019.h	Tue Jul 29 14:25:40 2014 +0000
+++ b/VNH5019.h	Thu Aug 07 12:23:30 2014 +0000
@@ -67,4 +67,76 @@
       VNH5019 m2;
 };
 
+inline
+void VNH5019::stop()
+{
+    INA = 0;
+    INB = 0;
+    PWM = 0.0;
+}
+
+inline
+void VNH5019::brake(float Brake)
+{
+   // normalize Brake to 0..1
+   if (Brake < 0)
+      Brake = -Brake;
+   if (Brake > 1.0)
+      Brake = 1.0;
+
+   INA = 0;
+   INB = 0;
+   PWM = Brake;
+}
+
+inline
+float VNH5019::get_current_mA()
+{
+   // Scale is 144mV per A
+   // Scale factor is 3.3 / 0.144 = 22.916667
+   return CS.read() * 22.916667;
+}
+
+inline
+bool VNH5019::is_fault()
+{
+  return !ENDIAG;
+}
+
+inline
+void VNH5019::disable()
+{
+    ENDIAG.output();
+    ENDIAG.write(0);
+}
+
+inline    
+void VNH5019::enable()
+{
+    ENDIAG.input();
+}
+
+inline
+DualVNH5019MotorShield::DualVNH5019MotorShield()
+: m1(PTD4, PTA4, PTC8, PTB0, PTD5),
+  m2(PTC9, PTA13, PTD3, PTB1, PTD0)
+{
+}
+
+inline
+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_)
+{
+}
+
+inline
+VNH5019& 
+DualVNH5019MotorShield::operator()(int m)
+{
+    return m == 1 ? m1 : m2;
+}
+
+
 #endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VNH5019Accel.cpp	Thu Aug 07 12:23:30 2014 +0000
@@ -0,0 +1,36 @@
+#include "VNH5019Accel.h"
+
+void VNH5019Accel::Interrupt()
+{
+    float MyRequestedSpeed = RequestedSpeed;
+    if (CurrentSpeed != MyRequestedSpeed)
+    {
+        if (std::abs(CurrentSpeed-MyRequestedSpeed) < VNH5019ChangePerTick)
+            CurrentSpeed = MyRequestedSpeed;
+        else if (MyRequestedSpeed > CurrentSpeed)
+            CurrentSpeed += VNH5019ChangePerTick;
+        else
+            CurrentSpeed -= VNH5019ChangePerTick;
+        Driver.speed(CurrentSpeed);
+    }
+    else
+    {
+        float MyRequestedBrake = RequestedBrake;
+        if (CurrentBrake != MyRequestedBrake)
+        {
+            if (std::abs(CurrentBrake-MyRequestedBrake) < VNH5019BrakeChangePerTick)
+                CurrentBrake = MyRequestedBrake;
+            else if (MyRequestedBrake > CurrentBrake)
+                CurrentBrake += VNH5019BrakeChangePerTick;
+          else
+                CurrentBrake -= VNH5019BrakeChangePerTick;
+            Driver.brake(CurrentBrake);
+        }
+    }
+}
+
+DualVNH5019AccelMotorShield::DualVNH5019AccelMotorShield()
+: m1(PTD4, PTA4, PTC8, PTB0, PTD5),
+  m2(PTC9, PTA13, PTD3, PTB1, PTD0)
+{
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VNH5019Accel.h	Thu Aug 07 12:23:30 2014 +0000
@@ -0,0 +1,140 @@
+// VNH5015 driver with acceleration control
+// A drop-in replacement for the basic VNH5019
+
+#if !defined(VNH5019ACCEL_H)
+#define VNH5019ACCEL_H
+
+#include "VNH5019.h"
+#include <Ticker.h>
+
+// The maximum acceleration, in fractions of full speed per second
+float const VNH5019MaxAccel = 4;
+
+// Tick period in microseconds for updating the speed
+int const VNH5019Tick_us = 20000;
+
+float const VNH5019ChangePerTick = VNH5019MaxAccel * VNH5019Tick_us / 1000000;
+
+// allow braking to come on faster
+float const VNH5019BrakeChangePerTick = VNH5019ChangePerTick*10;
+
+class VNH5019Accel
+{
+    public:
+        VNH5019Accel(PinName INA_, PinName INB_, PinName ENDIAG_, PinName CS_, PinName PWM_)
+            : Driver(INA_, INB_, ENDIAG_, CS_, PWM_),
+              CurrentSpeed(0.0),
+              CurrentBrake(0.0),
+              RequestedSpeed(0.0),
+              RequestedBrake(0.0)
+        {
+            timer.attach_us(this, &VNH5019Accel::Interrupt, VNH5019Tick_us);
+        }
+        
+        // set motor speed from -1.0 to +1.0
+        void speed(float Speed)
+        {
+            if (Speed < -1.0)
+                Speed = 1.0;
+            else if (Speed > 1.0)
+                Speed = 1.0;
+            RequestedSpeed = Speed;
+        }
+
+        // stop (no current to the motors)
+        void stop()
+        {
+            RequestedSpeed = 0.0;
+        }
+    
+        // Brake, with strength 0..1
+        void brake(float Brake)
+        {
+            if (Brake < 0.0)
+                Brake = 0;
+            else if (Brake > 1.0)
+                Brake = 1.0;
+            RequestedBrake = Brake;
+            RequestedSpeed = 0.0;
+        }
+
+        // returns the current through the motor, in mA
+        float get_current_mA()
+        {
+            return Driver.get_current_mA();
+        }
+
+        // returns true if there has been a fault
+        bool is_fault()
+        {
+            return Driver.is_fault();
+        }
+
+        // Clears the fault condition
+        // PRECONDITION: is_fault()
+        void clear_fault()
+        {
+            timer.detach();
+            Driver.clear_fault();
+            timer.attach_us(this, &VNH5019Accel::Interrupt, VNH5019Tick_us);
+        }
+
+        // disable the motor, and set outputs to zero.  This is a low power mode.
+        void disable()
+        {
+            timer.detach();
+            Driver.disable();
+        }
+
+        // enable the motor.        
+        void enable()
+        {
+            timer.detach();
+            Driver.enable();
+            timer.attach_us(this, &VNH5019Accel::Interrupt, VNH5019Tick_us);
+        }
+        
+        // set the PWM period of oscillation in seconds
+        void set_pwm_period(float p)
+        {
+            Driver.set_pwm_period(p);
+        }
+
+    private:
+        VNH5019 Driver;
+        Ticker timer;
+        float CurrentSpeed;  // this is only ever accessed from the ISR, so no need for volatile
+        float CurrentBrake;
+        volatile float RequestedSpeed;
+        volatile float RequestedBrake;
+        void Interrupt();
+};        
+
+// 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 DualVNH5019AccelMotorShield
+{
+    public:
+        // default pin selection
+        DualVNH5019AccelMotorShield(); // Default pin selection.
+
+                              // User-defined pin selection. 
+        DualVNH5019AccelMotorShield(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_)
+        {
+        }
+        
+        // returns the given motor object, 1 or 2.
+        VNH5019Accel& operator()(int m)
+        {
+            return m == 1 ? m1 : m2;
+        }
+
+      VNH5019Accel m1;
+      VNH5019Accel m2;
+};
+
+#endif