File content as of revision 8:2e68b9de685d:

// AccelStepper.cpp
//
// Copyright (C) 2009-2013 Mike McCauley
// $Id: AccelStepper.cpp,v 1.19 2014/10/31 06:05:27 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::newSpeed()
{
    computeNewSpeed();
}


void AccelStepper::moveTo(long absolute)
{
    if (_targetPos != absolute) {
        _targetPos = absolute;
        computeNewSpeed();
    }
}

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
bool AccelStepper::runSpeed()
{
    extern Timer t;
    // Dont do anything unless we actually have a step interval
    if (!_stepInterval)
        return false;

    //unsigned long time = micros();
    unsigned long time = t.read_us();
    unsigned long nextStepTime = _lastStepTime + _stepInterval;
    // Gymnastics to detect wrapping of either the nextStepTime and/or the current time
    if (   ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime)))
            || ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime)))) {
        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;
}

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.
bool AccelStepper::run()
{
    if (runSpeed())
        computeNewSpeed();
    return _speed != 0.0 || distanceToGo() != 0;
}

AccelStepper::AccelStepper(uint8_t interface, PinName pin1, PinName pin2, PinName pin3, PinName 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;
    _lastStepTime = 0;
    // _pin[0] = pin1;
    // _pin[1] = pin2;
    // _pin[2] = pin3;
    // _pin[3] = pin4;
    _pin0 = new DigitalOut(pin1);
    _pin1 = new DigitalOut(pin2);
    _pin2 = new DigitalOut(pin3);
    _pin3 = new DigitalOut(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;
    _lastStepTime = 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();
        }
    }
}

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]));
    *_pin0 = (mask & (1 << 0)) ? (HIGH ^ _pinInverted[0]) : (LOW ^ _pinInverted[0]);
    *_pin1 = (mask & (1 << 1)) ? (HIGH ^ _pinInverted[1]) : (LOW ^ _pinInverted[1]);
    if (_interface == FULL4WIRE || _interface == HALF4WIRE) {
        *_pin2 = (mask & (1 << 2)) ? (HIGH ^ _pinInverted[2]) : (LOW ^ _pinInverted[2]);
        *_pin3 = (mask & (1 << 3)) ? (HIGH ^ _pinInverted[3]) : (LOW ^ _pinInverted[3]);
    } else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
        *_pin2 = (mask & (1 << 2)) ? (HIGH ^ _pinInverted[2]) : (LOW ^ _pinInverted[2]);
}

// 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);
    wait_us(_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)
    if (_enablePin)
        // digitalWrite(_enablePin, LOW ^ _enableInverted);
        *_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)
    if (_enablePin) {
        //pinMode(_enablePin, OUTPUT);
        //digitalWrite(_enablePin, HIGH ^ _enableInverted);
        *_enablePin = HIGH ^ _enableInverted;
    }
}

void AccelStepper::setMinPulseWidth(unsigned int minWidth)
{
    _minPulseWidth = minWidth;
}

// void AccelStepper::setEnablePin(uint8_t enablePin)
void AccelStepper::setEnablePin(PinName enablePin)
{
    // _enablePin = enablePin;
    _enablePin = new DigitalOut(enablePin);

    // This happens after construction, so init pin now.
    // if (_enablePin != 0xff)
    if (*_enablePin) {
        //pinMode(_enablePin, OUTPUT);
        //digitalWrite(_enablePin, HIGH ^ _enableInverted);
        *_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())
        ;
}

bool 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);
    }
}