Yuliya Smirnova / AccelStepper

Библиотека для работы с шаговым двигателем

Dependents:   Delta_Robot_2018 Speed_Control

AccelStepper.cpp@0:deef8c36d17a, 2018-05-07 (annotated)

Committer:
yuliyasm
Date:
Mon May 07 11:39:53 2018 +0000
Revision:
0:deef8c36d17a
4

Who changed what in which revision?

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