Aaron Birenboim / Mbed 2 deprecated TankDrive Featured

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

Dependencies:   mbed

Attic/MotorDriveDBH1.h@4:7620d21baef3, 2015-08-16 (annotated)

Committer:
Mr_What
Date:
Sun Aug 16 22:46:25 2015 +0000
Revision:
4:7620d21baef3
Parent:
1:23d0a615756a 
seems to be running from app with reduced diagnostics

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mr_What 1:23d0a615756a 1 /*
Mr_What 1:23d0a615756a 2
Mr_What 1:23d0a615756a 3 Motor driver for DBH-1A/B/C series Motor controler (aliexpress, WINGXINE).
Mr_What 1:23d0a615756a 4 This controller has two inputs for each H-Bridge, and an enable pin,
Mr_What 1:23d0a615756a 5 and a current-sense.
Mr_What 1:23d0a615756a 6 It also provides a 5v-ish output appropriate for running the MCU
Mr_What 1:23d0a615756a 7
Mr_What 1:23d0a615756a 8 It was found that motors seemed to run a bit faster and more efficient
Mr_What 1:23d0a615756a 9 with PWM on the Enable. However, since spec claims only 0-98% PWM,
Mr_What 1:23d0a615756a 10 We still wish to PWM the direction enable. There might be a charge
Mr_What 1:23d0a615756a 11 pump someplace that needs some down time.
Mr_What 1:23d0a615756a 12
Mr_What 1:23d0a615756a 13 It seems to have a hard brake when both inputs set to 0, and enabled.
Mr_What 1:23d0a615756a 14 However, it does seem to coast when enable is 0, inputs don't matter in this state.
Mr_What 1:23d0a615756a 15
Mr_What 1:23d0a615756a 16 The goal of this driver is to PWM (mostly) EN when driving, but
Mr_What 1:23d0a615756a 17 hard-brake on speed 0 and speed 0 transitions.
Mr_What 1:23d0a615756a 18
Mr_What 1:23d0a615756a 19 */
Mr_What 1:23d0a615756a 20
Mr_What 1:23d0a615756a 21 // Don't allow immediate change of direction.
Mr_What 1:23d0a615756a 22 // let it "brake" for a little bit before starting in other direction
Mr_What 1:23d0a615756a 23 #define MOTOR_STOP 0
Mr_What 1:23d0a615756a 24 #define MOTOR_2STOP 1 // 1's is transition state indicator bit
Mr_What 1:23d0a615756a 25 #define MOTOR_FWD 4
Mr_What 1:23d0a615756a 26 #define MOTOR_REV 8
Mr_What 1:23d0a615756a 27 #define MOTOR_2FWD 5
Mr_What 1:23d0a615756a 28 #define MOTOR_2REV 9
Mr_What 1:23d0a615756a 29
Mr_What 1:23d0a615756a 30 #ifndef ABS
Mr_What 1:23d0a615756a 31 #define ABS(x) (((x)<0)?(-(x)):(x))
Mr_What 1:23d0a615756a 32 #endif
Mr_What 1:23d0a615756a 33
Mr_What 1:23d0a615756a 34 // ----------- Global state classes:
Mr_What 1:23d0a615756a 35 #include "MotorDriveBase.h"
Mr_What 1:23d0a615756a 36 class MotorDrive : public MotorDriveBase
Mr_What 1:23d0a615756a 37 {
Mr_What 1:23d0a615756a 38 protected:
Mr_What 1:23d0a615756a 39 static inline float getPWM(float pwm)
Mr_What 1:23d0a615756a 40 {
Mr_What 1:23d0a615756a 41 pwm = ABS(pwm);
Mr_What 1:23d0a615756a 42 if (pwm > 1.0f) return(1.0f);
Mr_What 1:23d0a615756a 43 if (pwm <= 0.0f) return(0.0f);
Mr_What 1:23d0a615756a 44 return(pwm);
Mr_What 1:23d0a615756a 45 }
Mr_What 1:23d0a615756a 46
Mr_What 1:23d0a615756a 47 void setReverse(bool rev) // true==set reverse direction, false==forward
Mr_What 1:23d0a615756a 48 {
Mr_What 1:23d0a615756a 49 if (rev)
Mr_What 1:23d0a615756a 50 {
Mr_What 1:23d0a615756a 51 //digitalWrite(Pin.IN2, 0 );
Mr_What 1:23d0a615756a 52 //analogWrite( Pin.IN1, _maxPWM);
Mr_What 1:23d0a615756a 53 IN2.write(0.0f);
Mr_What 1:23d0a615756a 54 IN1.write(_maxPWM);
Mr_What 1:23d0a615756a 55 }
Mr_What 1:23d0a615756a 56 else
Mr_What 1:23d0a615756a 57 {
Mr_What 1:23d0a615756a 58 //digitalWrite(Pin.IN1, 0 );
Mr_What 1:23d0a615756a 59 //analogWrite( Pin.IN2, _maxPWM);
Mr_What 1:23d0a615756a 60 IN1.write(0.0f);
Mr_What 1:23d0a615756a 61 IN2.write(_maxPWM);
Mr_What 1:23d0a615756a 62 }
Mr_What 1:23d0a615756a 63 }
Mr_What 1:23d0a615756a 64
Mr_What 1:23d0a615756a 65 public:
Mr_What 1:23d0a615756a 66 // it does not seem to hurt to have IN1=IN2=1, but it doesn't seem
Mr_What 1:23d0a615756a 67 // to do anything different/useful, so try to avoid this state.
Mr_What 1:23d0a615756a 68 //struct {
Mr_What 1:23d0a615756a 69 PwmOut IN1, IN2; // forward/reverse selectors.
Mr_What 1:23d0a615756a 70 PwmOut EN; // enable pin
Mr_What 1:23d0a615756a 71 AnalogIn CS; // current sense analog input pin
Mr_What 1:23d0a615756a 72 //} Pin;
Mr_What 1:23d0a615756a 73
Mr_What 1:23d0a615756a 74 float _speed; // current speed 0..1
Mr_What 1:23d0a615756a 75 float _decel; // time to allow to stop, in ms / PWM frac
Mr_What 1:23d0a615756a 76 unsigned char _mode;
Mr_What 1:23d0a615756a 77 int _modeDoneTime;
Mr_What 1:23d0a615756a 78 int _deadTime, _startupTime, _stopTime;
Mr_What 1:23d0a615756a 79 float _minPWM, _maxPWM;
Mr_What 1:23d0a615756a 80 int _msgCount;
Mr_What 1:23d0a615756a 81
Mr_What 1:23d0a615756a 82 MotorDrive(PinName namEN, PinName namIN1, PinName namIN2, PinName namCS) :
Mr_What 1:23d0a615756a 83 IN1(namIN1),
Mr_What 1:23d0a615756a 84 IN2(namIN2),
Mr_What 1:23d0a615756a 85 EN( namEN),
Mr_What 1:23d0a615756a 86 CS( namCS)
Mr_What 1:23d0a615756a 87 {
Mr_What 1:23d0a615756a 88 EN.write(0.0f);
Mr_What 1:23d0a615756a 89 IN2.write(0.0f);
Mr_What 1:23d0a615756a 90 IN2.write(0.0f);
Mr_What 1:23d0a615756a 91
Mr_What 1:23d0a615756a 92 IN1.period(0.001f); // these can be audible. They are just the little 98% limits the driver demands
Mr_What 1:23d0a615756a 93 IN2.period(0.001f);
Mr_What 1:23d0a615756a 94 EN.period(1.0f/8000.0f); // may want this higher frequency to make it ultrasonic
Mr_What 1:23d0a615756a 95
Mr_What 1:23d0a615756a 96 _deadTime = 250; // emergency stop if no command update in this time interval
Mr_What 1:23d0a615756a 97 _minPWM = 0.004f; // Motors won't move below this level
Mr_What 1:23d0a615756a 98 _maxPWM = 0.98f; // don't go over this PWM level. driver can't do it
Mr_What 1:23d0a615756a 99 _startupTime = 5; // when starting from still, issue full power pulse this long to get motors started
Mr_What 1:23d0a615756a 100 _stopTime = 3000; // Pause at least this long after emergency stop before restarting
Mr_What 1:23d0a615756a 101 _decel = 0.02f; // frac. of fullspeed/ms decel time allowance
Mr_What 1:23d0a615756a 102
Mr_What 1:23d0a615756a 103 _msgCount = 11; // issue this many diagnostic messages before going quiet
Mr_What 1:23d0a615756a 104
Mr_What 1:23d0a615756a 105 emergencyStop();
Mr_What 1:23d0a615756a 106 }
Mr_What 1:23d0a615756a 107
Mr_What 1:23d0a615756a 108 void setCommandTimeout(int dt)
Mr_What 1:23d0a615756a 109 {
Mr_What 1:23d0a615756a 110 _deadTime = dt;
Mr_What 1:23d0a615756a 111 cSerial.print("\r\nDeadman timeout ");cSerial.print(_deadTime);cSerial.println(" ms");
Mr_What 1:23d0a615756a 112 }
Mr_What 1:23d0a615756a 113
Mr_What 1:23d0a615756a 114 virtual void stop()
Mr_What 1:23d0a615756a 115 {
Mr_What 1:23d0a615756a 116 //analogWrite(Pin.IN1, 0);
Mr_What 1:23d0a615756a 117 //analogWrite(Pin.IN2, 0);
Mr_What 1:23d0a615756a 118 //abalogWrite(Pin.EN , 0);
Mr_What 1:23d0a615756a 119 EN.write(0.0f);
Mr_What 1:23d0a615756a 120 IN1.write(0.0f);
Mr_What 1:23d0a615756a 121 IN2.write(0.0f);
Mr_What 1:23d0a615756a 122 EN.write(1.0f);
Mr_What 1:23d0a615756a 123
Mr_What 1:23d0a615756a 124 int stoppingTime = (int)ABS(_speed * _decel);
Mr_What 1:23d0a615756a 125 if(_msgCount>0){_msgCount--;cSerial.print(stoppingTime);cSerial.println(" ms to stop.");}
Mr_What 1:23d0a615756a 126 _modeDoneTime = millis() + stoppingTime;
Mr_What 1:23d0a615756a 127 //speed=0; don't clobber command in case of direction change
Mr_What 1:23d0a615756a 128 _mode = MOTOR_2STOP;
Mr_What 1:23d0a615756a 129 }
Mr_What 1:23d0a615756a 130
Mr_What 1:23d0a615756a 131 virtual void emergencyStop()
Mr_What 1:23d0a615756a 132 {
Mr_What 1:23d0a615756a 133 cSerial.print("Emergency ");
Mr_What 1:23d0a615756a 134 _msgCount = 11; // turn on diagnostics for a few commands
Mr_What 1:23d0a615756a 135 stop();
Mr_What 1:23d0a615756a 136 _speed=0;
Mr_What 1:23d0a615756a 137 _modeDoneTime += _stopTime;
Mr_What 1:23d0a615756a 138 }
Mr_What 1:23d0a615756a 139 /*
Mr_What 1:23d0a615756a 140 void begin(const int en, const int in1, const int in2, const int cs,
Mr_What 1:23d0a615756a 141 const float de=2.0)
Mr_What 1:23d0a615756a 142 {
Mr_What 1:23d0a615756a 143 pinMode(en,OUTPUT); digitalWrite(en,0);
Mr_What 1:23d0a615756a 144 Pin.EN =en;
Mr_What 1:23d0a615756a 145 Pin.IN1 =in1;
Mr_What 1:23d0a615756a 146 Pin.IN2 =in2;
Mr_What 1:23d0a615756a 147 Pin.CS =cs;
Mr_What 1:23d0a615756a 148 _decel=de;
Mr_What 1:23d0a615756a 149
Mr_What 1:23d0a615756a 150 pinMode(Pin.IN1,OUTPUT); digitalWrite(Pin.IN1,0);
Mr_What 1:23d0a615756a 151 pinMode(Pin.IN2,OUTPUT); digitalWrite(Pin.IN2,0);
Mr_What 1:23d0a615756a 152
Mr_What 1:23d0a615756a 153 emergencyStop();
Mr_What 1:23d0a615756a 154 }
Mr_What 1:23d0a615756a 155 */
Mr_What 1:23d0a615756a 156
Mr_What 1:23d0a615756a 157 // Set speed -MAX_PWM for max reverse, MAX_PWM for max forward
Mr_What 1:23d0a615756a 158 virtual void setSpeed(const float spdReq, const long t)
Mr_What 1:23d0a615756a 159 {
Mr_What 1:23d0a615756a 160 unsigned char prevMode = _mode;
Mr_What 1:23d0a615756a 161 bool rev;
Mr_What 1:23d0a615756a 162 switch(prevMode)
Mr_What 1:23d0a615756a 163 {
Mr_What 1:23d0a615756a 164 case MOTOR_2STOP :
Mr_What 1:23d0a615756a 165 if(_msgCount>0){_msgCount--;cSerial.print("2STOP-->");}
Mr_What 1:23d0a615756a 166 if (t < _modeDoneTime)
Mr_What 1:23d0a615756a 167 { // make sure things are stopped
Mr_What 1:23d0a615756a 168 // digitalWrite(Pin.IN1,0);
Mr_What 1:23d0a615756a 169 // digitalWrite(Pin.IN2,0);
Mr_What 1:23d0a615756a 170 // digitalWrite(Pin.EN ,0);
Mr_What 1:23d0a615756a 171 EN.write(0.0f);
Mr_What 1:23d0a615756a 172 IN1.write(0.0f);
Mr_What 1:23d0a615756a 173 IN2.write(0.0f);
Mr_What 1:23d0a615756a 174 EN.write(1.0f);
Mr_What 1:23d0a615756a 175 return;
Mr_What 1:23d0a615756a 176 }
Mr_What 1:23d0a615756a 177 // done stoping, continue to STOP mode
Mr_What 1:23d0a615756a 178 _mode = MOTOR_STOP;
Mr_What 1:23d0a615756a 179 if(_msgCount>0){_msgCount--;cSerial.println("stopped.");}
Mr_What 1:23d0a615756a 180 case MOTOR_STOP :
Mr_What 1:23d0a615756a 181 if(_msgCount>0){_msgCount--;cSerial.print("STOP-->");}
Mr_What 1:23d0a615756a 182 if (ABS(spdReq) < _minPWM) return;
Mr_What 1:23d0a615756a 183 _mode = (spdReq < 0) ? MOTOR_2REV : MOTOR_2FWD;
Mr_What 1:23d0a615756a 184 rev = (_mode == MOTOR_2REV);
Mr_What 1:23d0a615756a 185 //digitalWrite(rev?Pin.IN1:Pin.IN2,1); // don't worry about PWM
Mr_What 1:23d0a615756a 186 //digitalWrite(rev?Pin.IN2:Pin.IN1,0); // this is transistional state
Mr_What 1:23d0a615756a 187 //digitalWrite(Pin.EN,1); // hard kick to get started
Mr_What 1:23d0a615756a 188 IN1.write(rev?1.0f:0.0f);
Mr_What 1:23d0a615756a 189 IN2.write(rev?0.0f:1.0f);
Mr_What 1:23d0a615756a 190 EN.write(1.0f); // hard kick to get started
Mr_What 1:23d0a615756a 191 _modeDoneTime = t + _startupTime;
Mr_What 1:23d0a615756a 192 _speed = spdReq;
Mr_What 1:23d0a615756a 193 if(_msgCount>0){_msgCount--;cSerial.print("Start ");cSerial.println(rev ? "REV" : "FWD");}
Mr_What 1:23d0a615756a 194 return;
Mr_What 1:23d0a615756a 195 case MOTOR_FWD :
Mr_What 1:23d0a615756a 196 case MOTOR_REV :
Mr_What 1:23d0a615756a 197 if(_msgCount>0){_msgCount--;cSerial.print(_speed);cSerial.print("-->");cSerial.println(spdReq);}
Mr_What 1:23d0a615756a 198 setReverse(_mode == MOTOR_2REV);
Mr_What 1:23d0a615756a 199 if (t > _modeDoneTime) { emergencyStop(); return; } // deadman expired
Mr_What 1:23d0a615756a 200 if ( (ABS(spdReq) < _minPWM) || // stop or change direction
Mr_What 1:23d0a615756a 201 ((spdReq < 0) && (prevMode == MOTOR_FWD)) ||
Mr_What 1:23d0a615756a 202 ((spdReq > 0) && (prevMode == MOTOR_REV)) )
Mr_What 1:23d0a615756a 203 {
Mr_What 1:23d0a615756a 204 stop();
Mr_What 1:23d0a615756a 205 // set speed so that it goes to this speed after coast-down
Mr_What 1:23d0a615756a 206 _speed = (ABS(spdReq) < _minPWM) ? 0 : spdReq;
Mr_What 1:23d0a615756a 207 return;
Mr_What 1:23d0a615756a 208 }
Mr_What 1:23d0a615756a 209 _speed = spdReq;
Mr_What 1:23d0a615756a 210 //analogWrite(EN,getPWM(_speed));
Mr_What 1:23d0a615756a 211 EN.write(_speed);
Mr_What 1:23d0a615756a 212 _modeDoneTime = t + _deadTime;
Mr_What 1:23d0a615756a 213 if(_msgCount>0){_msgCount--;cSerial.println(_speed);}
Mr_What 1:23d0a615756a 214 return;
Mr_What 1:23d0a615756a 215 case MOTOR_2REV :
Mr_What 1:23d0a615756a 216 case MOTOR_2FWD :
Mr_What 1:23d0a615756a 217 if(_msgCount>0){_msgCount--;cSerial.print("start");cSerial.println(spdReq);}
Mr_What 1:23d0a615756a 218 if (ABS(spdReq) < _minPWM)
Mr_What 1:23d0a615756a 219 {
Mr_What 1:23d0a615756a 220 _speed = 100; // give it some time to decel, although just starting
Mr_What 1:23d0a615756a 221 stop();
Mr_What 1:23d0a615756a 222 _speed = 0;
Mr_What 1:23d0a615756a 223 return;
Mr_What 1:23d0a615756a 224 }
Mr_What 1:23d0a615756a 225 if ( ((spdReq < 0.0f) && (_mode == MOTOR_2FWD)) ||
Mr_What 1:23d0a615756a 226 ((spdReq > 0.0f) && (_mode == MOTOR_2REV)) )
Mr_What 1:23d0a615756a 227 { // direction change
Mr_What 1:23d0a615756a 228 _speed = 0.5f; // give it some time to decel, although just starting
Mr_What 1:23d0a615756a 229 stop();
Mr_What 1:23d0a615756a 230 _speed = spdReq; // go to this speed after coast-down period
Mr_What 1:23d0a615756a 231 return;
Mr_What 1:23d0a615756a 232 }
Mr_What 1:23d0a615756a 233 // same direction, but speed request change
Mr_What 1:23d0a615756a 234 _speed = spdReq;
Mr_What 1:23d0a615756a 235 if (t >= _modeDoneTime)
Mr_What 1:23d0a615756a 236 {
Mr_What 1:23d0a615756a 237 _mode = (_speed > 0) ? MOTOR_FWD : MOTOR_REV;
Mr_What 1:23d0a615756a 238 _modeDoneTime = t + _deadTime;
Mr_What 1:23d0a615756a 239 setReverse(_mode == MOTOR_REV); // make sure direction is correct
Mr_What 1:23d0a615756a 240 EN.write(_speed);
Mr_What 1:23d0a615756a 241 if(_msgCount>0){_msgCount--;cSerial.print("Started ");cSerial.print((int)t);cSerial.print(" done ");cSerial.println(_modeDoneTime);}
Mr_What 1:23d0a615756a 242 }
Mr_What 1:23d0a615756a 243 return;
Mr_What 1:23d0a615756a 244
Mr_What 1:23d0a615756a 245 default:
Mr_What 1:23d0a615756a 246 cSerial.println("unrecognized state");
Mr_What 1:23d0a615756a 247 }
Mr_What 1:23d0a615756a 248 }
Mr_What 1:23d0a615756a 249
Mr_What 1:23d0a615756a 250 // update state, but no new command (no deadman reset)
Mr_What 1:23d0a615756a 251 // Checks if previous command is complete, and an automatic state transition
Mr_What 1:23d0a615756a 252 // is needed
Mr_What 1:23d0a615756a 253 virtual void update(long t) // current time, from millis()
Mr_What 1:23d0a615756a 254 {
Mr_What 1:23d0a615756a 255 //Serial.print(F("Update ")); Serial.println(t);
Mr_What 1:23d0a615756a 256 if ((_modeDoneTime > 0xfffff000) && (t < 999))
Mr_What 1:23d0a615756a 257 { // time counter must have wrapped around
Mr_What 1:23d0a615756a 258 _modeDoneTime = 0;
Mr_What 1:23d0a615756a 259 cSerial.println("Clock wrap-around");
Mr_What 1:23d0a615756a 260 }
Mr_What 1:23d0a615756a 261
Mr_What 1:23d0a615756a 262 unsigned char prevMode = _mode;
Mr_What 1:23d0a615756a 263 switch(prevMode)
Mr_What 1:23d0a615756a 264 {
Mr_What 1:23d0a615756a 265 case MOTOR_2STOP :
Mr_What 1:23d0a615756a 266 case MOTOR_STOP :
Mr_What 1:23d0a615756a 267 if ((t > _modeDoneTime) && _speed)
Mr_What 1:23d0a615756a 268 { // this was a temp stop in a direction change. Command desired speed.
Mr_What 1:23d0a615756a 269 if(_msgCount>0){_msgCount--;cSerial.print("Restart ");cSerial.println(_speed);}
Mr_What 1:23d0a615756a 270 setSpeed(_speed,t);
Mr_What 1:23d0a615756a 271 }
Mr_What 1:23d0a615756a 272 //else Serial.println("stopped.");
Mr_What 1:23d0a615756a 273 return;
Mr_What 1:23d0a615756a 274 case MOTOR_FWD :
Mr_What 1:23d0a615756a 275 case MOTOR_REV :
Mr_What 1:23d0a615756a 276 if (t > _modeDoneTime) emergencyStop(); // deadman expired
Mr_What 1:23d0a615756a 277 return;
Mr_What 1:23d0a615756a 278 case MOTOR_2REV :
Mr_What 1:23d0a615756a 279 case MOTOR_2FWD :
Mr_What 1:23d0a615756a 280 if (t > _modeDoneTime)
Mr_What 1:23d0a615756a 281 {
Mr_What 1:23d0a615756a 282 //mode = (prevMode == MOTOR_2REV) ? MOTOR_REV : MOTOR_FWD;
Mr_What 1:23d0a615756a 283 if(_msgCount>0){_msgCount--;cSerial.println("moving");}
Mr_What 1:23d0a615756a 284 setSpeed(_speed,t);
Mr_What 1:23d0a615756a 285 }
Mr_What 1:23d0a615756a 286 return;
Mr_What 1:23d0a615756a 287 }
Mr_What 1:23d0a615756a 288 }
Mr_What 1:23d0a615756a 289
Mr_What 1:23d0a615756a 290 float getCurrent()
Mr_What 1:23d0a615756a 291 {
Mr_What 1:23d0a615756a 292 //return(analogRead(Pin.CS));
Mr_What 1:23d0a615756a 293 return(CS.read());
Mr_What 1:23d0a615756a 294 }
Mr_What 1:23d0a615756a 295 };