AccelStepper.cpp

00001 // AccelStepper.cpp
00002 //
00003 // Copyright (C) 2009-2013 Mike McCauley
00004 // $Id: AccelStepper.cpp,v 1.19 2014/10/31 06:05:27 mikem Exp mikem $
00005 
00006 #include "AccelStepper.h"
00007 
00008 #if 0
00009 // Some debugging assistance
00010 void dump(uint8_t* p, int l)
00011 {
00012     int i;
00013 
00014     for (i = 0; i < l; i++) {
00015         Serial.print(p[i], HEX);
00016         Serial.print(" ");
00017     }
00018     Serial.println("");
00019 }
00020 #endif
00021 
00022 void AccelStepper::newSpeed()
00023 {
00024     computeNewSpeed();
00025 }
00026 
00027 
00028 void AccelStepper::moveTo(long absolute)
00029 {
00030     if (_targetPos != absolute) {
00031         _targetPos = absolute;
00032         computeNewSpeed();
00033     }
00034 }
00035 
00036 void AccelStepper::move(long relative)
00037 {
00038     moveTo(_currentPos + relative);
00039 }
00040 
00041 // Implements steps according to the current step interval
00042 // You must call this at least once per step
00043 // returns true if a step occurred
00044 bool AccelStepper::runSpeed()
00045 {
00046     extern Timer t;
00047     // Dont do anything unless we actually have a step interval
00048     if (!_stepInterval)
00049         return false;
00050 
00051     //unsigned long time = micros();
00052     unsigned long time = t.read_us();
00053     unsigned long nextStepTime = _lastStepTime + _stepInterval;
00054     // Gymnastics to detect wrapping of either the nextStepTime and/or the current time
00055     if (   ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime)))
00056             || ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime)))) {
00057         if (_direction == DIRECTION_CW) {
00058             // Clockwise
00059             _currentPos += 1;
00060         } else {
00061             // Anticlockwise
00062             _currentPos -= 1;
00063         }
00064         step(_currentPos);
00065 
00066         _lastStepTime = time;
00067         return true;
00068     } else {
00069         return false;
00070     }
00071 }
00072 
00073 long AccelStepper::distanceToGo()
00074 {
00075     return _targetPos - _currentPos;
00076 }
00077 
00078 long AccelStepper::targetPosition()
00079 {
00080     return _targetPos;
00081 }
00082 
00083 long AccelStepper::currentPosition()
00084 {
00085     return _currentPos;
00086 }
00087 
00088 // Useful during initialisations or after initial positioning
00089 // Sets speed to 0
00090 void AccelStepper::setCurrentPosition(long position)
00091 {
00092     _targetPos = _currentPos = position;
00093     _n = 0;
00094     _stepInterval = 0;
00095 }
00096 
00097 void AccelStepper::computeNewSpeed()
00098 {
00099     long distanceTo = distanceToGo(); // +ve is clockwise from curent location
00100 
00101     long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
00102 
00103     if (distanceTo == 0 && stepsToStop <= 1) {
00104         // We are at the target and its time to stop
00105         _stepInterval = 0;
00106         _speed = 0.0;
00107         _n = 0;
00108         return;
00109     }
00110 
00111     if (distanceTo > 0) {
00112         // We are anticlockwise from the target
00113         // Need to go clockwise from here, maybe decelerate now
00114         if (_n > 0) {
00115             // Currently accelerating, need to decel now? Or maybe going the wrong way?
00116             if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW)
00117                 _n = -stepsToStop; // Start deceleration
00118         } else if (_n < 0) {
00119             // Currently decelerating, need to accel again?
00120             if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW)
00121                 _n = -_n; // Start accceleration
00122         }
00123     } else if (distanceTo < 0) {
00124         // We are clockwise from the target
00125         // Need to go anticlockwise from here, maybe decelerate
00126         if (_n > 0) {
00127             // Currently accelerating, need to decel now? Or maybe going the wrong way?
00128             if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW)
00129                 _n = -stepsToStop; // Start deceleration
00130         } else if (_n < 0) {
00131             // Currently decelerating, need to accel again?
00132             if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW)
00133                 _n = -_n; // Start accceleration
00134         }
00135     }
00136 
00137     // Need to accelerate or decelerate
00138     if (_n == 0) {
00139         // First step from stopped
00140         _cn = _c0;
00141         _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW;
00142     } else {
00143         // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
00144         _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13
00145         _cn = max(_cn, _cmin);
00146     }
00147     _n++;
00148     _stepInterval = _cn;
00149     _speed = 1000000.0 / _cn;
00150     if (_direction == DIRECTION_CCW)
00151         _speed = -_speed;
00152 
00153 #if 0
00154     Serial.println(_speed);
00155     Serial.println(_acceleration);
00156     Serial.println(_cn);
00157     Serial.println(_c0);
00158     Serial.println(_n);
00159     Serial.println(_stepInterval);
00160     Serial.println(distanceTo);
00161     Serial.println(stepsToStop);
00162     Serial.println("-----");
00163 #endif
00164 }
00165 
00166 // Run the motor to implement speed and acceleration in order to proceed to the target position
00167 // You must call this at least once per step, preferably in your main loop
00168 // If the motor is in the desired position, the cost is very small
00169 // returns true if the motor is still running to the target position.
00170 bool AccelStepper::run()
00171 {
00172     if (runSpeed())
00173         computeNewSpeed();
00174     return _speed != 0.0 || distanceToGo() != 0;
00175 }
00176 
00177 AccelStepper::AccelStepper(uint8_t interface, PinName pin1, PinName pin2, PinName pin3, PinName pin4, bool enable)
00178 {
00179     _interface = interface;
00180     _currentPos = 0;
00181     _targetPos = 0;
00182     _speed = 0.0;
00183     _maxSpeed = 1.0;
00184     _acceleration = 0.0;
00185     _sqrt_twoa = 1.0;
00186     _stepInterval = 0;
00187     _minPulseWidth = 1;
00188     _lastStepTime = 0;
00189     // _pin[0] = pin1;
00190     // _pin[1] = pin2;
00191     // _pin[2] = pin3;
00192     // _pin[3] = pin4;
00193     _pin0 = new DigitalOut(pin1);
00194     _pin1 = new DigitalOut(pin2);
00195     _pin2 = new DigitalOut(pin3);
00196     _pin3 = new DigitalOut(pin4);
00197 
00198     // NEW
00199     _n = 0;
00200     _c0 = 0.0;
00201     _cn = 0.0;
00202     _cmin = 1.0;
00203     _direction = DIRECTION_CCW;
00204 
00205     int i;
00206     for (i = 0; i < 4; i++)
00207         _pinInverted[i] = 0;
00208     if (enable)
00209         enableOutputs();
00210     // Some reasonable default
00211     setAcceleration(1);
00212 }
00213 
00214 AccelStepper::AccelStepper(void (*forward)(), void (*backward)())
00215 {
00216     _interface = 0;
00217     _currentPos = 0;
00218     _targetPos = 0;
00219     _speed = 0.0;
00220     _maxSpeed = 1.0;
00221     _acceleration = 0.0;
00222     _sqrt_twoa = 1.0;
00223     _stepInterval = 0;
00224     _minPulseWidth = 1;
00225     _lastStepTime = 0;
00226     _forward = forward;
00227     _backward = backward;
00228 
00229     // NEW
00230     _n = 0;
00231     _c0 = 0.0;
00232     _cn = 0.0;
00233     _cmin = 1.0;
00234     _direction = DIRECTION_CCW;
00235 
00236     int i;
00237     for (i = 0; i < 4; i++)
00238         _pinInverted[i] = 0;
00239     // Some reasonable default
00240     setAcceleration(1);
00241 }
00242 
00243 void AccelStepper::setMaxSpeed(float speed)
00244 {
00245     if (_maxSpeed != speed) {
00246         _maxSpeed = speed;
00247         _cmin = 1000000.0 / speed;
00248         // Recompute _n from current speed and adjust speed if accelerating or cruising
00249         if (_n > 0) {
00250             _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16
00251             computeNewSpeed();
00252         }
00253     }
00254 }
00255 
00256 void AccelStepper::setAcceleration(float acceleration)
00257 {
00258     if (acceleration == 0.0)
00259         return;
00260     if (_acceleration != acceleration) {
00261         // Recompute _n per Equation 17
00262         _n = _n * (_acceleration / acceleration);
00263         // New c0 per Equation 7, with correction per Equation 15
00264         _c0 = 0.676 * sqrt(2.0 / acceleration) * 1000000.0; // Equation 15
00265         _acceleration = acceleration;
00266         computeNewSpeed();
00267     }
00268 }
00269 
00270 void AccelStepper::setSpeed(float speed)
00271 {
00272     if (speed == _speed)
00273         return;
00274     speed = constrain(speed, -_maxSpeed, _maxSpeed);
00275     if (speed == 0.0)
00276         _stepInterval = 0;
00277     else {
00278         _stepInterval = fabs(1000000.0 / speed);
00279         _direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW;
00280     }
00281     _speed = speed;
00282 }
00283 
00284 float AccelStepper::speed()
00285 {
00286     return _speed;
00287 }
00288 
00289 // Subclasses can override
00290 void AccelStepper::step(long step)
00291 {
00292     switch (_interface) {
00293         case FUNCTION:
00294             step0(step);
00295             break;
00296 
00297         case DRIVER:
00298             step1(step);
00299             break;
00300 
00301         case FULL2WIRE:
00302             step2(step);
00303             break;
00304 
00305         case FULL3WIRE:
00306             step3(step);
00307             break;
00308 
00309         case FULL4WIRE:
00310             step4(step);
00311             break;
00312 
00313         case HALF3WIRE:
00314             step6(step);
00315             break;
00316 
00317         case HALF4WIRE:
00318             step8(step);
00319             break;
00320     }
00321 }
00322 
00323 // You might want to override this to implement eg serial output
00324 // bit 0 of the mask corresponds to _pin[0]
00325 // bit 1 of the mask corresponds to _pin[1]
00326 // ....
00327 void AccelStepper::setOutputPins(uint8_t mask)
00328 {
00329 //    uint8_t numpins = 2;
00330 //    if (_interface == FULL4WIRE || _interface == HALF4WIRE)
00331 //        numpins = 4;
00332 //    else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
00333 //        numpins = 3;
00334 //    uint8_t i;
00335 //    for (i = 0; i < numpins; i++)
00336 //        digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i]));
00337     *_pin0 = (mask & (1 << 0)) ? (HIGH ^ _pinInverted[0]) : (LOW ^ _pinInverted[0]);
00338     *_pin1 = (mask & (1 << 1)) ? (HIGH ^ _pinInverted[1]) : (LOW ^ _pinInverted[1]);
00339     if (_interface == FULL4WIRE || _interface == HALF4WIRE) {
00340         *_pin2 = (mask & (1 << 2)) ? (HIGH ^ _pinInverted[2]) : (LOW ^ _pinInverted[2]);
00341         *_pin3 = (mask & (1 << 3)) ? (HIGH ^ _pinInverted[3]) : (LOW ^ _pinInverted[3]);
00342     } else if (_interface == FULL3WIRE || _interface == HALF3WIRE)
00343         *_pin2 = (mask & (1 << 2)) ? (HIGH ^ _pinInverted[2]) : (LOW ^ _pinInverted[2]);
00344 }
00345 
00346 // 0 pin step function (ie for functional usage)
00347 void AccelStepper::step0(long step)
00348 {
00349     if (_speed > 0)
00350         _forward();
00351     else
00352         _backward();
00353 }
00354 
00355 // 1 pin step function (ie for stepper drivers)
00356 // This is passed the current step number (0 to 7)
00357 // Subclasses can override
00358 void AccelStepper::step1(long step)
00359 {
00360     // _pin[0] is step, _pin[1] is direction
00361     setOutputPins(_direction ? 0b10 : 0b00); // Set direction first else get rogue pulses
00362     setOutputPins(_direction ? 0b11 : 0b01); // step HIGH
00363     // Caution 200ns setup time
00364     // Delay the minimum allowed pulse width
00365     //delayMicroseconds(_minPulseWidth);
00366     wait_us(_minPulseWidth);
00367     setOutputPins(_direction ? 0b10 : 0b00); // step LOW
00368 
00369 }
00370 
00371 
00372 // 2 pin step function
00373 // This is passed the current step number (0 to 7)
00374 // Subclasses can override
00375 void AccelStepper::step2(long step)
00376 {
00377     switch (step & 0x3) {
00378         case 0: /* 01 */
00379             setOutputPins(0b10);
00380             break;
00381 
00382         case 1: /* 11 */
00383             setOutputPins(0b11);
00384             break;
00385 
00386         case 2: /* 10 */
00387             setOutputPins(0b01);
00388             break;
00389 
00390         case 3: /* 00 */
00391             setOutputPins(0b00);
00392             break;
00393     }
00394 }
00395 // 3 pin step function
00396 // This is passed the current step number (0 to 7)
00397 // Subclasses can override
00398 void AccelStepper::step3(long step)
00399 {
00400     switch (step % 3) {
00401         case 0:    // 100
00402             setOutputPins(0b100);
00403             break;
00404 
00405         case 1:    // 001
00406             setOutputPins(0b001);
00407             break;
00408 
00409         case 2:    //010
00410             setOutputPins(0b010);
00411             break;
00412 
00413     }
00414 }
00415 
00416 // 4 pin step function for half stepper
00417 // This is passed the current step number (0 to 7)
00418 // Subclasses can override
00419 void AccelStepper::step4(long step)
00420 {
00421     switch (step & 0x3) {
00422         case 0:    // 1010
00423             setOutputPins(0b0101);
00424             break;
00425 
00426         case 1:    // 0110
00427             setOutputPins(0b0110);
00428             break;
00429 
00430         case 2:    //0101
00431             setOutputPins(0b1010);
00432             break;
00433 
00434         case 3:    //1001
00435             setOutputPins(0b1001);
00436             break;
00437     }
00438 }
00439 
00440 // 3 pin half step function
00441 // This is passed the current step number (0 to 7)
00442 // Subclasses can override
00443 void AccelStepper::step6(long step)
00444 {
00445     switch (step % 6) {
00446         case 0:    // 100
00447             setOutputPins(0b100);
00448             break;
00449 
00450         case 1:    // 101
00451             setOutputPins(0b101);
00452             break;
00453 
00454         case 2:    // 001
00455             setOutputPins(0b001);
00456             break;
00457 
00458         case 3:    // 011
00459             setOutputPins(0b011);
00460             break;
00461 
00462         case 4:    // 010
00463             setOutputPins(0b010);
00464             break;
00465 
00466         case 5:    // 011
00467             setOutputPins(0b110);
00468             break;
00469 
00470     }
00471 }
00472 
00473 // 4 pin half step function
00474 // This is passed the current step number (0 to 7)
00475 // Subclasses can override
00476 void AccelStepper::step8(long step)
00477 {
00478     switch (step & 0x7) {
00479         case 0:    // 1000
00480             setOutputPins(0b0001);
00481             break;
00482 
00483         case 1:    // 1010
00484             setOutputPins(0b0101);
00485             break;
00486 
00487         case 2:    // 0010
00488             setOutputPins(0b0100);
00489             break;
00490 
00491         case 3:    // 0110
00492             setOutputPins(0b0110);
00493             break;
00494 
00495         case 4:    // 0100
00496             setOutputPins(0b0010);
00497             break;
00498 
00499         case 5:    //0101
00500             setOutputPins(0b1010);
00501             break;
00502 
00503         case 6:    // 0001
00504             setOutputPins(0b1000);
00505             break;
00506 
00507         case 7:    //1001
00508             setOutputPins(0b1001);
00509             break;
00510     }
00511 }
00512 
00513 // Prevents power consumption on the outputs
00514 void    AccelStepper::disableOutputs()
00515 {
00516     if (! _interface) return;
00517 
00518     setOutputPins(0); // Handles inversion automatically
00519     // if (_enablePin != 0xff)
00520     if (_enablePin)
00521         // digitalWrite(_enablePin, LOW ^ _enableInverted);
00522         *_enablePin = LOW ^ _enableInverted;
00523 }
00524 
00525 void    AccelStepper::enableOutputs()
00526 {
00527     if (! _interface)
00528         return;
00529 
00530     //pinMode(_pin[0], OUTPUT);
00531     //pinMode(_pin[1], OUTPUT);
00532     if (_interface == FULL4WIRE || _interface == HALF4WIRE) {
00533         //pinMode(_pin[2], OUTPUT);
00534         //pinMode(_pin[3], OUTPUT);
00535     } else if (_interface == FULL3WIRE || _interface == HALF3WIRE) {
00536         //pinMode(_pin[2], OUTPUT);
00537     }
00538 
00539     // if (_enablePin != 0xff)
00540     if (_enablePin) {
00541         //pinMode(_enablePin, OUTPUT);
00542         //digitalWrite(_enablePin, HIGH ^ _enableInverted);
00543         *_enablePin = HIGH ^ _enableInverted;
00544     }
00545 }
00546 
00547 void AccelStepper::setMinPulseWidth(unsigned int minWidth)
00548 {
00549     _minPulseWidth = minWidth;
00550 }
00551 
00552 // void AccelStepper::setEnablePin(uint8_t enablePin)
00553 void AccelStepper::setEnablePin(PinName enablePin)
00554 {
00555     // _enablePin = enablePin;
00556     _enablePin = new DigitalOut(enablePin);
00557 
00558     // This happens after construction, so init pin now.
00559     // if (_enablePin != 0xff)
00560     if (*_enablePin) {
00561         //pinMode(_enablePin, OUTPUT);
00562         //digitalWrite(_enablePin, HIGH ^ _enableInverted);
00563         *_enablePin = HIGH ^ _enableInverted;
00564     }
00565 }
00566 
00567 void AccelStepper::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert)
00568 {
00569     _pinInverted[0] = stepInvert;
00570     _pinInverted[1] = directionInvert;
00571     _enableInverted = enableInvert;
00572 }
00573 
00574 void AccelStepper::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert)
00575 {
00576     _pinInverted[0] = pin1Invert;
00577     _pinInverted[1] = pin2Invert;
00578     _pinInverted[2] = pin3Invert;
00579     _pinInverted[3] = pin4Invert;
00580     _enableInverted = enableInvert;
00581 }
00582 
00583 // Blocks until the target position is reached and stopped
00584 void AccelStepper::runToPosition()
00585 {
00586     while (run())
00587         ;
00588 }
00589 
00590 bool AccelStepper::runSpeedToPosition()
00591 {
00592     if (_targetPos == _currentPos)
00593         return false;
00594     if (_targetPos >_currentPos)
00595         _direction = DIRECTION_CW;
00596     else
00597         _direction = DIRECTION_CCW;
00598     return runSpeed();
00599 }
00600 
00601 // Blocks until the new target position is reached
00602 void AccelStepper::runToNewPosition(long position)
00603 {
00604     moveTo(position);
00605     runToPosition();
00606 }
00607 
00608 void AccelStepper::stop()
00609 {
00610     if (_speed != 0.0) {
00611         long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding)
00612         if (_speed > 0)
00613             move(stepsToStop);
00614         else
00615             move(-stepsToStop);
00616     }
00617 }