Diff: MotorDrive298.h

Revision:

1:23d0a615756a

Child:

3:502f90649834

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDrive298.h	Fri Jul 31 17:37:52 2015 +0000
@@ -0,0 +1,314 @@
+/*
+Motor driver for Motor controllers using the L298 Dual-full-H-Bridge chip,
+or work alikes.
+This chip generally has two direction inputs and an enable pin.
+Typical use is:
+
+    IN1=1, IN2=0, EN=PWM   -- reverse at PWM speed
+    IN1=0, IN2=1, EN=PWM   -- forward at PWM speed
+    IN1=0, IN2=0, EN=1     -- electrical brake
+    IN1=X, IN2=X, EN=0     -- coast
+
+This controller has two inputs for each H-Bridge, and an enable pin.
+It also provides a 5v-ish OUTPUT appropriate for running the MCU
+
+It was found that motors seemed to run a bit faster and more efficient
+with PWM on the Enable.
+Future versions of this driver may allow user to put the PWM
+on the direction pin which seems to provide a more constant-velocity
+response, where putting PWM on the enable is more like the accelerator
+input on a car.  (It has a peak velocity, but until it gets gets close
+to this peak, the input is more closely related to acceleration)
+
+Driver will apply electrical brake when stopping, but
+try to use freewheeling PWM mode when driving.
+
+Aaron Birenboim, http://boim.com    31jul2015
+Apache License
+*/
+
+#define MAXPWM 0.98f
+
+// motor states (I'm afraid of enum which might be 16 bit on 8-bit MCU)
+#define MOTOR_STOPPED   0     // 1's -- running bit
+#define MOTOR_FWD       1     // 2's -- direction (1==rev)
+#define MOTOR_REV       3
+#define MOTOR_START_FWD 5     // 4's -- start-up pulse
+#define MOTOR_START_REV 7
+#define MOTOR_STOPPING  8     // 8 -- electrical braking
+
+#ifndef ABS
+#define ABS(x)  (((x)<0)?(-(x)):(x))
+#endif
+
+typedef char  BYTE;  // signed char, 8-bit
+typedef short SHORT; // signed int, 16-bit
+typedef int   BOOL;
+
+//#ifdef DBH1
+//#typedef PwmOut     DIRPIN;
+//#else
+//#typedef DigitalOut DIRPIN;
+//#else
+
+// ----------- Global state classes:
+#include "MotorDriveBase.h"
+class MotorDrive : public MotorDriveBase
+{
+protected:
+  inline float clipPWM(float pwm)
+  {
+    if (ABS(pwm) > 1.0f)
+      pwm = (pwm < 0) ? -1.0f : 1.0f;
+    return(pwm);
+  }
+  inline float getPWM(float pwm)
+  {
+    return(ABS(clipPWM(pwm)));
+  }
+
+  void setReverse(BOOL rev) // true==set reverse direction, false==forward
+  {
+    if (rev)
+      {
+        IN2.write(0);       // digitalWrite(Pin.IN2,0);
+#ifdef DBH1
+        IN1.write(MAXPWM);  // digitalWrite(Pin.IN1,1);
+#else
+        IN1.write(1);       // analogWrite(Pin.IN1,250);
+#endif
+      }
+    else
+      {
+        IN1.write(0);       // digitalWrite(Pin.IN1,0);
+#ifdef DBH1
+        IN2.write(MAXPWM);  // digitalWrite(Pin.IN2,1);
+#else
+        IN2.write(1);       // analogWrite(Pin.IN2,250);
+#endif
+      }
+  }
+  
+public:
+//  struct {
+    // it does not seem to hurt to have IN1=IN2=1, but it doesn't seem
+    // to do anything different/useful, so try to avoid this state.
+
+#ifdef DBH1
+    PwmOut     IN1, IN2;  // forward/reverse selectors.
+    AnalogIn CS;
+#else
+    DigitalOut IN1, IN2;  // forward/reverse selectors.
+#endif
+    PwmOut EN;        // enable pin
+//  } Pin;
+  float _speed;     // current speed
+  float _speedCmd;  // commanded speed
+
+  int _decel; // time to allow to stop, in ms from full speed
+  BYTE _mode;
+  int _doneTime;  // time when mode automatically transitions
+  int _deadTime;    // ms until deadman transition to emergency stop
+  int _startupTime; // ms of full-power pulse to start from dead stop
+  int _stopTime;    // ms to lock-out commands after emergency stop
+
+  int _msgCount;   // turn off diagnostics after this many messages
+  
+  MotorDrive (PinName namEN, PinName namIN1, PinName namIN2, PinName namCS) :
+       IN1(namIN1),
+       IN2(namIN2),
+       EN(namEN)
+#ifdef DBH1
+       ,CS(namCS)
+#endif
+  {
+      EN.write(0.0f);
+      IN2.write(0.0f);
+      IN2.write(0.0f);
+
+      IN1.period(0.001f);  // these can be audible.  They are just the little 98% limits the driver demands
+      IN2.period(0.001f);
+      EN.period(1.0f/8000.0f);  // may want this higher frequency to make it ultrasonic 
+    
+      _speed = _speedCmd = 0.0f;
+      
+      _decel = 500;
+      _deadTime = 250; // emergency stop if no command update in this many ms
+      _startupTime = 50; // when starting from still, issue full power pulse this long to get motors started
+      _stopTime = 3000;  // Pause at least this long after emergency stop before restarting
+
+      _msgCount=9;
+      emergencyStop();
+  }     
+
+
+  virtual void setCommandTimeout(const int ms) { _deadTime = ms; }
+  void setDecelRate(const int msFromFullSpeed) { _decel=msFromFullSpeed; }
+  void setStartPulseDuration(const int ms) { _startupTime=ms; }
+  void setStopTimeout(const int ms) { _stopTime=ms; }
+  void showDiagnostics(const int n) { _msgCount=n; }
+  void showState()
+  {
+    //BYTE IN1, IN2;  // forward/reverse selectors.
+    //BYTE EN;        // enable pin
+    //SHORT _speed;     // current speed
+    //SHORT _speedCmd;  // commanded speed
+    CmdSerial.printf("Decel %d ms from ful speed\r\n",_decel);
+    //BYTE _mode;
+    //unsigned long _doneTime;  // time when mode automatically transitions
+    CmdSerial.printf("Deadman Timeout %d ms\r\n",_deadTime);
+  //SHORT _maxPWM;      // clip PWM commands to this magnitude
+  //SHORT _startupTime; // ms of full-power pulse to start from dead stop
+  //SHORT _stopTime;    // ms to lock-out commands after emergency stop
+
+  }
+  
+  virtual void stop()
+  {
+    _speedCmd=0;
+    EN.write(0);     // digitalWrite(Pin.EN , 0);
+    IN1.write(0);    // digitalWrite(Pin.IN1, 0);
+    IN2.write(0);    // digitalWrite(Pin.IN2, 0);
+    EN.write(1);     // digitalWrite(Pin.EN , 1);
+    int stoppingTime = (int)ABS(_speed * _decel);
+if(_msgCount>0){_msgCount--;CmdSerial.printf("%dms to stop.\r\n",stoppingTime);}
+    _doneTime = millis() + stoppingTime;
+    //speed=0;  don't clobber command in case of direction change
+    _mode = MOTOR_STOPPING;
+  }
+
+  virtual void emergencyStop()
+  {
+    CmdSerial.puts("Emergency ");
+    _msgCount = 11;  // turn on diagnostics for a few commands
+    stop();
+    _speedCmd=0;
+    _doneTime += _stopTime;
+  }
+
+
+  // Set speed -MAX_PWM for max reverse, MAX_PWM for max forward
+  virtual void setSpeed(const float spdReq, int t)
+  {
+    BYTE prevMode = _mode;
+    bool rev;
+    switch(prevMode)
+      {
+      case MOTOR_STOPPING :
+        _speedCmd = spdReq;
+        if (t < _doneTime)
+          {  // make sure things are stopped
+            IN1.write(0);      // digitalWrite(Pin.IN1,0);
+            IN2.write(0);      // digitalWrite(Pin.IN2,0);
+            EN.write(1);       // digitalWrite(Pin.EN ,1);
+            return;
+          }
+        // done stoping, continue to STOP mode
+        _speed = 0;
+        _mode = MOTOR_STOPPED;
+if(_msgCount>0){_msgCount--;CmdSerial.puts("stopped.\r");}
+      case MOTOR_STOPPED :
+        if (spdReq == 0) return;  // leave in full brake stop
+        _mode = (spdReq < 0) ? MOTOR_START_REV : MOTOR_START_FWD;
+        rev = (_mode == MOTOR_START_REV);
+        IN1.write(rev?1:0);   // digitalWrite(rev?Pin.IN1:Pin.IN2,1); // don't worry about PWM
+        IN2.write(rev?0:1);   // digitalWrite(rev?Pin.IN2:Pin.IN1,0); // this is transistional state
+        EN.write(1);          // digitalWrite(Pin.EN,1);   // hard kick to get started
+        _doneTime = t + _startupTime;
+        _speedCmd = spdReq;
+if(_msgCount>0){_msgCount--;CmdSerial.printf("Start %s\r\n",rev?"REV":"FWD");}
+        return;
+      case MOTOR_FWD :
+      case MOTOR_REV :
+        if (t > _doneTime) { emergencyStop(); return; } // deadman expired
+        if ( (spdReq == 0)  ||  // stop or change direction
+             ((spdReq < 0) && (prevMode == MOTOR_FWD)) ||
+             ((spdReq > 0) && (prevMode == MOTOR_REV)) )
+          {
+            stop();
+            _speedCmd = clipPWM(spdReq);
+            return;
+          }
+        setReverse(spdReq < 0);
+        _speed = _speedCmd = spdReq;
+        EN.write(getPWM(_speed));      // analogWrite(Pin.EN,getPWM(_speed));
+        _doneTime = t + _deadTime;
+//if(_msgCount>0){_msgCount--;Serial.println(_speed);}
+        return;
+      case MOTOR_START_REV :
+      case MOTOR_START_FWD :
+        if (spdReq == 0)
+          {
+            _speed = 100;  // give it some time to decel, although just starting
+            stop();
+            return;
+          }
+        if ( ((spdReq < 0) && (_mode == MOTOR_START_FWD)) ||
+             ((spdReq > 0) && (_mode == MOTOR_START_REV)) )
+          { // direction change
+            _speed = 100;  // give it some time to decel, although just starting
+            stop();
+            _speedCmd = spdReq;  // go to this speed after coast-down period
+            return;
+          }
+        // same direction, but speed request change
+        _speed = _speedCmd = spdReq;
+        if (t >= _doneTime)
+          {
+            _mode = (_speedCmd > 0) ? MOTOR_FWD : MOTOR_REV;
+            _doneTime = t + _deadTime;
+            setReverse(_mode == MOTOR_REV);  // make sure direction is correct
+            EN.write(getPWM(_speedCmd));           // analogWrite(Pin.EN,getPWM(_speedCmd));
+            if(_msgCount>0){_msgCount--;CmdSerial.printf("Started %.3f\r\n",_speedCmd);}
+          }
+        return;
+      }
+  }
+
+  // update state, but no new command was received
+  // Check if previous command is complete,
+  //   and an automatic state transition is needed
+  virtual void update(int t)  // current time, from millis()
+  {
+//Serial.print(F("Update "));  Serial.println(t);
+    if ((_doneTime > 0xfffff000) && (t < 999))
+      {  // time counter must have wrapped around
+        _doneTime = 0;
+        CmdSerial.puts("\rClock wrap-around");
+      }
+
+    BYTE prevMode = _mode;
+    switch(prevMode)
+      {
+      case MOTOR_STOPPING : 
+      case MOTOR_STOPPED :
+        if ((t > _doneTime) && _speedCmd)
+          { // this was a temp stop in a direction change.  Command desired speed.
+if(_msgCount>0){_msgCount--;CmdSerial.printf("Restart %.3f\r\n",_speedCmd);}
+            setSpeed(_speedCmd,t);
+          }
+//else Serial.println("stopped.");
+        return;
+      case MOTOR_FWD :
+      case MOTOR_REV :
+        if (t > _doneTime) emergencyStop(); // deadman expired
+        return;
+      case MOTOR_START_REV :
+      case MOTOR_START_FWD :
+        if (t > _doneTime)
+          {
+if(_msgCount>0){_msgCount--;CmdSerial.puts("moving\r\n");}
+            setSpeed(_speedCmd,t);
+          }
+        return;
+      }
+  }
+
+#ifdef DBH1
+  int getCurrent()
+  {
+    return(CS.read());
+  }
+#endif
+
+};