Diff: AccelStepper.cpp

Revision:

11:f44fc4db4ab1

diff -r 4b88be251088 -r f44fc4db4ab1 AccelStepper.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AccelStepper.cpp	Tue May 23 17:28:21 2017 +0000
@@ -0,0 +1,651 @@
+// 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);
+    }
+}
+