aircord / Mbed 2 deprecated dmx_to_stepper

test2017-1-13

Dependencies:   AT24C1024 DMX-STM32 mbed

AccelStepper.cpp@0:d94213e24c2e, 2017-01-13 (annotated)

Committer:
tomo370
Date:
Fri Jan 13 03:28:16 2017 +0000
Revision:
0:d94213e24c2e
Child:
4:c3f3fdde7eee
stepper

Who changed what in which revision?

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