Yuliya Smirnova / Mbed 2 deprecated firstDelta

4653

Dependencies:   mbed-STM32F103C8T6 mbed USBDevice_STM32F103

AccelStepper.cpp@2:d4dad64faadb, 2018-02-03 (annotated)

Committer:
yuliyasm
Date:
Sat Feb 03 19:25:25 2018 +0000
Revision:
2:d4dad64faadb
preAlfa Stepper Motor Control

Who changed what in which revision?

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