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
MotorDrive298.h
- Committer:
- Mr_What
- Date:
- 2015-08-13
- Revision:
- 3:502f90649834
- Parent:
- 1:23d0a615756a
- Child:
- 4:7620d21baef3
File content as of revision 3:502f90649834:
/* 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--;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() { 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--;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--;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--;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 };