File content as of revision 4:7620d21baef3:

/*
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),
#ifdef DBH1
       CS(namCS),
#endif
       EN(namEN)
  {
      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
    DiagSerial.printf("Decel %d ms from ful speed\r\n",_decel);
    //BYTE _mode;
    //unsigned long _doneTime;  // time when mode automatically transitions
    DiagSerial.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--;DiagSerial.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()
  {
    DiagSerial.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--;DiagSerial.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--;DiagSerial.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--;DiagSerial.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--;DiagSerial.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--;DiagSerial.puts("moving\r\n");}
            setSpeed(_speedCmd,t);
          }
        return;
      }
  }

#ifdef DBH1
  int getCurrent()
  {
    return(CS.read());
  }
#endif

};