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@3:502f90649834, 2015-08-13 (annotated)
- Committer:
- Mr_What
- Date:
- Thu Aug 13 17:50:28 2015 +0000
- Revision:
- 3:502f90649834
- Parent:
- 1:23d0a615756a
- Child:
- 4:7620d21baef3
seems to be working from terminal, but locks up from app. I guess it can't handle the fast commands.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Mr_What | 1:23d0a615756a | 1 | /* |
Mr_What | 1:23d0a615756a | 2 | Motor driver for Motor controllers using the L298 Dual-full-H-Bridge chip, |
Mr_What | 1:23d0a615756a | 3 | or work alikes. |
Mr_What | 1:23d0a615756a | 4 | This chip generally has two direction inputs and an enable pin. |
Mr_What | 1:23d0a615756a | 5 | Typical use is: |
Mr_What | 1:23d0a615756a | 6 | |
Mr_What | 1:23d0a615756a | 7 | IN1=1, IN2=0, EN=PWM -- reverse at PWM speed |
Mr_What | 1:23d0a615756a | 8 | IN1=0, IN2=1, EN=PWM -- forward at PWM speed |
Mr_What | 1:23d0a615756a | 9 | IN1=0, IN2=0, EN=1 -- electrical brake |
Mr_What | 1:23d0a615756a | 10 | IN1=X, IN2=X, EN=0 -- coast |
Mr_What | 1:23d0a615756a | 11 | |
Mr_What | 1:23d0a615756a | 12 | This controller has two inputs for each H-Bridge, and an enable pin. |
Mr_What | 1:23d0a615756a | 13 | It also provides a 5v-ish OUTPUT appropriate for running the MCU |
Mr_What | 1:23d0a615756a | 14 | |
Mr_What | 1:23d0a615756a | 15 | It was found that motors seemed to run a bit faster and more efficient |
Mr_What | 1:23d0a615756a | 16 | with PWM on the Enable. |
Mr_What | 1:23d0a615756a | 17 | Future versions of this driver may allow user to put the PWM |
Mr_What | 1:23d0a615756a | 18 | on the direction pin which seems to provide a more constant-velocity |
Mr_What | 1:23d0a615756a | 19 | response, where putting PWM on the enable is more like the accelerator |
Mr_What | 1:23d0a615756a | 20 | input on a car. (It has a peak velocity, but until it gets gets close |
Mr_What | 1:23d0a615756a | 21 | to this peak, the input is more closely related to acceleration) |
Mr_What | 1:23d0a615756a | 22 | |
Mr_What | 1:23d0a615756a | 23 | Driver will apply electrical brake when stopping, but |
Mr_What | 1:23d0a615756a | 24 | try to use freewheeling PWM mode when driving. |
Mr_What | 1:23d0a615756a | 25 | |
Mr_What | 1:23d0a615756a | 26 | Aaron Birenboim, http://boim.com 31jul2015 |
Mr_What | 1:23d0a615756a | 27 | Apache License |
Mr_What | 1:23d0a615756a | 28 | */ |
Mr_What | 1:23d0a615756a | 29 | |
Mr_What | 1:23d0a615756a | 30 | #define MAXPWM 0.98f |
Mr_What | 1:23d0a615756a | 31 | |
Mr_What | 1:23d0a615756a | 32 | // motor states (I'm afraid of enum which might be 16 bit on 8-bit MCU) |
Mr_What | 1:23d0a615756a | 33 | #define MOTOR_STOPPED 0 // 1's -- running bit |
Mr_What | 1:23d0a615756a | 34 | #define MOTOR_FWD 1 // 2's -- direction (1==rev) |
Mr_What | 1:23d0a615756a | 35 | #define MOTOR_REV 3 |
Mr_What | 1:23d0a615756a | 36 | #define MOTOR_START_FWD 5 // 4's -- start-up pulse |
Mr_What | 1:23d0a615756a | 37 | #define MOTOR_START_REV 7 |
Mr_What | 1:23d0a615756a | 38 | #define MOTOR_STOPPING 8 // 8 -- electrical braking |
Mr_What | 1:23d0a615756a | 39 | |
Mr_What | 1:23d0a615756a | 40 | #ifndef ABS |
Mr_What | 1:23d0a615756a | 41 | #define ABS(x) (((x)<0)?(-(x)):(x)) |
Mr_What | 1:23d0a615756a | 42 | #endif |
Mr_What | 1:23d0a615756a | 43 | |
Mr_What | 1:23d0a615756a | 44 | typedef char BYTE; // signed char, 8-bit |
Mr_What | 1:23d0a615756a | 45 | typedef short SHORT; // signed int, 16-bit |
Mr_What | 1:23d0a615756a | 46 | typedef int BOOL; |
Mr_What | 1:23d0a615756a | 47 | |
Mr_What | 1:23d0a615756a | 48 | //#ifdef DBH1 |
Mr_What | 1:23d0a615756a | 49 | //#typedef PwmOut DIRPIN; |
Mr_What | 1:23d0a615756a | 50 | //#else |
Mr_What | 1:23d0a615756a | 51 | //#typedef DigitalOut DIRPIN; |
Mr_What | 1:23d0a615756a | 52 | //#else |
Mr_What | 1:23d0a615756a | 53 | |
Mr_What | 1:23d0a615756a | 54 | // ----------- Global state classes: |
Mr_What | 1:23d0a615756a | 55 | #include "MotorDriveBase.h" |
Mr_What | 1:23d0a615756a | 56 | class MotorDrive : public MotorDriveBase |
Mr_What | 1:23d0a615756a | 57 | { |
Mr_What | 1:23d0a615756a | 58 | protected: |
Mr_What | 1:23d0a615756a | 59 | inline float clipPWM(float pwm) |
Mr_What | 1:23d0a615756a | 60 | { |
Mr_What | 1:23d0a615756a | 61 | if (ABS(pwm) > 1.0f) |
Mr_What | 1:23d0a615756a | 62 | pwm = (pwm < 0) ? -1.0f : 1.0f; |
Mr_What | 1:23d0a615756a | 63 | return(pwm); |
Mr_What | 1:23d0a615756a | 64 | } |
Mr_What | 1:23d0a615756a | 65 | inline float getPWM(float pwm) |
Mr_What | 1:23d0a615756a | 66 | { |
Mr_What | 1:23d0a615756a | 67 | return(ABS(clipPWM(pwm))); |
Mr_What | 1:23d0a615756a | 68 | } |
Mr_What | 1:23d0a615756a | 69 | |
Mr_What | 1:23d0a615756a | 70 | void setReverse(BOOL rev) // true==set reverse direction, false==forward |
Mr_What | 1:23d0a615756a | 71 | { |
Mr_What | 1:23d0a615756a | 72 | if (rev) |
Mr_What | 1:23d0a615756a | 73 | { |
Mr_What | 1:23d0a615756a | 74 | IN2.write(0); // digitalWrite(Pin.IN2,0); |
Mr_What | 1:23d0a615756a | 75 | #ifdef DBH1 |
Mr_What | 1:23d0a615756a | 76 | IN1.write(MAXPWM); // digitalWrite(Pin.IN1,1); |
Mr_What | 1:23d0a615756a | 77 | #else |
Mr_What | 1:23d0a615756a | 78 | IN1.write(1); // analogWrite(Pin.IN1,250); |
Mr_What | 1:23d0a615756a | 79 | #endif |
Mr_What | 1:23d0a615756a | 80 | } |
Mr_What | 1:23d0a615756a | 81 | else |
Mr_What | 1:23d0a615756a | 82 | { |
Mr_What | 1:23d0a615756a | 83 | IN1.write(0); // digitalWrite(Pin.IN1,0); |
Mr_What | 1:23d0a615756a | 84 | #ifdef DBH1 |
Mr_What | 1:23d0a615756a | 85 | IN2.write(MAXPWM); // digitalWrite(Pin.IN2,1); |
Mr_What | 1:23d0a615756a | 86 | #else |
Mr_What | 1:23d0a615756a | 87 | IN2.write(1); // analogWrite(Pin.IN2,250); |
Mr_What | 1:23d0a615756a | 88 | #endif |
Mr_What | 1:23d0a615756a | 89 | } |
Mr_What | 1:23d0a615756a | 90 | } |
Mr_What | 1:23d0a615756a | 91 | |
Mr_What | 1:23d0a615756a | 92 | public: |
Mr_What | 1:23d0a615756a | 93 | // struct { |
Mr_What | 1:23d0a615756a | 94 | // it does not seem to hurt to have IN1=IN2=1, but it doesn't seem |
Mr_What | 1:23d0a615756a | 95 | // to do anything different/useful, so try to avoid this state. |
Mr_What | 1:23d0a615756a | 96 | |
Mr_What | 1:23d0a615756a | 97 | #ifdef DBH1 |
Mr_What | 1:23d0a615756a | 98 | PwmOut IN1, IN2; // forward/reverse selectors. |
Mr_What | 1:23d0a615756a | 99 | AnalogIn CS; |
Mr_What | 1:23d0a615756a | 100 | #else |
Mr_What | 1:23d0a615756a | 101 | DigitalOut IN1, IN2; // forward/reverse selectors. |
Mr_What | 1:23d0a615756a | 102 | #endif |
Mr_What | 1:23d0a615756a | 103 | PwmOut EN; // enable pin |
Mr_What | 1:23d0a615756a | 104 | // } Pin; |
Mr_What | 1:23d0a615756a | 105 | float _speed; // current speed |
Mr_What | 1:23d0a615756a | 106 | float _speedCmd; // commanded speed |
Mr_What | 1:23d0a615756a | 107 | |
Mr_What | 1:23d0a615756a | 108 | int _decel; // time to allow to stop, in ms from full speed |
Mr_What | 1:23d0a615756a | 109 | BYTE _mode; |
Mr_What | 1:23d0a615756a | 110 | int _doneTime; // time when mode automatically transitions |
Mr_What | 1:23d0a615756a | 111 | int _deadTime; // ms until deadman transition to emergency stop |
Mr_What | 1:23d0a615756a | 112 | int _startupTime; // ms of full-power pulse to start from dead stop |
Mr_What | 1:23d0a615756a | 113 | int _stopTime; // ms to lock-out commands after emergency stop |
Mr_What | 1:23d0a615756a | 114 | |
Mr_What | 1:23d0a615756a | 115 | int _msgCount; // turn off diagnostics after this many messages |
Mr_What | 1:23d0a615756a | 116 | |
Mr_What | 1:23d0a615756a | 117 | MotorDrive (PinName namEN, PinName namIN1, PinName namIN2, PinName namCS) : |
Mr_What | 1:23d0a615756a | 118 | IN1(namIN1), |
Mr_What | 1:23d0a615756a | 119 | IN2(namIN2), |
Mr_What | 1:23d0a615756a | 120 | EN(namEN) |
Mr_What | 1:23d0a615756a | 121 | #ifdef DBH1 |
Mr_What | 1:23d0a615756a | 122 | ,CS(namCS) |
Mr_What | 1:23d0a615756a | 123 | #endif |
Mr_What | 1:23d0a615756a | 124 | { |
Mr_What | 1:23d0a615756a | 125 | EN.write(0.0f); |
Mr_What | 1:23d0a615756a | 126 | IN2.write(0.0f); |
Mr_What | 1:23d0a615756a | 127 | IN2.write(0.0f); |
Mr_What | 1:23d0a615756a | 128 | |
Mr_What | 1:23d0a615756a | 129 | IN1.period(0.001f); // these can be audible. They are just the little 98% limits the driver demands |
Mr_What | 1:23d0a615756a | 130 | IN2.period(0.001f); |
Mr_What | 1:23d0a615756a | 131 | EN.period(1.0f/8000.0f); // may want this higher frequency to make it ultrasonic |
Mr_What | 1:23d0a615756a | 132 | |
Mr_What | 1:23d0a615756a | 133 | _speed = _speedCmd = 0.0f; |
Mr_What | 1:23d0a615756a | 134 | |
Mr_What | 1:23d0a615756a | 135 | _decel = 500; |
Mr_What | 1:23d0a615756a | 136 | _deadTime = 250; // emergency stop if no command update in this many ms |
Mr_What | 1:23d0a615756a | 137 | _startupTime = 50; // when starting from still, issue full power pulse this long to get motors started |
Mr_What | 1:23d0a615756a | 138 | _stopTime = 3000; // Pause at least this long after emergency stop before restarting |
Mr_What | 1:23d0a615756a | 139 | |
Mr_What | 1:23d0a615756a | 140 | _msgCount=9; |
Mr_What | 1:23d0a615756a | 141 | emergencyStop(); |
Mr_What | 1:23d0a615756a | 142 | } |
Mr_What | 1:23d0a615756a | 143 | |
Mr_What | 1:23d0a615756a | 144 | |
Mr_What | 1:23d0a615756a | 145 | virtual void setCommandTimeout(const int ms) { _deadTime = ms; } |
Mr_What | 1:23d0a615756a | 146 | void setDecelRate(const int msFromFullSpeed) { _decel=msFromFullSpeed; } |
Mr_What | 1:23d0a615756a | 147 | void setStartPulseDuration(const int ms) { _startupTime=ms; } |
Mr_What | 1:23d0a615756a | 148 | void setStopTimeout(const int ms) { _stopTime=ms; } |
Mr_What | 1:23d0a615756a | 149 | void showDiagnostics(const int n) { _msgCount=n; } |
Mr_What | 1:23d0a615756a | 150 | void showState() |
Mr_What | 1:23d0a615756a | 151 | { |
Mr_What | 1:23d0a615756a | 152 | //BYTE IN1, IN2; // forward/reverse selectors. |
Mr_What | 1:23d0a615756a | 153 | //BYTE EN; // enable pin |
Mr_What | 1:23d0a615756a | 154 | //SHORT _speed; // current speed |
Mr_What | 1:23d0a615756a | 155 | //SHORT _speedCmd; // commanded speed |
Mr_What | 1:23d0a615756a | 156 | CmdSerial.printf("Decel %d ms from ful speed\r\n",_decel); |
Mr_What | 1:23d0a615756a | 157 | //BYTE _mode; |
Mr_What | 1:23d0a615756a | 158 | //unsigned long _doneTime; // time when mode automatically transitions |
Mr_What | 1:23d0a615756a | 159 | CmdSerial.printf("Deadman Timeout %d ms\r\n",_deadTime); |
Mr_What | 1:23d0a615756a | 160 | //SHORT _maxPWM; // clip PWM commands to this magnitude |
Mr_What | 1:23d0a615756a | 161 | //SHORT _startupTime; // ms of full-power pulse to start from dead stop |
Mr_What | 1:23d0a615756a | 162 | //SHORT _stopTime; // ms to lock-out commands after emergency stop |
Mr_What | 1:23d0a615756a | 163 | |
Mr_What | 1:23d0a615756a | 164 | } |
Mr_What | 1:23d0a615756a | 165 | |
Mr_What | 1:23d0a615756a | 166 | virtual void stop() |
Mr_What | 1:23d0a615756a | 167 | { |
Mr_What | 1:23d0a615756a | 168 | _speedCmd=0; |
Mr_What | 1:23d0a615756a | 169 | EN.write(0); // digitalWrite(Pin.EN , 0); |
Mr_What | 1:23d0a615756a | 170 | IN1.write(0); // digitalWrite(Pin.IN1, 0); |
Mr_What | 1:23d0a615756a | 171 | IN2.write(0); // digitalWrite(Pin.IN2, 0); |
Mr_What | 1:23d0a615756a | 172 | EN.write(1); // digitalWrite(Pin.EN , 1); |
Mr_What | 1:23d0a615756a | 173 | int stoppingTime = (int)ABS(_speed * _decel); |
Mr_What | 3:502f90649834 | 174 | if(_msgCount>0){_msgCount--;DiagSerial.printf("%dms to stop.\r\n",stoppingTime);} |
Mr_What | 1:23d0a615756a | 175 | _doneTime = millis() + stoppingTime; |
Mr_What | 1:23d0a615756a | 176 | //speed=0; don't clobber command in case of direction change |
Mr_What | 1:23d0a615756a | 177 | _mode = MOTOR_STOPPING; |
Mr_What | 1:23d0a615756a | 178 | } |
Mr_What | 1:23d0a615756a | 179 | |
Mr_What | 1:23d0a615756a | 180 | virtual void emergencyStop() |
Mr_What | 1:23d0a615756a | 181 | { |
Mr_What | 1:23d0a615756a | 182 | CmdSerial.puts("Emergency "); |
Mr_What | 1:23d0a615756a | 183 | _msgCount = 11; // turn on diagnostics for a few commands |
Mr_What | 1:23d0a615756a | 184 | stop(); |
Mr_What | 1:23d0a615756a | 185 | _speedCmd=0; |
Mr_What | 1:23d0a615756a | 186 | _doneTime += _stopTime; |
Mr_What | 1:23d0a615756a | 187 | } |
Mr_What | 1:23d0a615756a | 188 | |
Mr_What | 1:23d0a615756a | 189 | |
Mr_What | 1:23d0a615756a | 190 | // Set speed -MAX_PWM for max reverse, MAX_PWM for max forward |
Mr_What | 1:23d0a615756a | 191 | virtual void setSpeed(const float spdReq, int t) |
Mr_What | 1:23d0a615756a | 192 | { |
Mr_What | 1:23d0a615756a | 193 | BYTE prevMode = _mode; |
Mr_What | 1:23d0a615756a | 194 | bool rev; |
Mr_What | 1:23d0a615756a | 195 | switch(prevMode) |
Mr_What | 1:23d0a615756a | 196 | { |
Mr_What | 1:23d0a615756a | 197 | case MOTOR_STOPPING : |
Mr_What | 1:23d0a615756a | 198 | _speedCmd = spdReq; |
Mr_What | 1:23d0a615756a | 199 | if (t < _doneTime) |
Mr_What | 1:23d0a615756a | 200 | { // make sure things are stopped |
Mr_What | 1:23d0a615756a | 201 | IN1.write(0); // digitalWrite(Pin.IN1,0); |
Mr_What | 1:23d0a615756a | 202 | IN2.write(0); // digitalWrite(Pin.IN2,0); |
Mr_What | 1:23d0a615756a | 203 | EN.write(1); // digitalWrite(Pin.EN ,1); |
Mr_What | 1:23d0a615756a | 204 | return; |
Mr_What | 1:23d0a615756a | 205 | } |
Mr_What | 1:23d0a615756a | 206 | // done stoping, continue to STOP mode |
Mr_What | 1:23d0a615756a | 207 | _speed = 0; |
Mr_What | 1:23d0a615756a | 208 | _mode = MOTOR_STOPPED; |
Mr_What | 3:502f90649834 | 209 | if(_msgCount>0){_msgCount--;DiagSerial.puts("stopped.\r");} |
Mr_What | 1:23d0a615756a | 210 | case MOTOR_STOPPED : |
Mr_What | 1:23d0a615756a | 211 | if (spdReq == 0) return; // leave in full brake stop |
Mr_What | 1:23d0a615756a | 212 | _mode = (spdReq < 0) ? MOTOR_START_REV : MOTOR_START_FWD; |
Mr_What | 1:23d0a615756a | 213 | rev = (_mode == MOTOR_START_REV); |
Mr_What | 1:23d0a615756a | 214 | IN1.write(rev?1:0); // digitalWrite(rev?Pin.IN1:Pin.IN2,1); // don't worry about PWM |
Mr_What | 1:23d0a615756a | 215 | IN2.write(rev?0:1); // digitalWrite(rev?Pin.IN2:Pin.IN1,0); // this is transistional state |
Mr_What | 1:23d0a615756a | 216 | EN.write(1); // digitalWrite(Pin.EN,1); // hard kick to get started |
Mr_What | 1:23d0a615756a | 217 | _doneTime = t + _startupTime; |
Mr_What | 1:23d0a615756a | 218 | _speedCmd = spdReq; |
Mr_What | 3:502f90649834 | 219 | if(_msgCount>0){_msgCount--;DiagSerial.printf("Start %s\r\n",rev?"REV":"FWD");} |
Mr_What | 1:23d0a615756a | 220 | return; |
Mr_What | 1:23d0a615756a | 221 | case MOTOR_FWD : |
Mr_What | 1:23d0a615756a | 222 | case MOTOR_REV : |
Mr_What | 1:23d0a615756a | 223 | if (t > _doneTime) { emergencyStop(); return; } // deadman expired |
Mr_What | 1:23d0a615756a | 224 | if ( (spdReq == 0) || // stop or change direction |
Mr_What | 1:23d0a615756a | 225 | ((spdReq < 0) && (prevMode == MOTOR_FWD)) || |
Mr_What | 1:23d0a615756a | 226 | ((spdReq > 0) && (prevMode == MOTOR_REV)) ) |
Mr_What | 1:23d0a615756a | 227 | { |
Mr_What | 1:23d0a615756a | 228 | stop(); |
Mr_What | 1:23d0a615756a | 229 | _speedCmd = clipPWM(spdReq); |
Mr_What | 1:23d0a615756a | 230 | return; |
Mr_What | 1:23d0a615756a | 231 | } |
Mr_What | 1:23d0a615756a | 232 | setReverse(spdReq < 0); |
Mr_What | 1:23d0a615756a | 233 | _speed = _speedCmd = spdReq; |
Mr_What | 1:23d0a615756a | 234 | EN.write(getPWM(_speed)); // analogWrite(Pin.EN,getPWM(_speed)); |
Mr_What | 1:23d0a615756a | 235 | _doneTime = t + _deadTime; |
Mr_What | 1:23d0a615756a | 236 | //if(_msgCount>0){_msgCount--;Serial.println(_speed);} |
Mr_What | 1:23d0a615756a | 237 | return; |
Mr_What | 1:23d0a615756a | 238 | case MOTOR_START_REV : |
Mr_What | 1:23d0a615756a | 239 | case MOTOR_START_FWD : |
Mr_What | 1:23d0a615756a | 240 | if (spdReq == 0) |
Mr_What | 1:23d0a615756a | 241 | { |
Mr_What | 1:23d0a615756a | 242 | _speed = 100; // give it some time to decel, although just starting |
Mr_What | 1:23d0a615756a | 243 | stop(); |
Mr_What | 1:23d0a615756a | 244 | return; |
Mr_What | 1:23d0a615756a | 245 | } |
Mr_What | 1:23d0a615756a | 246 | if ( ((spdReq < 0) && (_mode == MOTOR_START_FWD)) || |
Mr_What | 1:23d0a615756a | 247 | ((spdReq > 0) && (_mode == MOTOR_START_REV)) ) |
Mr_What | 1:23d0a615756a | 248 | { // direction change |
Mr_What | 1:23d0a615756a | 249 | _speed = 100; // give it some time to decel, although just starting |
Mr_What | 1:23d0a615756a | 250 | stop(); |
Mr_What | 1:23d0a615756a | 251 | _speedCmd = spdReq; // go to this speed after coast-down period |
Mr_What | 1:23d0a615756a | 252 | return; |
Mr_What | 1:23d0a615756a | 253 | } |
Mr_What | 1:23d0a615756a | 254 | // same direction, but speed request change |
Mr_What | 1:23d0a615756a | 255 | _speed = _speedCmd = spdReq; |
Mr_What | 1:23d0a615756a | 256 | if (t >= _doneTime) |
Mr_What | 1:23d0a615756a | 257 | { |
Mr_What | 1:23d0a615756a | 258 | _mode = (_speedCmd > 0) ? MOTOR_FWD : MOTOR_REV; |
Mr_What | 1:23d0a615756a | 259 | _doneTime = t + _deadTime; |
Mr_What | 1:23d0a615756a | 260 | setReverse(_mode == MOTOR_REV); // make sure direction is correct |
Mr_What | 1:23d0a615756a | 261 | EN.write(getPWM(_speedCmd)); // analogWrite(Pin.EN,getPWM(_speedCmd)); |
Mr_What | 1:23d0a615756a | 262 | if(_msgCount>0){_msgCount--;CmdSerial.printf("Started %.3f\r\n",_speedCmd);} |
Mr_What | 1:23d0a615756a | 263 | } |
Mr_What | 1:23d0a615756a | 264 | return; |
Mr_What | 1:23d0a615756a | 265 | } |
Mr_What | 1:23d0a615756a | 266 | } |
Mr_What | 1:23d0a615756a | 267 | |
Mr_What | 1:23d0a615756a | 268 | // update state, but no new command was received |
Mr_What | 1:23d0a615756a | 269 | // Check if previous command is complete, |
Mr_What | 1:23d0a615756a | 270 | // and an automatic state transition is needed |
Mr_What | 1:23d0a615756a | 271 | virtual void update(int t) // current time, from millis() |
Mr_What | 1:23d0a615756a | 272 | { |
Mr_What | 1:23d0a615756a | 273 | //Serial.print(F("Update ")); Serial.println(t); |
Mr_What | 1:23d0a615756a | 274 | if ((_doneTime > 0xfffff000) && (t < 999)) |
Mr_What | 1:23d0a615756a | 275 | { // time counter must have wrapped around |
Mr_What | 1:23d0a615756a | 276 | _doneTime = 0; |
Mr_What | 1:23d0a615756a | 277 | CmdSerial.puts("\rClock wrap-around"); |
Mr_What | 1:23d0a615756a | 278 | } |
Mr_What | 1:23d0a615756a | 279 | |
Mr_What | 1:23d0a615756a | 280 | BYTE prevMode = _mode; |
Mr_What | 1:23d0a615756a | 281 | switch(prevMode) |
Mr_What | 1:23d0a615756a | 282 | { |
Mr_What | 1:23d0a615756a | 283 | case MOTOR_STOPPING : |
Mr_What | 1:23d0a615756a | 284 | case MOTOR_STOPPED : |
Mr_What | 1:23d0a615756a | 285 | if ((t > _doneTime) && _speedCmd) |
Mr_What | 1:23d0a615756a | 286 | { // this was a temp stop in a direction change. Command desired speed. |
Mr_What | 3:502f90649834 | 287 | if(_msgCount>0){_msgCount--;DiagSerial.printf("Restart %.3f\r\n",_speedCmd);} |
Mr_What | 1:23d0a615756a | 288 | setSpeed(_speedCmd,t); |
Mr_What | 1:23d0a615756a | 289 | } |
Mr_What | 1:23d0a615756a | 290 | //else Serial.println("stopped."); |
Mr_What | 1:23d0a615756a | 291 | return; |
Mr_What | 1:23d0a615756a | 292 | case MOTOR_FWD : |
Mr_What | 1:23d0a615756a | 293 | case MOTOR_REV : |
Mr_What | 1:23d0a615756a | 294 | if (t > _doneTime) emergencyStop(); // deadman expired |
Mr_What | 1:23d0a615756a | 295 | return; |
Mr_What | 1:23d0a615756a | 296 | case MOTOR_START_REV : |
Mr_What | 1:23d0a615756a | 297 | case MOTOR_START_FWD : |
Mr_What | 1:23d0a615756a | 298 | if (t > _doneTime) |
Mr_What | 1:23d0a615756a | 299 | { |
Mr_What | 3:502f90649834 | 300 | if(_msgCount>0){_msgCount--;DiagSerial.puts("moving\r\n");} |
Mr_What | 1:23d0a615756a | 301 | setSpeed(_speedCmd,t); |
Mr_What | 1:23d0a615756a | 302 | } |
Mr_What | 1:23d0a615756a | 303 | return; |
Mr_What | 1:23d0a615756a | 304 | } |
Mr_What | 1:23d0a615756a | 305 | } |
Mr_What | 1:23d0a615756a | 306 | |
Mr_What | 1:23d0a615756a | 307 | #ifdef DBH1 |
Mr_What | 1:23d0a615756a | 308 | int getCurrent() |
Mr_What | 1:23d0a615756a | 309 | { |
Mr_What | 1:23d0a615756a | 310 | return(CS.read()); |
Mr_What | 1:23d0a615756a | 311 | } |
Mr_What | 1:23d0a615756a | 312 | #endif |
Mr_What | 1:23d0a615756a | 313 | |
Mr_What | 1:23d0a615756a | 314 | }; |