aircord / Mbed 2 deprecated dmx_to_stepper

test2017-1-13

Dependencies:   AT24C1024 DMX-STM32 mbed

AccelStepper.cpp@8:2e68b9de685d, 2017-07-28 (annotated)

Committer:
tomo370
Date:
Fri Jul 28 04:44:42 2017 +0000
Revision:
8:2e68b9de685d
Parent:
4:c3f3fdde7eee 
2017_07_28ver

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 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 }