Zeran Gu / Mbed 2 deprecated STM32F103C8T6_Stepper

thanks to Zoltan Hudak publish the way to use stm32f103c8t6 on mbed. now you can use it with stepper driver

Dependencies:   mbed-STM32F103C8T6 mbed

Fork of STM32F103C8T6_Hello by Zoltan Hudak

AccelStepper.cpp@11:f44fc4db4ab1, 2017-05-23 (annotated)

Committer:
Zeran
Date:
Tue May 23 17:28:21 2017 +0000
Revision:
11:f44fc4db4ab1
stm32f103c8t6 stepper driver

Who changed what in which revision?

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