STM32 / Mbed 2 deprecated motorController2

MTR CTRL

Dependencies:   mbed

Dependents:   Nucleo_read_analog_value

Fork of motorController2 by 6408proj

AccelStepper.cpp@3:8d5c0d22d61d, 2017-02-14 (annotated)

Committer:
abckcp
Date:
Tue Feb 14 22:50:03 2017 +0000
Revision:
3:8d5c0d22d61d
Parent:
0:a0391774b0e5 
Nucleo_read_analog_values Alicia

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yweng36 0:a0391774b0e5 1 // AccelStepper.cpp
yweng36 0:a0391774b0e5 2 //
yweng36 0:a0391774b0e5 3 // Copyright (C) 2009-2013 Mike McCauley
yweng36 0:a0391774b0e5 4 // $Id: AccelStepper.cpp,v 1.19 2014/10/31 06:05:27 mikem Exp mikem $
yweng36 0:a0391774b0e5 5
yweng36 0:a0391774b0e5 6 #include "AccelStepper.h"
yweng36 0:a0391774b0e5 7
yweng36 0:a0391774b0e5 8 #if 0
yweng36 0:a0391774b0e5 9 // Some debugging assistance
yweng36 0:a0391774b0e5 10 void dump(uint8_t* p, int l)
yweng36 0:a0391774b0e5 11 {
yweng36 0:a0391774b0e5 12 int i;
yweng36 0:a0391774b0e5 13
yweng36 0:a0391774b0e5 14 for (i = 0; i < l; i++)
yweng36 0:a0391774b0e5 15 {
yweng36 0:a0391774b0e5 16 Serial.print(p[i], HEX);
yweng36 0:a0391774b0e5 17 Serial.print(" ");
yweng36 0:a0391774b0e5 18 }
yweng36 0:a0391774b0e5 19 Serial.println("");
yweng36 0:a0391774b0e5 20 }
yweng36 0:a0391774b0e5 21 #endif
yweng36 0:a0391774b0e5 22
yweng36 0:a0391774b0e5 23 void AccelStepper::moveTo(long absolute)
yweng36 0:a0391774b0e5 24 {
yweng36 0:a0391774b0e5 25 if (_targetPos != absolute)
yweng36 0:a0391774b0e5 26 {
yweng36 0:a0391774b0e5 27 _targetPos = absolute;
yweng36 0:a0391774b0e5 28 computeNewSpeed();
yweng36 0:a0391774b0e5 29 // compute new n?
yweng36 0:a0391774b0e5 30 }
yweng36 0:a0391774b0e5 31 }
yweng36 0:a0391774b0e5 32
yweng36 0:a0391774b0e5 33 void AccelStepper::move(long relative)
yweng36 0:a0391774b0e5 34 {
yweng36 0:a0391774b0e5 35 moveTo(_currentPos + relative);
yweng36 0:a0391774b0e5 36 }
yweng36 0:a0391774b0e5 37
yweng36 0:a0391774b0e5 38 // Implements steps according to the current step interval
yweng36 0:a0391774b0e5 39 // You must call this at least once per step
yweng36 0:a0391774b0e5 40 // returns true if a step occurred
yweng36 0:a0391774b0e5 41 bool AccelStepper::runSpeed()
yweng36 0:a0391774b0e5 42 {
yweng36 0:a0391774b0e5 43 extern Timer t;
yweng36 0:a0391774b0e5 44 // Dont do anything unless we actually have a step interval
yweng36 0:a0391774b0e5 45 if (!_stepInterval)
yweng36 0:a0391774b0e5 46 return false;
yweng36 0:a0391774b0e5 47
yweng36 0:a0391774b0e5 48 //unsigned long time = micros();
yweng36 0:a0391774b0e5 49 unsigned long time = t.read_us();
yweng36 0:a0391774b0e5 50 unsigned long nextStepTime = _lastStepTime + _stepInterval;
yweng36 0:a0391774b0e5 51 // Gymnastics to detect wrapping of either the nextStepTime and/or the current time
yweng36 0:a0391774b0e5 52 if ( ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime)))
yweng36 0:a0391774b0e5 53 || ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime))))
yweng36 0:a0391774b0e5 54 {
yweng36 0:a0391774b0e5 55 if (_direction == DIRECTION_CW)
yweng36 0:a0391774b0e5 56 {
yweng36 0:a0391774b0e5 57 // Clockwise
yweng36 0:a0391774b0e5 58 _currentPos += 1;
yweng36 0:a0391774b0e5 59 }
yweng36 0:a0391774b0e5 60 else
yweng36 0:a0391774b0e5 61 {
yweng36 0:a0391774b0e5 62 // Anticlockwise
yweng36 0:a0391774b0e5 63 _currentPos -= 1;
yweng36 0:a0391774b0e5 64 }
yweng36 0:a0391774b0e5 65 step(_currentPos);
yweng36 0:a0391774b0e5 66
yweng36 0:a0391774b0e5 67 _lastStepTime = time;
yweng36 0:a0391774b0e5 68 return true;
yweng36 0:a0391774b0e5 69 }
yweng36 0:a0391774b0e5 70 else
yweng36 0:a0391774b0e5 71 {
yweng36 0:a0391774b0e5 72 return false;
yweng36 0:a0391774b0e5 73 }
yweng36 0:a0391774b0e5 74 }
yweng36 0:a0391774b0e5 75
yweng36 0:a0391774b0e5 76 long AccelStepper::distanceToGo()
yweng36 0:a0391774b0e5 77 {
yweng36 0:a0391774b0e5 78 return _targetPos - _currentPos;
yweng36 0:a0391774b0e5 79 }
yweng36 0:a0391774b0e5 80
yweng36 0:a0391774b0e5 81 long AccelStepper::targetPosition()
yweng36 0:a0391774b0e5 82 {
yweng36 0:a0391774b0e5 83 return _targetPos;
yweng36 0:a0391774b0e5 84 }
yweng36 0:a0391774b0e5 85
yweng36 0:a0391774b0e5 86 long AccelStepper::currentPosition()
yweng36 0:a0391774b0e5 87 {
yweng36 0:a0391774b0e5 88 return _currentPos;
yweng36 0:a0391774b0e5 89 }
yweng36 0:a0391774b0e5 90
yweng36 0:a0391774b0e5 91 // Useful during initialisations or after initial positioning
yweng36 0:a0391774b0e5 92 // Sets speed to 0
yweng36 0:a0391774b0e5 93 void AccelStepper::setCurrentPosition(long position)
yweng36 0:a0391774b0e5 94 {
yweng36 0:a0391774b0e5 95 _targetPos = _currentPos = position;
yweng36 0:a0391774b0e5 96 _n = 0;
yweng36 0:a0391774b0e5 97 _stepInterval = 0;
yweng36 0:a0391774b0e5 98 }
yweng36 0:a0391774b0e5 99
yweng36 0:a0391774b0e5 100 void AccelStepper::computeNewSpeed()
yweng36 0:a0391774b0e5 101 {
yweng36 0:a0391774b0e5 102 long distanceTo = distanceToGo(); // +ve is clockwise from curent location
yweng36 0:a0391774b0e5 103
yweng36 0:a0391774b0e5 104 long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
yweng36 0:a0391774b0e5 105
yweng36 0:a0391774b0e5 106 if (distanceTo == 0 && stepsToStop <= 1)
yweng36 0:a0391774b0e5 107 {
yweng36 0:a0391774b0e5 108 // We are at the target and its time to stop
yweng36 0:a0391774b0e5 109 _stepInterval = 0;
yweng36 0:a0391774b0e5 110 _speed = 0.0;
yweng36 0:a0391774b0e5 111 _n = 0;
yweng36 0:a0391774b0e5 112 return;
yweng36 0:a0391774b0e5 113 }
yweng36 0:a0391774b0e5 114
yweng36 0:a0391774b0e5 115 if (distanceTo > 0)
yweng36 0:a0391774b0e5 116 {
yweng36 0:a0391774b0e5 117 // We are anticlockwise from the target
yweng36 0:a0391774b0e5 118 // Need to go clockwise from here, maybe decelerate now
yweng36 0:a0391774b0e5 119 if (_n > 0)
yweng36 0:a0391774b0e5 120 {
yweng36 0:a0391774b0e5 121 // Currently accelerating, need to decel now? Or maybe going the wrong way?
yweng36 0:a0391774b0e5 122 if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW)
yweng36 0:a0391774b0e5 123 _n = -stepsToStop; // Start deceleration
yweng36 0:a0391774b0e5 124 }
yweng36 0:a0391774b0e5 125 else if (_n < 0)
yweng36 0:a0391774b0e5 126 {
yweng36 0:a0391774b0e5 127 // Currently decelerating, need to accel again?
yweng36 0:a0391774b0e5 128 if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW)
yweng36 0:a0391774b0e5 129 _n = -_n; // Start accceleration
yweng36 0:a0391774b0e5 130 }
yweng36 0:a0391774b0e5 131 }
yweng36 0:a0391774b0e5 132 else if (distanceTo < 0)
yweng36 0:a0391774b0e5 133 {
yweng36 0:a0391774b0e5 134 // We are clockwise from the target
yweng36 0:a0391774b0e5 135 // Need to go anticlockwise from here, maybe decelerate
yweng36 0:a0391774b0e5 136 if (_n > 0)
yweng36 0:a0391774b0e5 137 {
yweng36 0:a0391774b0e5 138 // Currently accelerating, need to decel now? Or maybe going the wrong way?
yweng36 0:a0391774b0e5 139 if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW)
yweng36 0:a0391774b0e5 140 _n = -stepsToStop; // Start deceleration
yweng36 0:a0391774b0e5 141 }
yweng36 0:a0391774b0e5 142 else if (_n < 0)
yweng36 0:a0391774b0e5 143 {
yweng36 0:a0391774b0e5 144 // Currently decelerating, need to accel again?
yweng36 0:a0391774b0e5 145 if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW)
yweng36 0:a0391774b0e5 146 _n = -_n; // Start accceleration
yweng36 0:a0391774b0e5 147 }
yweng36 0:a0391774b0e5 148 }
yweng36 0:a0391774b0e5 149
yweng36 0:a0391774b0e5 150 // Need to accelerate or decelerate
yweng36 0:a0391774b0e5 151 if (_n == 0)
yweng36 0:a0391774b0e5 152 {
yweng36 0:a0391774b0e5 153 // First step from stopped
yweng36 0:a0391774b0e5 154 _cn = _c0;
yweng36 0:a0391774b0e5 155 _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
yweng36 0:a0391774b0e5 156 }
yweng36 0:a0391774b0e5 157 else
yweng36 0:a0391774b0e5 158 {
yweng36 0:a0391774b0e5 159 // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
yweng36 0:a0391774b0e5 160 _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
yweng36 0:a0391774b0e5 161 _cn = max(_cn, _cmin);
yweng36 0:a0391774b0e5 162 }
yweng36 0:a0391774b0e5 163 _n++;
yweng36 0:a0391774b0e5 164 _stepInterval = _cn;
yweng36 0:a0391774b0e5 165 _speed = 1000000.0 / _cn;
yweng36 0:a0391774b0e5 166 if (_direction == DIRECTION_CCW)
yweng36 0:a0391774b0e5 167 _speed = -_speed;
yweng36 0:a0391774b0e5 168
yweng36 0:a0391774b0e5 169 #if 0
yweng36 0:a0391774b0e5 170 Serial.println(_speed);
yweng36 0:a0391774b0e5 171 Serial.println(_acceleration);
yweng36 0:a0391774b0e5 172 Serial.println(_cn);
yweng36 0:a0391774b0e5 173 Serial.println(_c0);
yweng36 0:a0391774b0e5 174 Serial.println(_n);
yweng36 0:a0391774b0e5 175 Serial.println(_stepInterval);
yweng36 0:a0391774b0e5 176 Serial.println(distanceTo);
yweng36 0:a0391774b0e5 177 Serial.println(stepsToStop);
yweng36 0:a0391774b0e5 178 Serial.println("-----");
yweng36 0:a0391774b0e5 179 #endif
yweng36 0:a0391774b0e5 180 }
yweng36 0:a0391774b0e5 181
yweng36 0:a0391774b0e5 182 // Run the motor to implement speed and acceleration in order to proceed to the target position
yweng36 0:a0391774b0e5 183 // You must call this at least once per step, preferably in your main loop
yweng36 0:a0391774b0e5 184 // If the motor is in the desired position, the cost is very small
yweng36 0:a0391774b0e5 185 // returns true if the motor is still running to the target position.
yweng36 0:a0391774b0e5 186 bool AccelStepper::run()
yweng36 0:a0391774b0e5 187 {
yweng36 0:a0391774b0e5 188 if (runSpeed())
yweng36 0:a0391774b0e5 189 computeNewSpeed();
yweng36 0:a0391774b0e5 190 return _speed != 0.0 || distanceToGo() != 0;
yweng36 0:a0391774b0e5 191 }
yweng36 0:a0391774b0e5 192
yweng36 0:a0391774b0e5 193 AccelStepper::AccelStepper(uint8_t interface, PinName pin1, PinName pin2, PinName pin3, PinName pin4, bool enable)
yweng36 0:a0391774b0e5 194 {
yweng36 0:a0391774b0e5 195 _interface = interface;
yweng36 0:a0391774b0e5 196 _currentPos = 0;
yweng36 0:a0391774b0e5 197 _targetPos = 0;
yweng36 0:a0391774b0e5 198 _speed = 0.0;
yweng36 0:a0391774b0e5 199 _maxSpeed = 1.0;
yweng36 0:a0391774b0e5 200 _acceleration = 0.0;
yweng36 0:a0391774b0e5 201 _sqrt_twoa = 1.0;
yweng36 0:a0391774b0e5 202 _stepInterval = 0;
yweng36 0:a0391774b0e5 203 _minPulseWidth = 1;
yweng36 0:a0391774b0e5 204 _lastStepTime = 0;
yweng36 0:a0391774b0e5 205 // _pin[0] = pin1;
yweng36 0:a0391774b0e5 206 // _pin[1] = pin2;
yweng36 0:a0391774b0e5 207 // _pin[2] = pin3;
yweng36 0:a0391774b0e5 208 // _pin[3] = pin4;
yweng36 0:a0391774b0e5 209 _pin0 = new DigitalOut(pin1);
yweng36 0:a0391774b0e5 210 _pin1 = new DigitalOut(pin2);
yweng36 0:a0391774b0e5 211 _pin2 = new DigitalOut(pin3);
yweng36 0:a0391774b0e5 212 _pin3 = new DigitalOut(pin4);
yweng36 0:a0391774b0e5 213
yweng36 0:a0391774b0e5 214 // NEW
yweng36 0:a0391774b0e5 215 _n = 0;
yweng36 0:a0391774b0e5 216 _c0 = 0.0;
yweng36 0:a0391774b0e5 217 _cn = 0.0;
yweng36 0:a0391774b0e5 218 _cmin = 1.0;
yweng36 0:a0391774b0e5 219 _direction = DIRECTION_CCW;
yweng36 0:a0391774b0e5 220
yweng36 0:a0391774b0e5 221 int i;
yweng36 0:a0391774b0e5 222 for (i = 0; i < 4; i++)
yweng36 0:a0391774b0e5 223 _pinInverted[i] = 0;
yweng36 0:a0391774b0e5 224 if (enable)
yweng36 0:a0391774b0e5 225 enableOutputs();
yweng36 0:a0391774b0e5 226 // Some reasonable default
yweng36 0:a0391774b0e5 227 setAcceleration(1);
yweng36 0:a0391774b0e5 228 }
yweng36 0:a0391774b0e5 229
yweng36 0:a0391774b0e5 230 AccelStepper::AccelStepper(void (*forward)(), void (*backward)())
yweng36 0:a0391774b0e5 231 {
yweng36 0:a0391774b0e5 232 _interface = 0;
yweng36 0:a0391774b0e5 233 _currentPos = 0;
yweng36 0:a0391774b0e5 234 _targetPos = 0;
yweng36 0:a0391774b0e5 235 _speed = 0.0;
yweng36 0:a0391774b0e5 236 _maxSpeed = 1.0;
yweng36 0:a0391774b0e5 237 _acceleration = 0.0;
yweng36 0:a0391774b0e5 238 _sqrt_twoa = 1.0;
yweng36 0:a0391774b0e5 239 _stepInterval = 0;
yweng36 0:a0391774b0e5 240 _minPulseWidth = 1;
yweng36 0:a0391774b0e5 241 _lastStepTime = 0;
yweng36 0:a0391774b0e5 242 _forward = forward;
yweng36 0:a0391774b0e5 243 _backward = backward;
yweng36 0:a0391774b0e5 244
yweng36 0:a0391774b0e5 245 // NEW
yweng36 0:a0391774b0e5 246 _n = 0;
yweng36 0:a0391774b0e5 247 _c0 = 0.0;
yweng36 0:a0391774b0e5 248 _cn = 0.0;
yweng36 0:a0391774b0e5 249 _cmin = 1.0;
yweng36 0:a0391774b0e5 250 _direction = DIRECTION_CCW;
yweng36 0:a0391774b0e5 251
yweng36 0:a0391774b0e5 252 int i;
yweng36 0:a0391774b0e5 253 for (i = 0; i < 4; i++)
yweng36 0:a0391774b0e5 254 _pinInverted[i] = 0;
yweng36 0:a0391774b0e5 255 // Some reasonable default
yweng36 0:a0391774b0e5 256 setAcceleration(1);
yweng36 0:a0391774b0e5 257 }
yweng36 0:a0391774b0e5 258
yweng36 0:a0391774b0e5 259 void AccelStepper::setMaxSpeed(float speed)
yweng36 0:a0391774b0e5 260 {
yweng36 0:a0391774b0e5 261 if (_maxSpeed != speed)
yweng36 0:a0391774b0e5 262 {
yweng36 0:a0391774b0e5 263 _maxSpeed = speed;
yweng36 0:a0391774b0e5 264 _cmin = 1000000.0 / speed;
yweng36 0:a0391774b0e5 265 // Recompute _n from current speed and adjust speed if accelerating or cruising
yweng36 0:a0391774b0e5 266 if (_n > 0)
yweng36 0:a0391774b0e5 267 {
yweng36 0:a0391774b0e5 268 _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
yweng36 0:a0391774b0e5 269 computeNewSpeed();
yweng36 0:a0391774b0e5 270 }
yweng36 0:a0391774b0e5 271 }
yweng36 0:a0391774b0e5 272 }
yweng36 0:a0391774b0e5 273
yweng36 0:a0391774b0e5 274 void AccelStepper::setAcceleration(float acceleration)
yweng36 0:a0391774b0e5 275 {
yweng36 0:a0391774b0e5 276 if (acceleration == 0.0)
yweng36 0:a0391774b0e5 277 return;
yweng36 0:a0391774b0e5 278 if (_acceleration != acceleration)
yweng36 0:a0391774b0e5 279 {
yweng36 0:a0391774b0e5 280 // Recompute _n per Equation 17
yweng36 0:a0391774b0e5 281 _n = _n * (_acceleration / acceleration);
yweng36 0:a0391774b0e5 282 // New c0 per Equation 7, with correction per Equation 15
yweng36 0:a0391774b0e5 283 _c0 = 0.676 * sqrt(2.0 / acceleration) * 1000000.0; // Equation 15
yweng36 0:a0391774b0e5 284 _acceleration = acceleration;
yweng36 0:a0391774b0e5 285 computeNewSpeed();
yweng36 0:a0391774b0e5 286 }
yweng36 0:a0391774b0e5 287 }
yweng36 0:a0391774b0e5 288
yweng36 0:a0391774b0e5 289 void AccelStepper::setSpeed(float speed)
yweng36 0:a0391774b0e5 290 {
yweng36 0:a0391774b0e5 291 if (speed == _speed)
yweng36 0:a0391774b0e5 292 return;
yweng36 0:a0391774b0e5 293 speed = constrain(speed, -_maxSpeed, _maxSpeed);
yweng36 0:a0391774b0e5 294 if (speed == 0.0)
yweng36 0:a0391774b0e5 295 _stepInterval = 0;
yweng36 0:a0391774b0e5 296 else
yweng36 0:a0391774b0e5 297 {
yweng36 0:a0391774b0e5 298 _stepInterval = fabs(1000000.0 / speed);
yweng36 0:a0391774b0e5 299 _direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW;
yweng36 0:a0391774b0e5 300 }
yweng36 0:a0391774b0e5 301 _speed = speed;
yweng36 0:a0391774b0e5 302 }
yweng36 0:a0391774b0e5 303
yweng36 0:a0391774b0e5 304 float AccelStepper::speed()
yweng36 0:a0391774b0e5 305 {
yweng36 0:a0391774b0e5 306 return _speed;
yweng36 0:a0391774b0e5 307 }
yweng36 0:a0391774b0e5 308
yweng36 0:a0391774b0e5 309 // Subclasses can override
yweng36 0:a0391774b0e5 310 void AccelStepper::step(long step)
yweng36 0:a0391774b0e5 311 {
yweng36 0:a0391774b0e5 312 switch (_interface)
yweng36 0:a0391774b0e5 313 {
yweng36 0:a0391774b0e5 314 case FUNCTION:
yweng36 0:a0391774b0e5 315 step0(step);
yweng36 0:a0391774b0e5 316 break;
yweng36 0:a0391774b0e5 317
yweng36 0:a0391774b0e5 318 case DRIVER:
yweng36 0:a0391774b0e5 319 step1(step);
yweng36 0:a0391774b0e5 320 break;
yweng36 0:a0391774b0e5 321
yweng36 0:a0391774b0e5 322 case FULL2WIRE:
yweng36 0:a0391774b0e5 323 step2(step);
yweng36 0:a0391774b0e5 324 break;
yweng36 0:a0391774b0e5 325
yweng36 0:a0391774b0e5 326 case FULL3WIRE:
yweng36 0:a0391774b0e5 327 step3(step);
yweng36 0:a0391774b0e5 328 break;
yweng36 0:a0391774b0e5 329
yweng36 0:a0391774b0e5 330 case FULL4WIRE:
yweng36 0:a0391774b0e5 331 step4(step);
yweng36 0:a0391774b0e5 332 break;
yweng36 0:a0391774b0e5 333
yweng36 0:a0391774b0e5 334 case HALF3WIRE:
yweng36 0:a0391774b0e5 335 step6(step);
yweng36 0:a0391774b0e5 336 break;
yweng36 0:a0391774b0e5 337
yweng36 0:a0391774b0e5 338 case HALF4WIRE:
yweng36 0:a0391774b0e5 339 step8(step);
yweng36 0:a0391774b0e5 340 break;
yweng36 0:a0391774b0e5 341 }
yweng36 0:a0391774b0e5 342 }
yweng36 0:a0391774b0e5 343
yweng36 0:a0391774b0e5 344 // You might want to override this to implement eg serial output
yweng36 0:a0391774b0e5 345 // bit 0 of the mask corresponds to _pin[0]
yweng36 0:a0391774b0e5 346 // bit 1 of the mask corresponds to _pin[1]
yweng36 0:a0391774b0e5 347 // ....
yweng36 0:a0391774b0e5 348 void AccelStepper::setOutputPins(uint8_t mask)
yweng36 0:a0391774b0e5 349 {
yweng36 0:a0391774b0e5 350 // uint8_t numpins = 2;
yweng36 0:a0391774b0e5 351 // if (_interface == FULL4WIRE || _interface == HALF4WIRE)
yweng36 0:a0391774b0e5 352 // numpins = 4;
yweng36 0:a0391774b0e5 353 // else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
yweng36 0:a0391774b0e5 354 // numpins = 3;
yweng36 0:a0391774b0e5 355 // uint8_t i;
yweng36 0:a0391774b0e5 356 // for (i = 0; i < numpins; i++)
yweng36 0:a0391774b0e5 357 // digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i]));
yweng36 0:a0391774b0e5 358 *_pin0 = (mask & (1 << 0)) ? (HIGH ^ _pinInverted[0]) : (LOW ^ _pinInverted[0]);
yweng36 0:a0391774b0e5 359 *_pin1 = (mask & (1 << 1)) ? (HIGH ^ _pinInverted[1]) : (LOW ^ _pinInverted[1]);
yweng36 0:a0391774b0e5 360 if (_interface == FULL4WIRE || _interface == HALF4WIRE) {
yweng36 0:a0391774b0e5 361 *_pin2 = (mask & (1 << 2)) ? (HIGH ^ _pinInverted[2]) : (LOW ^ _pinInverted[2]);
yweng36 0:a0391774b0e5 362 *_pin3 = (mask & (1 << 3)) ? (HIGH ^ _pinInverted[3]) : (LOW ^ _pinInverted[3]);
yweng36 0:a0391774b0e5 363 }
yweng36 0:a0391774b0e5 364 else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
yweng36 0:a0391774b0e5 365 *_pin2 = (mask & (1 << 2)) ? (HIGH ^ _pinInverted[2]) : (LOW ^ _pinInverted[2]);
yweng36 0:a0391774b0e5 366 }
yweng36 0:a0391774b0e5 367
yweng36 0:a0391774b0e5 368 // 0 pin step function (ie for functional usage)
yweng36 0:a0391774b0e5 369 void AccelStepper::step0(long step)
yweng36 0:a0391774b0e5 370 {
yweng36 0:a0391774b0e5 371 if (_speed > 0)
yweng36 0:a0391774b0e5 372 _forward();
yweng36 0:a0391774b0e5 373 else
yweng36 0:a0391774b0e5 374 _backward();
yweng36 0:a0391774b0e5 375 }
yweng36 0:a0391774b0e5 376
yweng36 0:a0391774b0e5 377 // 1 pin step function (ie for stepper drivers)
yweng36 0:a0391774b0e5 378 // This is passed the current step number (0 to 7)
yweng36 0:a0391774b0e5 379 // Subclasses can override
yweng36 0:a0391774b0e5 380 void AccelStepper::step1(long step)
yweng36 0:a0391774b0e5 381 {
yweng36 0:a0391774b0e5 382 // _pin[0] is step, _pin[1] is direction
yweng36 0:a0391774b0e5 383 setOutputPins(_direction ? 0b10 : 0b00); // Set direction first else get rogue pulses
yweng36 0:a0391774b0e5 384 setOutputPins(_direction ? 0b11 : 0b01); // step HIGH
yweng36 0:a0391774b0e5 385 // Caution 200ns setup time
yweng36 0:a0391774b0e5 386 // Delay the minimum allowed pulse width
yweng36 0:a0391774b0e5 387 //delayMicroseconds(_minPulseWidth);
yweng36 0:a0391774b0e5 388 wait_us(_minPulseWidth);
yweng36 0:a0391774b0e5 389 setOutputPins(_direction ? 0b10 : 0b00); // step LOW
yweng36 0:a0391774b0e5 390
yweng36 0:a0391774b0e5 391 }
yweng36 0:a0391774b0e5 392
yweng36 0:a0391774b0e5 393
yweng36 0:a0391774b0e5 394 // 2 pin step function
yweng36 0:a0391774b0e5 395 // This is passed the current step number (0 to 7)
yweng36 0:a0391774b0e5 396 // Subclasses can override
yweng36 0:a0391774b0e5 397 void AccelStepper::step2(long step)
yweng36 0:a0391774b0e5 398 {
yweng36 0:a0391774b0e5 399 switch (step & 0x3)
yweng36 0:a0391774b0e5 400 {
yweng36 0:a0391774b0e5 401 case 0: /* 01 */
yweng36 0:a0391774b0e5 402 setOutputPins(0b10);
yweng36 0:a0391774b0e5 403 break;
yweng36 0:a0391774b0e5 404
yweng36 0:a0391774b0e5 405 case 1: /* 11 */
yweng36 0:a0391774b0e5 406 setOutputPins(0b11);
yweng36 0:a0391774b0e5 407 break;
yweng36 0:a0391774b0e5 408
yweng36 0:a0391774b0e5 409 case 2: /* 10 */
yweng36 0:a0391774b0e5 410 setOutputPins(0b01);
yweng36 0:a0391774b0e5 411 break;
yweng36 0:a0391774b0e5 412
yweng36 0:a0391774b0e5 413 case 3: /* 00 */
yweng36 0:a0391774b0e5 414 setOutputPins(0b00);
yweng36 0:a0391774b0e5 415 break;
yweng36 0:a0391774b0e5 416 }
yweng36 0:a0391774b0e5 417 }
yweng36 0:a0391774b0e5 418 // 3 pin step function
yweng36 0:a0391774b0e5 419 // This is passed the current step number (0 to 7)
yweng36 0:a0391774b0e5 420 // Subclasses can override
yweng36 0:a0391774b0e5 421 void AccelStepper::step3(long step)
yweng36 0:a0391774b0e5 422 {
yweng36 0:a0391774b0e5 423 switch (step % 3)
yweng36 0:a0391774b0e5 424 {
yweng36 0:a0391774b0e5 425 case 0: // 100
yweng36 0:a0391774b0e5 426 setOutputPins(0b100);
yweng36 0:a0391774b0e5 427 break;
yweng36 0:a0391774b0e5 428
yweng36 0:a0391774b0e5 429 case 1: // 001
yweng36 0:a0391774b0e5 430 setOutputPins(0b001);
yweng36 0:a0391774b0e5 431 break;
yweng36 0:a0391774b0e5 432
yweng36 0:a0391774b0e5 433 case 2: //010
yweng36 0:a0391774b0e5 434 setOutputPins(0b010);
yweng36 0:a0391774b0e5 435 break;
yweng36 0:a0391774b0e5 436
yweng36 0:a0391774b0e5 437 }
yweng36 0:a0391774b0e5 438 }
yweng36 0:a0391774b0e5 439
yweng36 0:a0391774b0e5 440 // 4 pin step function for half stepper
yweng36 0:a0391774b0e5 441 // This is passed the current step number (0 to 7)
yweng36 0:a0391774b0e5 442 // Subclasses can override
yweng36 0:a0391774b0e5 443 void AccelStepper::step4(long step)
yweng36 0:a0391774b0e5 444 {
yweng36 0:a0391774b0e5 445 switch (step & 0x3)
yweng36 0:a0391774b0e5 446 {
yweng36 0:a0391774b0e5 447 case 0: // 1010
yweng36 0:a0391774b0e5 448 setOutputPins(0b0101);
yweng36 0:a0391774b0e5 449 break;
yweng36 0:a0391774b0e5 450
yweng36 0:a0391774b0e5 451 case 1: // 0110
yweng36 0:a0391774b0e5 452 setOutputPins(0b0110);
yweng36 0:a0391774b0e5 453 break;
yweng36 0:a0391774b0e5 454
yweng36 0:a0391774b0e5 455 case 2: //0101
yweng36 0:a0391774b0e5 456 setOutputPins(0b1010);
yweng36 0:a0391774b0e5 457 break;
yweng36 0:a0391774b0e5 458
yweng36 0:a0391774b0e5 459 case 3: //1001
yweng36 0:a0391774b0e5 460 setOutputPins(0b1001);
yweng36 0:a0391774b0e5 461 break;
yweng36 0:a0391774b0e5 462 }
yweng36 0:a0391774b0e5 463 }
yweng36 0:a0391774b0e5 464
yweng36 0:a0391774b0e5 465 // 3 pin half step function
yweng36 0:a0391774b0e5 466 // This is passed the current step number (0 to 7)
yweng36 0:a0391774b0e5 467 // Subclasses can override
yweng36 0:a0391774b0e5 468 void AccelStepper::step6(long step)
yweng36 0:a0391774b0e5 469 {
yweng36 0:a0391774b0e5 470 switch (step % 6)
yweng36 0:a0391774b0e5 471 {
yweng36 0:a0391774b0e5 472 case 0: // 100
yweng36 0:a0391774b0e5 473 setOutputPins(0b100);
yweng36 0:a0391774b0e5 474 break;
yweng36 0:a0391774b0e5 475
yweng36 0:a0391774b0e5 476 case 1: // 101
yweng36 0:a0391774b0e5 477 setOutputPins(0b101);
yweng36 0:a0391774b0e5 478 break;
yweng36 0:a0391774b0e5 479
yweng36 0:a0391774b0e5 480 case 2: // 001
yweng36 0:a0391774b0e5 481 setOutputPins(0b001);
yweng36 0:a0391774b0e5 482 break;
yweng36 0:a0391774b0e5 483
yweng36 0:a0391774b0e5 484 case 3: // 011
yweng36 0:a0391774b0e5 485 setOutputPins(0b011);
yweng36 0:a0391774b0e5 486 break;
yweng36 0:a0391774b0e5 487
yweng36 0:a0391774b0e5 488 case 4: // 010
yweng36 0:a0391774b0e5 489 setOutputPins(0b010);
yweng36 0:a0391774b0e5 490 break;
yweng36 0:a0391774b0e5 491
yweng36 0:a0391774b0e5 492 case 5: // 011
yweng36 0:a0391774b0e5 493 setOutputPins(0b110);
yweng36 0:a0391774b0e5 494 break;
yweng36 0:a0391774b0e5 495
yweng36 0:a0391774b0e5 496 }
yweng36 0:a0391774b0e5 497 }
yweng36 0:a0391774b0e5 498
yweng36 0:a0391774b0e5 499 // 4 pin half step function
yweng36 0:a0391774b0e5 500 // This is passed the current step number (0 to 7)
yweng36 0:a0391774b0e5 501 // Subclasses can override
yweng36 0:a0391774b0e5 502 void AccelStepper::step8(long step)
yweng36 0:a0391774b0e5 503 {
yweng36 0:a0391774b0e5 504 switch (step & 0x7)
yweng36 0:a0391774b0e5 505 {
yweng36 0:a0391774b0e5 506 case 0: // 1000
yweng36 0:a0391774b0e5 507 setOutputPins(0b0001);
yweng36 0:a0391774b0e5 508 break;
yweng36 0:a0391774b0e5 509
yweng36 0:a0391774b0e5 510 case 1: // 1010
yweng36 0:a0391774b0e5 511 setOutputPins(0b0101);
yweng36 0:a0391774b0e5 512 break;
yweng36 0:a0391774b0e5 513
yweng36 0:a0391774b0e5 514 case 2: // 0010
yweng36 0:a0391774b0e5 515 setOutputPins(0b0100);
yweng36 0:a0391774b0e5 516 break;
yweng36 0:a0391774b0e5 517
yweng36 0:a0391774b0e5 518 case 3: // 0110
yweng36 0:a0391774b0e5 519 setOutputPins(0b0110);
yweng36 0:a0391774b0e5 520 break;
yweng36 0:a0391774b0e5 521
yweng36 0:a0391774b0e5 522 case 4: // 0100
yweng36 0:a0391774b0e5 523 setOutputPins(0b0010);
yweng36 0:a0391774b0e5 524 break;
yweng36 0:a0391774b0e5 525
yweng36 0:a0391774b0e5 526 case 5: //0101
yweng36 0:a0391774b0e5 527 setOutputPins(0b1010);
yweng36 0:a0391774b0e5 528 break;
yweng36 0:a0391774b0e5 529
yweng36 0:a0391774b0e5 530 case 6: // 0001
yweng36 0:a0391774b0e5 531 setOutputPins(0b1000);
yweng36 0:a0391774b0e5 532 break;
yweng36 0:a0391774b0e5 533
yweng36 0:a0391774b0e5 534 case 7: //1001
yweng36 0:a0391774b0e5 535 setOutputPins(0b1001);
yweng36 0:a0391774b0e5 536 break;
yweng36 0:a0391774b0e5 537 }
yweng36 0:a0391774b0e5 538 }
yweng36 0:a0391774b0e5 539
yweng36 0:a0391774b0e5 540 // Prevents power consumption on the outputs
yweng36 0:a0391774b0e5 541 void AccelStepper::disableOutputs()
yweng36 0:a0391774b0e5 542 {
yweng36 0:a0391774b0e5 543 if (! _interface) return;
yweng36 0:a0391774b0e5 544
yweng36 0:a0391774b0e5 545 setOutputPins(0); // Handles inversion automatically
yweng36 0:a0391774b0e5 546 // if (_enablePin != 0xff)
yweng36 0:a0391774b0e5 547 if (_enablePin)
yweng36 0:a0391774b0e5 548 // digitalWrite(_enablePin, LOW ^ _enableInverted);
yweng36 0:a0391774b0e5 549 *_enablePin = LOW ^ _enableInverted;
yweng36 0:a0391774b0e5 550 }
yweng36 0:a0391774b0e5 551
yweng36 0:a0391774b0e5 552 void AccelStepper::enableOutputs()
yweng36 0:a0391774b0e5 553 {
yweng36 0:a0391774b0e5 554 if (! _interface)
yweng36 0:a0391774b0e5 555 return;
yweng36 0:a0391774b0e5 556
yweng36 0:a0391774b0e5 557 //pinMode(_pin[0], OUTPUT);
yweng36 0:a0391774b0e5 558 //pinMode(_pin[1], OUTPUT);
yweng36 0:a0391774b0e5 559 if (_interface == FULL4WIRE || _interface == HALF4WIRE)
yweng36 0:a0391774b0e5 560 {
yweng36 0:a0391774b0e5 561 //pinMode(_pin[2], OUTPUT);
yweng36 0:a0391774b0e5 562 //pinMode(_pin[3], OUTPUT);
yweng36 0:a0391774b0e5 563 }
yweng36 0:a0391774b0e5 564 else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
yweng36 0:a0391774b0e5 565 {
yweng36 0:a0391774b0e5 566 //pinMode(_pin[2], OUTPUT);
yweng36 0:a0391774b0e5 567 }
yweng36 0:a0391774b0e5 568
yweng36 0:a0391774b0e5 569 // if (_enablePin != 0xff)
yweng36 0:a0391774b0e5 570 if (_enablePin)
yweng36 0:a0391774b0e5 571 {
yweng36 0:a0391774b0e5 572 //pinMode(_enablePin, OUTPUT);
yweng36 0:a0391774b0e5 573 //digitalWrite(_enablePin, HIGH ^ _enableInverted);
yweng36 0:a0391774b0e5 574 *_enablePin = HIGH ^ _enableInverted;
yweng36 0:a0391774b0e5 575 }
yweng36 0:a0391774b0e5 576 }
yweng36 0:a0391774b0e5 577
yweng36 0:a0391774b0e5 578 void AccelStepper::setMinPulseWidth(unsigned int minWidth)
yweng36 0:a0391774b0e5 579 {
yweng36 0:a0391774b0e5 580 _minPulseWidth = minWidth;
yweng36 0:a0391774b0e5 581 }
yweng36 0:a0391774b0e5 582
yweng36 0:a0391774b0e5 583 // void AccelStepper::setEnablePin(uint8_t enablePin)
yweng36 0:a0391774b0e5 584 void AccelStepper::setEnablePin(PinName enablePin)
yweng36 0:a0391774b0e5 585 {
yweng36 0:a0391774b0e5 586 // _enablePin = enablePin;
yweng36 0:a0391774b0e5 587 _enablePin = new DigitalOut(enablePin);
yweng36 0:a0391774b0e5 588
yweng36 0:a0391774b0e5 589 // This happens after construction, so init pin now.
yweng36 0:a0391774b0e5 590 // if (_enablePin != 0xff)
yweng36 0:a0391774b0e5 591 if (*_enablePin)
yweng36 0:a0391774b0e5 592 {
yweng36 0:a0391774b0e5 593 //pinMode(_enablePin, OUTPUT);
yweng36 0:a0391774b0e5 594 //digitalWrite(_enablePin, HIGH ^ _enableInverted);
yweng36 0:a0391774b0e5 595 *_enablePin = HIGH ^ _enableInverted;
yweng36 0:a0391774b0e5 596 }
yweng36 0:a0391774b0e5 597 }
yweng36 0:a0391774b0e5 598
yweng36 0:a0391774b0e5 599 void AccelStepper::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert)
yweng36 0:a0391774b0e5 600 {
yweng36 0:a0391774b0e5 601 _pinInverted[0] = stepInvert;
yweng36 0:a0391774b0e5 602 _pinInverted[1] = directionInvert;
yweng36 0:a0391774b0e5 603 _enableInverted = enableInvert;
yweng36 0:a0391774b0e5 604 }
yweng36 0:a0391774b0e5 605
yweng36 0:a0391774b0e5 606 void AccelStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert)
yweng36 0:a0391774b0e5 607 {
yweng36 0:a0391774b0e5 608 _pinInverted[0] = pin1Invert;
yweng36 0:a0391774b0e5 609 _pinInverted[1] = pin2Invert;
yweng36 0:a0391774b0e5 610 _pinInverted[2] = pin3Invert;
yweng36 0:a0391774b0e5 611 _pinInverted[3] = pin4Invert;
yweng36 0:a0391774b0e5 612 _enableInverted = enableInvert;
yweng36 0:a0391774b0e5 613 }
yweng36 0:a0391774b0e5 614
yweng36 0:a0391774b0e5 615 // Blocks until the target position is reached and stopped
yweng36 0:a0391774b0e5 616 void AccelStepper::runToPosition()
yweng36 0:a0391774b0e5 617 {
yweng36 0:a0391774b0e5 618 while (run())
yweng36 0:a0391774b0e5 619 ;
yweng36 0:a0391774b0e5 620 }
yweng36 0:a0391774b0e5 621
yweng36 0:a0391774b0e5 622 bool AccelStepper::runSpeedToPosition()
yweng36 0:a0391774b0e5 623 {
yweng36 0:a0391774b0e5 624 if (_targetPos == _currentPos)
yweng36 0:a0391774b0e5 625 return false;
yweng36 0:a0391774b0e5 626 if (_targetPos >_currentPos)
yweng36 0:a0391774b0e5 627 _direction = DIRECTION_CW;
yweng36 0:a0391774b0e5 628 else
yweng36 0:a0391774b0e5 629 _direction = DIRECTION_CCW;
yweng36 0:a0391774b0e5 630 return runSpeed();
yweng36 0:a0391774b0e5 631 }
yweng36 0:a0391774b0e5 632
yweng36 0:a0391774b0e5 633 // Blocks until the new target position is reached
yweng36 0:a0391774b0e5 634 void AccelStepper::runToNewPosition(long position)
yweng36 0:a0391774b0e5 635 {
yweng36 0:a0391774b0e5 636 moveTo(position);
yweng36 0:a0391774b0e5 637 runToPosition();
yweng36 0:a0391774b0e5 638 }
yweng36 0:a0391774b0e5 639
yweng36 0:a0391774b0e5 640 void AccelStepper::stop()
yweng36 0:a0391774b0e5 641 {
yweng36 0:a0391774b0e5 642 if (_speed != 0.0)
yweng36 0:a0391774b0e5 643 {
yweng36 0:a0391774b0e5 644 long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding)
yweng36 0:a0391774b0e5 645 if (_speed > 0)
yweng36 0:a0391774b0e5 646 move(stepsToStop);
yweng36 0:a0391774b0e5 647 else
yweng36 0:a0391774b0e5 648 move(-stepsToStop);
yweng36 0:a0391774b0e5 649 }
yweng36 0:a0391774b0e5 650 }
yweng36 0:a0391774b0e5 651