Matteus Car
/
motorController2
AccelMotor for Mbed
AccelStepper.cpp
- Committer:
- yweng36
- Date:
- 2016-02-25
- Revision:
- 0:a0391774b0e5
File content as of revision 0:a0391774b0e5:
// 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); } }