MotorDrive298.h

00001 /*
00002 Motor driver for Motor controllers using the L298 Dual-full-H-Bridge chip,
00003 or work alikes.
00004 This chip generally has two direction inputs and an enable pin.
00005 Typical use is:
00006 
00007     IN1=1, IN2=0, EN=PWM   -- reverse at PWM speed
00008     IN1=0, IN2=1, EN=PWM   -- forward at PWM speed
00009     IN1=0, IN2=0, EN=1     -- electrical brake
00010     IN1=X, IN2=X, EN=0     -- coast
00011 
00012 This controller has two inputs for each H-Bridge, and an enable pin.
00013 It also provides a 5v-ish OUTPUT appropriate for running the MCU
00014 
00015 It was found that motors seemed to run a bit faster and more efficient
00016 with PWM on the Enable.
00017 Future versions of this driver may allow user to put the PWM
00018 on the direction pin which seems to provide a more constant-velocity
00019 response, where putting PWM on the enable is more like the accelerator
00020 input on a car.  (It has a peak velocity, but until it gets gets close
00021 to this peak, the input is more closely related to acceleration)
00022 
00023 Driver will apply electrical brake when stopping, but
00024 try to use freewheeling PWM mode when driving.
00025 
00026 Aaron Birenboim, http://boim.com    31jul2015
00027 Apache License
00028 */
00029 
00030 #define MAXPWM 0.98f
00031 
00032 // motor states (I'm afraid of enum which might be 16 bit on 8-bit MCU)
00033 #define MOTOR_STOPPED   0     // 1's -- running bit
00034 #define MOTOR_FWD       1     // 2's -- direction (1==rev)
00035 #define MOTOR_REV       3
00036 #define MOTOR_START_FWD 5     // 4's -- start-up pulse
00037 #define MOTOR_START_REV 7
00038 #define MOTOR_STOPPING  8     // 8 -- electrical braking
00039 
00040 #ifndef ABS
00041 #define ABS(x)  (((x)<0)?(-(x)):(x))
00042 #endif
00043 
00044 typedef char  BYTE;  // signed char, 8-bit
00045 typedef short SHORT; // signed int, 16-bit
00046 typedef int   BOOL;
00047 
00048 //#ifdef DBH1
00049 //#typedef PwmOut     DIRPIN;
00050 //#else
00051 //#typedef DigitalOut DIRPIN;
00052 //#else
00053 
00054 // ----------- Global state classes:
00055 #include "MotorDriveBase.h"
00056 class MotorDrive : public MotorDriveBase
00057 {
00058 protected:
00059   inline float clipPWM(float pwm)
00060   {
00061     if (ABS(pwm) > 1.0f)
00062       pwm = (pwm < 0) ? -1.0f : 1.0f;
00063     return(pwm);
00064   }
00065   inline float getPWM(float pwm)
00066   {
00067     return(ABS(clipPWM(pwm)));
00068   }
00069 
00070   void setReverse(BOOL rev) // true==set reverse direction, false==forward
00071   {
00072     if (rev)
00073       {
00074         IN2.write(0);       // digitalWrite(Pin.IN2,0);
00075 #ifdef DBH1
00076         IN1.write(MAXPWM);  // digitalWrite(Pin.IN1,1);
00077 #else
00078         IN1.write(1);       // analogWrite(Pin.IN1,250);
00079 #endif
00080       }
00081     else
00082       {
00083         IN1.write(0);       // digitalWrite(Pin.IN1,0);
00084 #ifdef DBH1
00085         IN2.write(MAXPWM);  // digitalWrite(Pin.IN2,1);
00086 #else
00087         IN2.write(1);       // analogWrite(Pin.IN2,250);
00088 #endif
00089       }
00090   }
00091   
00092 public:
00093 //  struct {
00094     // it does not seem to hurt to have IN1=IN2=1, but it doesn't seem
00095     // to do anything different/useful, so try to avoid this state.
00096 
00097 #ifdef DBH1
00098     PwmOut     IN1, IN2;  // forward/reverse selectors.
00099     AnalogIn CS;
00100 #else
00101     DigitalOut IN1, IN2;  // forward/reverse selectors.
00102 #endif
00103     PwmOut EN;        // enable pin
00104 //  } Pin;
00105   float _speed;     // current speed
00106   float _speedCmd;  // commanded speed
00107 
00108   int _decel; // time to allow to stop, in ms from full speed
00109   BYTE _mode;
00110   int _doneTime;  // time when mode automatically transitions
00111   int _deadTime;    // ms until deadman transition to emergency stop
00112   int _startupTime; // ms of full-power pulse to start from dead stop
00113   int _stopTime;    // ms to lock-out commands after emergency stop
00114 
00115   int _msgCount;   // turn off diagnostics after this many messages
00116   
00117   MotorDrive (PinName namEN, PinName namIN1, PinName namIN2, PinName namCS) :
00118        IN1(namIN1),
00119        IN2(namIN2),
00120 #ifdef DBH1
00121        CS(namCS),
00122 #endif
00123        EN(namEN)
00124   {
00125       EN.write(0.0f);
00126       IN2.write(0.0f);
00127       IN2.write(0.0f);
00128 
00129       IN1.period(0.001f);  // these can be audible.  They are just the little 98% limits the driver demands
00130       IN2.period(0.001f);
00131       EN.period(1.0f/8000.0f);  // may want this higher frequency to make it ultrasonic 
00132     
00133       _speed = _speedCmd = 0.0f;
00134       
00135       _decel = 500;
00136       _deadTime = 250; // emergency stop if no command update in this many ms
00137       _startupTime = 50; // when starting from still, issue full power pulse this long to get motors started
00138       _stopTime = 3000;  // Pause at least this long after emergency stop before restarting
00139 
00140       _msgCount=9;
00141       emergencyStop();
00142   }     
00143 
00144 
00145   virtual void setCommandTimeout(const int ms) { _deadTime = ms; }
00146   void setDecelRate(const int msFromFullSpeed) { _decel=msFromFullSpeed; }
00147   void setStartPulseDuration(const int ms) { _startupTime=ms; }
00148   void setStopTimeout(const int ms) { _stopTime=ms; }
00149   void showDiagnostics(const int n) { _msgCount=n; }
00150   void showState()
00151   {
00152     //BYTE IN1, IN2;  // forward/reverse selectors.
00153     //BYTE EN;        // enable pin
00154     //SHORT _speed;     // current speed
00155     //SHORT _speedCmd;  // commanded speed
00156     DiagSerial.printf("Decel %d ms from ful speed\r\n",_decel);
00157     //BYTE _mode;
00158     //unsigned long _doneTime;  // time when mode automatically transitions
00159     DiagSerial.printf("Deadman Timeout %d ms\r\n",_deadTime);
00160   //SHORT _maxPWM;      // clip PWM commands to this magnitude
00161   //SHORT _startupTime; // ms of full-power pulse to start from dead stop
00162   //SHORT _stopTime;    // ms to lock-out commands after emergency stop
00163 
00164   }
00165   
00166   virtual void stop()
00167   {
00168     _speedCmd=0;
00169     EN.write(0);     // digitalWrite(Pin.EN , 0);
00170     IN1.write(0);    // digitalWrite(Pin.IN1, 0);
00171     IN2.write(0);    // digitalWrite(Pin.IN2, 0);
00172     EN.write(1);     // digitalWrite(Pin.EN , 1);
00173     int stoppingTime = (int)ABS(_speed * _decel);
00174 if(_msgCount>0){_msgCount--;DiagSerial.printf("%dms to stop.\r\n",stoppingTime);}
00175     _doneTime = millis() + stoppingTime;
00176     //speed=0;  don't clobber command in case of direction change
00177     _mode = MOTOR_STOPPING;
00178   }
00179 
00180   virtual void emergencyStop()
00181   {
00182     DiagSerial.puts("Emergency ");
00183     _msgCount = 11;  // turn on diagnostics for a few commands
00184     stop();
00185     _speedCmd=0;
00186     _doneTime += _stopTime;
00187   }
00188 
00189 
00190   // Set speed -MAX_PWM for max reverse, MAX_PWM for max forward
00191   virtual void setSpeed(const float spdReq, int t)
00192   {
00193     BYTE prevMode = _mode;
00194     bool rev;
00195     switch(prevMode)
00196       {
00197       case MOTOR_STOPPING :
00198         _speedCmd = spdReq;
00199         if (t < _doneTime)
00200           {  // make sure things are stopped
00201             IN1.write(0);      // digitalWrite(Pin.IN1,0);
00202             IN2.write(0);      // digitalWrite(Pin.IN2,0);
00203             EN.write(1);       // digitalWrite(Pin.EN ,1);
00204             return;
00205           }
00206         // done stoping, continue to STOP mode
00207         _speed = 0;
00208         _mode = MOTOR_STOPPED;
00209 if(_msgCount>0){_msgCount--;DiagSerial.puts("stopped.\r");}
00210       case MOTOR_STOPPED :
00211         if (spdReq == 0) return;  // leave in full brake stop
00212         _mode = (spdReq < 0) ? MOTOR_START_REV : MOTOR_START_FWD;
00213         rev = (_mode == MOTOR_START_REV);
00214         IN1.write(rev?1:0);   // digitalWrite(rev?Pin.IN1:Pin.IN2,1); // don't worry about PWM
00215         IN2.write(rev?0:1);   // digitalWrite(rev?Pin.IN2:Pin.IN1,0); // this is transistional state
00216         EN.write(1);          // digitalWrite(Pin.EN,1);   // hard kick to get started
00217         _doneTime = t + _startupTime;
00218         _speedCmd = spdReq;
00219 if(_msgCount>0){_msgCount--;DiagSerial.printf("Start %s\r\n",rev?"REV":"FWD");}
00220         return;
00221       case MOTOR_FWD :
00222       case MOTOR_REV :
00223         if (t > _doneTime) { emergencyStop(); return; } // deadman expired
00224         if ( (spdReq == 0)  ||  // stop or change direction
00225              ((spdReq < 0) && (prevMode == MOTOR_FWD)) ||
00226              ((spdReq > 0) && (prevMode == MOTOR_REV)) )
00227           {
00228             stop();
00229             _speedCmd = clipPWM(spdReq);
00230             return;
00231           }
00232         setReverse(spdReq < 0);
00233         _speed = _speedCmd = spdReq;
00234         EN.write(getPWM(_speed));      // analogWrite(Pin.EN,getPWM(_speed));
00235         _doneTime = t + _deadTime;
00236 //if(_msgCount>0){_msgCount--;Serial.println(_speed);}
00237         return;
00238       case MOTOR_START_REV :
00239       case MOTOR_START_FWD :
00240         if (spdReq == 0)
00241           {
00242             _speed = 100;  // give it some time to decel, although just starting
00243             stop();
00244             return;
00245           }
00246         if ( ((spdReq < 0) && (_mode == MOTOR_START_FWD)) ||
00247              ((spdReq > 0) && (_mode == MOTOR_START_REV)) )
00248           { // direction change
00249             _speed = 100;  // give it some time to decel, although just starting
00250             stop();
00251             _speedCmd = spdReq;  // go to this speed after coast-down period
00252             return;
00253           }
00254         // same direction, but speed request change
00255         _speed = _speedCmd = spdReq;
00256         if (t >= _doneTime)
00257           {
00258             _mode = (_speedCmd > 0) ? MOTOR_FWD : MOTOR_REV;
00259             _doneTime = t + _deadTime;
00260             setReverse(_mode == MOTOR_REV);  // make sure direction is correct
00261             EN.write(getPWM(_speedCmd));           // analogWrite(Pin.EN,getPWM(_speedCmd));
00262             if(_msgCount>0){_msgCount--;DiagSerial.printf("Started %.3f\r\n",_speedCmd);}
00263           }
00264         return;
00265       }
00266   }
00267 
00268   // update state, but no new command was received
00269   // Check if previous command is complete,
00270   //   and an automatic state transition is needed
00271   virtual void update(int t)  // current time, from millis()
00272   {
00273 //Serial.print(F("Update "));  Serial.println(t);
00274     if ((_doneTime > 0xfffff000) && (t < 999))
00275       {  // time counter must have wrapped around
00276         _doneTime = 0;
00277         CmdSerial.puts("\rClock wrap-around");
00278       }
00279 
00280     BYTE prevMode = _mode;
00281     switch(prevMode)
00282       {
00283       case MOTOR_STOPPING : 
00284       case MOTOR_STOPPED :
00285         if ((t > _doneTime) && _speedCmd)
00286           { // this was a temp stop in a direction change.  Command desired speed.
00287 if(_msgCount>0){_msgCount--;DiagSerial.printf("Restart %.3f\r\n",_speedCmd);}
00288             setSpeed(_speedCmd,t);
00289           }
00290 //else Serial.println("stopped.");
00291         return;
00292       case MOTOR_FWD :
00293       case MOTOR_REV :
00294         if (t > _doneTime) emergencyStop(); // deadman expired
00295         return;
00296       case MOTOR_START_REV :
00297       case MOTOR_START_FWD :
00298         if (t > _doneTime)
00299           {
00300 if(_msgCount>0){_msgCount--;DiagSerial.puts("moving\r\n");}
00301             setSpeed(_speedCmd,t);
00302           }
00303         return;
00304       }
00305   }
00306 
00307 #ifdef DBH1
00308   int getCurrent()
00309   {
00310     return(CS.read());
00311   }
00312 #endif
00313 
00314 };