// Shows how to run AccelStepper in the simplest,
// fixed speed mode with no accelerations
#include "AccelStepper.h"

AccelStepper stepper; // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5

void setup()
{
  stepper.setMaxSpeed(1000);
  stepper.setSpeed(1000);
}

void loop()
{
  stepper.runSpeed();
}


accelstepper-example.ino

diagram.json

// AccelStepper.cpp
//
// Copyright (C) 2009-2013 Mike McCauley
// $Id: AccelStepper.cpp,v 1.24 2020/04/20 00:15:03 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; // Caution: does not account for costs in step()

    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;
  _enableInverted = false;

  // 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 (speed < 0.0)
    speed = -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 < 0.0)
    acceleration = -acceleration;
  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)
{
  (void)(step); // Unused
  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)
{
  (void)(step); // Unused

  // _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())
    YIELD; // Let system housekeeping occur
}

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);
}
