From 3d6ac39a847f07fd27377e83f24187b629f6f364 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Miranda?= Date: Tue, 4 Nov 2025 00:18:08 +0000 Subject: [PATCH] Upload files to "/" --- AccelStepper.cpp | 644 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 644 insertions(+) create mode 100644 AccelStepper.cpp diff --git a/AccelStepper.cpp b/AccelStepper.cpp new file mode 100644 index 0000000..52fa566 --- /dev/null +++ b/AccelStepper.cpp @@ -0,0 +1,644 @@ +// AccelStepper.cpp +// +// Copyright (C) 2009-2013 Mike McCauley +// $Id: AccelStepper.cpp,v 1.23 2016/08/09 00:39:10 mikem Exp mikem $ + +#include "AccelStepper.h" + +#if 0 +// Some debugging assistance +void dump(uint8_t* p, int l) +{ + int i; + + for (i = 0; i < l; i++) + { + Serial.print(p[i], HEX); + Serial.print(" "); + } + Serial.println(""); +} +#endif + +void AccelStepper::moveTo(long absolute) +{ + if (_targetPos != absolute) + { + _targetPos = absolute; + computeNewSpeed(); + // compute new n? + } +} + +void AccelStepper::move(long relative) +{ + moveTo(_currentPos + relative); +} + +// Implements steps according to the current step interval +// You must call this at least once per step +// returns true if a step occurred +boolean AccelStepper::runSpeed() +{ + // Dont do anything unless we actually have a step interval + if (!_stepInterval) + return false; + + unsigned long time = micros(); + if (time - _lastStepTime >= _stepInterval) + { + if (_direction == DIRECTION_CW) + { + // Clockwise + _currentPos += 1; + } + else + { + // Anticlockwise + _currentPos -= 1; + } + step(_currentPos); + + _lastStepTime = time; + return true; + } + else + { + return false; + } +} + +long AccelStepper::distanceToGo() +{ + return _targetPos - _currentPos; +} + +long AccelStepper::targetPosition() +{ + return _targetPos; +} + +long AccelStepper::currentPosition() +{ + return _currentPos; +} + +// Useful during initialisations or after initial positioning +// Sets speed to 0 +void AccelStepper::setCurrentPosition(long position) +{ + _targetPos = _currentPos = position; + _n = 0; + _stepInterval = 0; + _speed = 0.0; +} + +void AccelStepper::computeNewSpeed() +{ + long distanceTo = distanceToGo(); // +ve is clockwise from curent location + + long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16 + + if (distanceTo == 0 && stepsToStop <= 1) + { + // We are at the target and its time to stop + _stepInterval = 0; + _speed = 0.0; + _n = 0; + return; + } + + if (distanceTo > 0) + { + // We are anticlockwise from the target + // Need to go clockwise from here, maybe decelerate now + if (_n > 0) + { + // Currently accelerating, need to decel now? Or maybe going the wrong way? + if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW) + _n = -stepsToStop; // Start deceleration + } + else if (_n < 0) + { + // Currently decelerating, need to accel again? + if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW) + _n = -_n; // Start accceleration + } + } + else if (distanceTo < 0) + { + // We are clockwise from the target + // Need to go anticlockwise from here, maybe decelerate + if (_n > 0) + { + // Currently accelerating, need to decel now? Or maybe going the wrong way? + if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW) + _n = -stepsToStop; // Start deceleration + } + else if (_n < 0) + { + // Currently decelerating, need to accel again? + if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW) + _n = -_n; // Start accceleration + } + } + + // Need to accelerate or decelerate + if (_n == 0) + { + // First step from stopped + _cn = _c0; + _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW; + } + else + { + // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve). + _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 + _cn = max(_cn, _cmin); + } + _n++; + _stepInterval = _cn; + _speed = 1000000.0 / _cn; + if (_direction == DIRECTION_CCW) + _speed = -_speed; + +#if 0 + Serial.println(_speed); + Serial.println(_acceleration); + Serial.println(_cn); + Serial.println(_c0); + Serial.println(_n); + Serial.println(_stepInterval); + Serial.println(distanceTo); + Serial.println(stepsToStop); + Serial.println("-----"); +#endif +} + +// Run the motor to implement speed and acceleration in order to proceed to the target position +// You must call this at least once per step, preferably in your main loop +// If the motor is in the desired position, the cost is very small +// returns true if the motor is still running to the target position. +boolean AccelStepper::run() +{ + if (runSpeed()) + computeNewSpeed(); + return _speed != 0.0 || distanceToGo() != 0; +} + +AccelStepper::AccelStepper(uint8_t interface, uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, bool enable) +{ + _interface = interface; + _currentPos = 0; + _targetPos = 0; + _speed = 0.0; + _maxSpeed = 1.0; + _acceleration = 0.0; + _sqrt_twoa = 1.0; + _stepInterval = 0; + _minPulseWidth = 1; + _enablePin = 0xff; + _lastStepTime = 0; + _pin[0] = pin1; + _pin[1] = pin2; + _pin[2] = pin3; + _pin[3] = pin4; + + // NEW + _n = 0; + _c0 = 0.0; + _cn = 0.0; + _cmin = 1.0; + _direction = DIRECTION_CCW; + + int i; + for (i = 0; i < 4; i++) + _pinInverted[i] = 0; + if (enable) + enableOutputs(); + // Some reasonable default + setAcceleration(1); +} + +AccelStepper::AccelStepper(void (*forward)(), void (*backward)()) +{ + _interface = 0; + _currentPos = 0; + _targetPos = 0; + _speed = 0.0; + _maxSpeed = 1.0; + _acceleration = 0.0; + _sqrt_twoa = 1.0; + _stepInterval = 0; + _minPulseWidth = 1; + _enablePin = 0xff; + _lastStepTime = 0; + _pin[0] = 0; + _pin[1] = 0; + _pin[2] = 0; + _pin[3] = 0; + _forward = forward; + _backward = backward; + + // NEW + _n = 0; + _c0 = 0.0; + _cn = 0.0; + _cmin = 1.0; + _direction = DIRECTION_CCW; + + int i; + for (i = 0; i < 4; i++) + _pinInverted[i] = 0; + // Some reasonable default + setAcceleration(1); +} + +void AccelStepper::setMaxSpeed(float speed) +{ + if (_maxSpeed != speed) + { + _maxSpeed = speed; + _cmin = 1000000.0 / speed; + // Recompute _n from current speed and adjust speed if accelerating or cruising + if (_n > 0) + { + _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16 + computeNewSpeed(); + } + } +} + +float AccelStepper::maxSpeed() +{ + return _maxSpeed; +} + +void AccelStepper::setAcceleration(float acceleration) +{ + if (acceleration == 0.0) + return; + if (_acceleration != acceleration) + { + // Recompute _n per Equation 17 + _n = _n * (_acceleration / acceleration); + // New c0 per Equation 7, with correction per Equation 15 + _c0 = 0.676 * sqrt(2.0 / acceleration) * 1000000.0; // Equation 15 + _acceleration = acceleration; + computeNewSpeed(); + } +} + +void AccelStepper::setSpeed(float speed) +{ + if (speed == _speed) + return; + speed = constrain(speed, -_maxSpeed, _maxSpeed); + if (speed == 0.0) + _stepInterval = 0; + else + { + _stepInterval = fabs(1000000.0 / speed); + _direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW; + } + _speed = speed; +} + +float AccelStepper::speed() +{ + return _speed; +} + +// Subclasses can override +void AccelStepper::step(long step) +{ + switch (_interface) + { + case FUNCTION: + step0(step); + break; + + case DRIVER: + step1(step); + break; + + case FULL2WIRE: + step2(step); + break; + + case FULL3WIRE: + step3(step); + break; + + case FULL4WIRE: + step4(step); + break; + + case HALF3WIRE: + step6(step); + break; + + case HALF4WIRE: + step8(step); + break; + } +} + +// You might want to override this to implement eg serial output +// bit 0 of the mask corresponds to _pin[0] +// bit 1 of the mask corresponds to _pin[1] +// .... +void AccelStepper::setOutputPins(uint8_t mask) +{ + uint8_t numpins = 2; + if (_interface == FULL4WIRE || _interface == HALF4WIRE) + numpins = 4; + else if (_interface == FULL3WIRE || _interface == HALF3WIRE) + numpins = 3; + uint8_t i; + for (i = 0; i < numpins; i++) + digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i])); +} + +// 0 pin step function (ie for functional usage) +void AccelStepper::step0(long step) +{ + if (_speed > 0) + _forward(); + else + _backward(); +} + +// 1 pin step function (ie for stepper drivers) +// This is passed the current step number (0 to 7) +// Subclasses can override +void AccelStepper::step1(long step) +{ + // _pin[0] is step, _pin[1] is direction + setOutputPins(_direction ? 0b10 : 0b00); // Set direction first else get rogue pulses + setOutputPins(_direction ? 0b11 : 0b01); // step HIGH + // Caution 200ns setup time + // Delay the minimum allowed pulse width + delayMicroseconds(_minPulseWidth); + setOutputPins(_direction ? 0b10 : 0b00); // step LOW + +} + + +// 2 pin step function +// This is passed the current step number (0 to 7) +// Subclasses can override +void AccelStepper::step2(long step) +{ + switch (step & 0x3) + { + case 0: /* 01 */ + setOutputPins(0b10); + break; + + case 1: /* 11 */ + setOutputPins(0b11); + break; + + case 2: /* 10 */ + setOutputPins(0b01); + break; + + case 3: /* 00 */ + setOutputPins(0b00); + break; + } +} +// 3 pin step function +// This is passed the current step number (0 to 7) +// Subclasses can override +void AccelStepper::step3(long step) +{ + switch (step % 3) + { + case 0: // 100 + setOutputPins(0b100); + break; + + case 1: // 001 + setOutputPins(0b001); + break; + + case 2: //010 + setOutputPins(0b010); + break; + + } +} + +// 4 pin step function for half stepper +// This is passed the current step number (0 to 7) +// Subclasses can override +void AccelStepper::step4(long step) +{ + switch (step & 0x3) + { + case 0: // 1010 + setOutputPins(0b0101); + break; + + case 1: // 0110 + setOutputPins(0b0110); + break; + + case 2: //0101 + setOutputPins(0b1010); + break; + + case 3: //1001 + setOutputPins(0b1001); + break; + } +} + +// 3 pin half step function +// This is passed the current step number (0 to 7) +// Subclasses can override +void AccelStepper::step6(long step) +{ + switch (step % 6) + { + case 0: // 100 + setOutputPins(0b100); + break; + + case 1: // 101 + setOutputPins(0b101); + break; + + case 2: // 001 + setOutputPins(0b001); + break; + + case 3: // 011 + setOutputPins(0b011); + break; + + case 4: // 010 + setOutputPins(0b010); + break; + + case 5: // 011 + setOutputPins(0b110); + break; + + } +} + +// 4 pin half step function +// This is passed the current step number (0 to 7) +// Subclasses can override +void AccelStepper::step8(long step) +{ + switch (step & 0x7) + { + case 0: // 1000 + setOutputPins(0b0001); + break; + + case 1: // 1010 + setOutputPins(0b0101); + break; + + case 2: // 0010 + setOutputPins(0b0100); + break; + + case 3: // 0110 + setOutputPins(0b0110); + break; + + case 4: // 0100 + setOutputPins(0b0010); + break; + + case 5: //0101 + setOutputPins(0b1010); + break; + + case 6: // 0001 + setOutputPins(0b1000); + break; + + case 7: //1001 + setOutputPins(0b1001); + break; + } +} + +// Prevents power consumption on the outputs +void AccelStepper::disableOutputs() +{ + if (! _interface) return; + + setOutputPins(0); // Handles inversion automatically + if (_enablePin != 0xff) + { + pinMode(_enablePin, OUTPUT); + digitalWrite(_enablePin, LOW ^ _enableInverted); + } +} + +void AccelStepper::enableOutputs() +{ + if (! _interface) + return; + + pinMode(_pin[0], OUTPUT); + pinMode(_pin[1], OUTPUT); + if (_interface == FULL4WIRE || _interface == HALF4WIRE) + { + pinMode(_pin[2], OUTPUT); + pinMode(_pin[3], OUTPUT); + } + else if (_interface == FULL3WIRE || _interface == HALF3WIRE) + { + pinMode(_pin[2], OUTPUT); + } + + if (_enablePin != 0xff) + { + pinMode(_enablePin, OUTPUT); + digitalWrite(_enablePin, HIGH ^ _enableInverted); + } +} + +void AccelStepper::setMinPulseWidth(unsigned int minWidth) +{ + _minPulseWidth = minWidth; +} + +void AccelStepper::setEnablePin(uint8_t enablePin) +{ + _enablePin = enablePin; + + // This happens after construction, so init pin now. + if (_enablePin != 0xff) + { + pinMode(_enablePin, OUTPUT); + digitalWrite(_enablePin, HIGH ^ _enableInverted); + } +} + +void AccelStepper::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert) +{ + _pinInverted[0] = stepInvert; + _pinInverted[1] = directionInvert; + _enableInverted = enableInvert; +} + +void AccelStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert) +{ + _pinInverted[0] = pin1Invert; + _pinInverted[1] = pin2Invert; + _pinInverted[2] = pin3Invert; + _pinInverted[3] = pin4Invert; + _enableInverted = enableInvert; +} + +// Blocks until the target position is reached and stopped +void AccelStepper::runToPosition() +{ + while (run()) + ; +} + +boolean AccelStepper::runSpeedToPosition() +{ + if (_targetPos == _currentPos) + return false; + if (_targetPos >_currentPos) + _direction = DIRECTION_CW; + else + _direction = DIRECTION_CCW; + return runSpeed(); +} + +// Blocks until the new target position is reached +void AccelStepper::runToNewPosition(long position) +{ + moveTo(position); + runToPosition(); +} + +void AccelStepper::stop() +{ + if (_speed != 0.0) + { + long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding) + if (_speed > 0) + move(stepsToStop); + else + move(-stepsToStop); + } +} + +bool AccelStepper::isRunning() +{ + return !(_speed == 0.0 && _targetPos == _currentPos); +}