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