Basic tank-style drive robot control firmware for Freescale FRDM-K64F. Controls motors on a Dual-Full-H-Bridge with EN, like DBH-1x series, from Bluetooth serial commands
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 + +};