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