4653
Dependencies: mbed-STM32F103C8T6 mbed USBDevice_STM32F103
Diff: AccelStepper.cpp
- Revision:
- 3:086f8c1079ff
- Parent:
- 2:d4dad64faadb
diff -r d4dad64faadb -r 086f8c1079ff AccelStepper.cpp --- 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); - } -} -