Diff: AccelStepper.cpp

Revision:

3:086f8c1079ff

Parent:

2:d4dad64faadb

--- a/AccelStepper.cpp	Sat Feb 03 19:25:25 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,651 +0,0 @@
-// 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::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
-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);
-    }
-}
-