├── AccelStepperEncoder.cpp ├── AccelStepperEncoder.h ├── LICENSE ├── README.md └── keywords.txt /AccelStepperEncoder.cpp: -------------------------------------------------------------------------------- 1 | // AccelStepperEncoder.cpp 2 | // 3 | // Copyright (C) 2009-2013 Mike McCauley 4 | // $Id: AccelStepperEncoder.cpp,v 1.17 2013/08/02 01:53:21 mikem Exp mikem $ 5 | 6 | #include "AccelStepperEncoder.h" 7 | 8 | //#define DEBUG 9 | 10 | #if 0 11 | // Some debugging assistance 12 | void dump(uint8_t* p, int l) 13 | { 14 | int i; 15 | 16 | for (i = 0; i < l; i++) 17 | { 18 | Serial.print(p[i], HEX); 19 | Serial.print(""); 20 | } 21 | Serial.println(""); 22 | } 23 | #endif 24 | 25 | void AccelStepperEncoder::moveTo(long absolute) 26 | { 27 | if (_targetPos != absolute) 28 | { 29 | _targetPos = absolute; 30 | _encoderTargetPos = encoderPositionForMotor(_targetPos); 31 | computeNewSpeed(); 32 | // compute new n? 33 | } 34 | } 35 | 36 | float AccelStepperEncoder::encoderPositionForMotor(long motorPos) 37 | { 38 | return (float)motorPos / _motorToEncoderRatio; 39 | } 40 | 41 | void AccelStepperEncoder::move(long relative) 42 | { 43 | moveTo(_currentPos + relative); 44 | } 45 | 46 | // Implements steps according to the current step interval 47 | // You must call this at least once per step 48 | // returns true if a step occurred 49 | boolean AccelStepperEncoder::runSpeed() 50 | { 51 | // Dont do anything unless we actually have a step interval 52 | if (!_stepInterval) 53 | return false; 54 | 55 | unsigned long time = micros(); 56 | // Gymnastics to detect wrapping of either the nextStepTime and/or the current time 57 | unsigned long nextStepTime = _lastStepTime + _stepInterval; 58 | if ( ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime))) 59 | || ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime)))) 60 | { 61 | if (_direction == DIRECTION_CW) 62 | { 63 | // Clockwise 64 | _currentPos += 1; 65 | } 66 | else 67 | { 68 | // Anticlockwise 69 | _currentPos -= 1; 70 | } 71 | step(_currentPos); 72 | 73 | _lastStepTime = time; 74 | return true; 75 | } 76 | else 77 | { 78 | return false; 79 | } 80 | } 81 | 82 | long AccelStepperEncoder::distanceToGo() 83 | { 84 | return _targetPos - _currentPos; 85 | } 86 | 87 | long AccelStepperEncoder::targetPosition() 88 | { 89 | return _targetPos; 90 | } 91 | 92 | long AccelStepperEncoder::currentPosition() 93 | { 94 | return _currentPos; 95 | } 96 | 97 | // Useful during initialisations or after initial positioning 98 | // Sets speed to 0 99 | void AccelStepperEncoder::setCurrentPosition(long position) 100 | { 101 | _targetPos = _currentPos = position; 102 | _n = 0; 103 | _stepInterval = 0; 104 | } 105 | 106 | void AccelStepperEncoder::computeNewSpeed() 107 | { 108 | long distanceTo = distanceToGo(); // +ve is clockwise from c urent location 109 | 110 | long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16 111 | 112 | if (distanceTo == 0 && stepsToStop <= 1) 113 | { 114 | // We are at the target and its time to stop 115 | _stepInterval = 0; 116 | _speed = 0.0; 117 | _n = 0; 118 | return; 119 | } 120 | 121 | if (distanceTo > 0) 122 | { 123 | // We are anticlockwise from the target 124 | // Need to go clockwise from here, maybe decelerate now 125 | if (_n > 0) 126 | { 127 | // Currently accelerating, need to decel now? Or maybe going the wrong way? 128 | if ((stepsToStop >= distanceTo) || _direction == DIRECTION_CCW) 129 | _n = -stepsToStop; // Start deceleration 130 | } 131 | else if (_n < 0) 132 | { 133 | // Currently decelerating, need to accel again? 134 | if ((stepsToStop < distanceTo) && _direction == DIRECTION_CW) 135 | _n = -_n; // Start accceleration 136 | } 137 | } 138 | else if (distanceTo < 0) 139 | { 140 | // We are clockwise from the target 141 | // Need to go anticlockwise from here, maybe decelerate 142 | if (_n > 0) 143 | { 144 | // Currently accelerating, need to decel now? Or maybe going the wrong way? 145 | if ((stepsToStop >= -distanceTo) || _direction == DIRECTION_CW) 146 | _n = -stepsToStop; // Start deceleration 147 | } 148 | else if (_n < 0) 149 | { 150 | // Currently decelerating, need to accel again? 151 | if ((stepsToStop < -distanceTo) && _direction == DIRECTION_CCW) 152 | _n = -_n; // Start accceleration 153 | } 154 | } 155 | 156 | // Need to accelerate or decelerate 157 | if (_n == 0) 158 | { 159 | // First step from stopped 160 | _cn = _c0; 161 | _direction = (distanceTo > 0) ? DIRECTION_CW : DIRECTION_CCW; 162 | } 163 | else 164 | { 165 | // Subsequent step. Works for accel (n is +_ve) and decel (n is -ve). 166 | _cn = _cn - ((2.0 * _cn) / ((4.0 * _n) + 1)); // Equation 13 167 | _cn = max(_cn, _cmin); 168 | } 169 | _n++; 170 | _stepInterval = _cn; 171 | _speed = 1000000.0 / _cn; 172 | if (_direction == DIRECTION_CCW) 173 | _speed = -_speed; 174 | 175 | #ifdef DEBUG_ACCEL 176 | Serial.print("_speed: "); 177 | Serial.println(_speed); 178 | Serial.print("_acceleration: "); 179 | Serial.println(_acceleration); 180 | Serial.print("_cn: "); 181 | Serial.println(_cn); 182 | Serial.print("_c0: "); 183 | Serial.println(_c0); 184 | Serial.print("_n: "); 185 | Serial.println(_n); 186 | Serial.print("_stepInterval: "); 187 | Serial.println(_stepInterval); 188 | Serial.print("distanceTo: "); 189 | Serial.println(distanceTo); 190 | Serial.print("stepsToStop: "); 191 | Serial.println(stepsToStop); 192 | Serial.print("_currentPos: "); 193 | Serial.println(_currentPos); 194 | Serial.print("readEnc(): "); 195 | Serial.println(readEnc()); 196 | 197 | Serial.println("-----"); 198 | #endif 199 | } 200 | 201 | // Run the motor to implement speed and acceleration in order to proceed to the target position 202 | // You must call this at least once per step, preferably in your main loop 203 | // If the motor is in the desired position, the cost is very small 204 | // returns true if the motor is still running to the target position. 205 | boolean AccelStepperEncoder::run() 206 | { 207 | if (runSpeed()) 208 | { 209 | computeNewSpeed(); 210 | } 211 | return _speed != 0.0 || distanceToGo() != 0; 212 | } 213 | 214 | // Look at motor position vs actual position, and correct motor position if necessary. 215 | float AccelStepperEncoder::computeDeviation() 216 | { 217 | float expectedEncPos = encoderPositionForMotor(_currentPos); 218 | float actualEncPos = readEnc(); 219 | float deviation = actualEncPos - expectedEncPos; 220 | 221 | if (deviation > maxDeviation) maxDeviation = deviation; 222 | else if (deviation < minDeviation) minDeviation = deviation; 223 | 224 | #ifdef DEBUG_DEV 225 | Serial.print("expectedEncPos: "); 226 | Serial.println(expectedEncPos); 227 | Serial.print("actualEncPos: "); 228 | Serial.println(actualEncPos); 229 | Serial.print("deviation: "); 230 | Serial.print(deviation); 231 | Serial.print(" (min: "); 232 | Serial.print(minDeviation); 233 | Serial.print(", max: "); 234 | Serial.print(maxDeviation); 235 | Serial.println(")"); 236 | Serial.println("~~~~~"); 237 | #endif 238 | return deviation; 239 | } 240 | 241 | float AccelStepperEncoder::correctDeviation() 242 | { 243 | float deviation = computeDeviation(); 244 | if (abs(deviation) > acceptableDeviation) 245 | { 246 | #ifdef DEBUG 247 | Serial.print("CORRECTING DEVIATION!"); 248 | Serial.println(deviation); 249 | #endif 250 | synchroniseMotorWithEncoder(); 251 | } 252 | else 253 | { 254 | #ifdef DEBUG 255 | //Serial.print("Deviation is not enough to fix: "); 256 | //Serial.println(deviation); 257 | #endif 258 | } 259 | return deviation; 260 | } 261 | 262 | /* 263 | Resets the motor position to reflect the actual position (as got via encoder) 264 | */ 265 | void AccelStepperEncoder::synchroniseMotorWithEncoder() 266 | { 267 | #ifdef DEBUG 268 | Serial.print("CurrentPos was: "); 269 | Serial.print(_currentPos); 270 | #endif 271 | _currentPos = readEnc() * _motorToEncoderRatio; 272 | #ifdef DEBUG 273 | Serial.print(", is now set to: "); 274 | Serial.println(_currentPos); 275 | #endif 276 | _targetPos = _currentPos; 277 | 278 | //setSpeed(0); 279 | } 280 | 281 | AccelStepperEncoder::AccelStepperEncoder(uint8_t interface, uint8_t pin1, uint8_t pin2, uint8_t pin3, uint8_t pin4, bool enable) 282 | { 283 | _interface = interface; 284 | _currentPos = 0; 285 | _targetPos = 0; 286 | _speed = 0.0; 287 | _maxSpeed = 1.0; 288 | _acceleration = 1.0; 289 | _sqrt_twoa = 1.0; 290 | _stepInterval = 0; 291 | _minPulseWidth = 1; 292 | _enablePin = 0xff; 293 | _lastStepTime = 0; 294 | _pin[0] = pin1; 295 | _pin[1] = pin2; 296 | _pin[2] = pin3; 297 | _pin[3] = pin4; 298 | 299 | // NEW 300 | _n = 0; 301 | _c0 = 0.0; 302 | _cn = 0.0; 303 | _cmin = 1.0; 304 | _direction = DIRECTION_CCW; 305 | 306 | int i; 307 | for (i = 0; i < 4; i++) 308 | _pinInverted[i] = 0; 309 | if (enable) 310 | enableOutputs(); 311 | } 312 | 313 | AccelStepperEncoder::AccelStepperEncoder(void (*forward)(), void (*backward)()) 314 | { 315 | _interface = 0; 316 | _currentPos = 0; 317 | _targetPos = 0; 318 | _speed = 0.0; 319 | _maxSpeed = 1.0; 320 | _acceleration = 1.0; 321 | _sqrt_twoa = 1.0; 322 | _stepInterval = 0; 323 | _minPulseWidth = 1; 324 | _enablePin = 0xff; 325 | _lastStepTime = 0; 326 | _pin[0] = 0; 327 | _pin[1] = 0; 328 | _pin[2] = 0; 329 | _pin[3] = 0; 330 | _forward = forward; 331 | _backward = backward; 332 | 333 | // NEW 334 | _n = 0; 335 | _c0 = 0.0; 336 | _cn = 0.0; 337 | _cmin = 1.0; 338 | _direction = DIRECTION_CCW; 339 | 340 | int i; 341 | for (i = 0; i < 4; i++) 342 | _pinInverted[i] = 0; 343 | } 344 | 345 | void AccelStepperEncoder::setMaxSpeed(float speed) 346 | { 347 | if (_maxSpeed != speed) 348 | { 349 | _maxSpeed = speed; 350 | _cmin = 1000000.0 / speed; 351 | // Recompute _n from current speed and adjust speed if accelerating or cruising 352 | if (_n > 0) 353 | { 354 | _n = (long)((_speed * _speed) / (2.0 * _acceleration)); // Equation 16 355 | computeNewSpeed(); 356 | } 357 | } 358 | } 359 | 360 | void AccelStepperEncoder::setAcceleration(float acceleration) 361 | { 362 | if (acceleration == 0.0) 363 | return; 364 | if (_acceleration != acceleration) 365 | { 366 | // Recompute _n per Equation 17 367 | _n = _n * (_acceleration / acceleration); 368 | // New c0 per Equation 7 369 | _c0 = sqrt(2.0 / acceleration) * 1000000.0; 370 | _acceleration = acceleration; 371 | computeNewSpeed(); 372 | } 373 | } 374 | 375 | void AccelStepperEncoder::setSpeed(float speed) 376 | { 377 | if (speed == _speed) 378 | return; 379 | speed = constrain(speed, -_maxSpeed, _maxSpeed); 380 | if (speed == 0.0) 381 | _stepInterval = 0; 382 | else 383 | { 384 | _stepInterval = fabs(1000000.0 / speed); 385 | _direction = (speed > 0.0) ? DIRECTION_CW : DIRECTION_CCW; 386 | } 387 | _speed = speed; 388 | } 389 | 390 | float AccelStepperEncoder::speed() 391 | { 392 | return _speed; 393 | } 394 | 395 | // Subclasses can override 396 | void AccelStepperEncoder::step(long step) 397 | { 398 | switch (_interface) 399 | { 400 | case FUNCTION: 401 | step0(step); 402 | break; 403 | 404 | case DRIVER: 405 | step1(step); 406 | break; 407 | 408 | case FULL2WIRE: 409 | step2(step); 410 | break; 411 | 412 | case FULL3WIRE: 413 | step3(step); 414 | break; 415 | 416 | case FULL4WIRE: 417 | step4(step); 418 | break; 419 | 420 | case HALF3WIRE: 421 | step6(step); 422 | break; 423 | 424 | case HALF4WIRE: 425 | step8(step); 426 | break; 427 | } 428 | } 429 | 430 | // You might want to override this to implement eg serial output 431 | // bit 0 of the mask corresponds to _pin[0] 432 | // bit 1 of the mask corresponds to _pin[1] 433 | // .... 434 | void AccelStepperEncoder::setOutputPins(uint8_t mask) 435 | { 436 | uint8_t numpins = 2; 437 | if (_interface == FULL4WIRE || _interface == HALF4WIRE) 438 | numpins = 4; 439 | uint8_t i; 440 | for (i = 0; i < numpins; i++) 441 | digitalWrite(_pin[i], (mask & (1 << i)) ? (HIGH ^ _pinInverted[i]) : (LOW ^ _pinInverted[i])); 442 | } 443 | 444 | // 0 pin step function (ie for functional usage) 445 | void AccelStepperEncoder::step0(long step) 446 | { 447 | if (_speed > 0) 448 | _forward(); 449 | else 450 | _backward(); 451 | } 452 | 453 | // 1 pin step function (ie for stepper drivers) 454 | // This is passed the current step number (0 to 7) 455 | // Subclasses can override 456 | void AccelStepperEncoder::step1(long step) 457 | { 458 | // _pin[0] is step, _pin[1] is direction 459 | setOutputPins(_direction ? 0b10 : 0b00); // Set direction first else get rogue pulses 460 | setOutputPins(_direction ? 0b11 : 0b01); // step HIGH 461 | // Caution 200ns setup time 462 | // Delay the minimum allowed pulse width 463 | delayMicroseconds(_minPulseWidth); 464 | setOutputPins(_direction ? 0b10 : 0b00); // step LOW 465 | 466 | } 467 | 468 | 469 | // 2 pin step function 470 | // This is passed the current step number (0 to 7) 471 | // Subclasses can override 472 | void AccelStepperEncoder::step2(long step) 473 | { 474 | switch (step & 0x3) 475 | { 476 | case 0: /* 01 */ 477 | setOutputPins(0b10); 478 | break; 479 | 480 | case 1: /* 11 */ 481 | setOutputPins(0b11); 482 | break; 483 | 484 | case 2: /* 10 */ 485 | setOutputPins(0b01); 486 | break; 487 | 488 | case 3: /* 00 */ 489 | setOutputPins(0b00); 490 | break; 491 | } 492 | } 493 | // 3 pin step function 494 | // This is passed the current step number (0 to 7) 495 | // Subclasses can override 496 | void AccelStepperEncoder::step3(long step) 497 | { 498 | switch (step % 3) 499 | { 500 | case 0: // 100 501 | setOutputPins(0b100); 502 | break; 503 | 504 | case 1: // 001 505 | setOutputPins(0b001); 506 | break; 507 | 508 | case 2: //010 509 | setOutputPins(0b010); 510 | break; 511 | 512 | } 513 | } 514 | 515 | // 4 pin step function for half stepper 516 | // This is passed the current step number (0 to 7) 517 | // Subclasses can override 518 | void AccelStepperEncoder::step4(long step) 519 | { 520 | switch (step & 0x3) 521 | { 522 | case 0: // 1010 523 | setOutputPins(0b0101); 524 | break; 525 | 526 | case 1: // 0110 527 | setOutputPins(0b0110); 528 | break; 529 | 530 | case 2: //0101 531 | setOutputPins(0b1010); 532 | break; 533 | 534 | case 3: //1001 535 | setOutputPins(0b1001); 536 | break; 537 | } 538 | } 539 | 540 | // 3 pin half step function 541 | // This is passed the current step number (0 to 7) 542 | // Subclasses can override 543 | void AccelStepperEncoder::step6(long step) 544 | { 545 | switch (step % 6) 546 | { 547 | case 0: // 100 548 | setOutputPins(0b100); 549 | break; 550 | 551 | case 1: // 101 552 | setOutputPins(0b101); 553 | break; 554 | 555 | case 2: // 001 556 | setOutputPins(0b001); 557 | break; 558 | 559 | case 3: // 011 560 | setOutputPins(0b011); 561 | break; 562 | 563 | case 4: // 010 564 | setOutputPins(0b010); 565 | break; 566 | 567 | case 5: // 011 568 | setOutputPins(0b110); 569 | break; 570 | 571 | } 572 | } 573 | 574 | // 4 pin half step function 575 | // This is passed the current step number (0 to 7) 576 | // Subclasses can override 577 | void AccelStepperEncoder::step8(long step) 578 | { 579 | switch (step & 0x7) 580 | { 581 | case 0: // 1000 582 | setOutputPins(0b0001); 583 | break; 584 | 585 | case 1: // 1010 586 | setOutputPins(0b0101); 587 | break; 588 | 589 | case 2: // 0010 590 | setOutputPins(0b0100); 591 | break; 592 | 593 | case 3: // 0110 594 | setOutputPins(0b0110); 595 | break; 596 | 597 | case 4: // 0100 598 | setOutputPins(0b0010); 599 | break; 600 | 601 | case 5: //0101 602 | setOutputPins(0b1010); 603 | break; 604 | 605 | case 6: // 0001 606 | setOutputPins(0b1000); 607 | break; 608 | 609 | case 7: //1001 610 | setOutputPins(0b1001); 611 | break; 612 | } 613 | } 614 | 615 | // Prevents power consumption on the outputs 616 | void AccelStepperEncoder::disableOutputs() 617 | { 618 | if (! _interface) return; 619 | 620 | setOutputPins(0); // Handles inversion automatically 621 | if (_enablePin != 0xff) 622 | digitalWrite(_enablePin, LOW ^ _enableInverted); 623 | } 624 | 625 | void AccelStepperEncoder::enableOutputs() 626 | { 627 | if (! _interface) 628 | return; 629 | 630 | pinMode(_pin[0], OUTPUT); 631 | pinMode(_pin[1], OUTPUT); 632 | if (_interface == FULL4WIRE || _interface == HALF4WIRE) 633 | { 634 | pinMode(_pin[2], OUTPUT); 635 | pinMode(_pin[3], OUTPUT); 636 | } 637 | 638 | if (_enablePin != 0xff) 639 | { 640 | pinMode(_enablePin, OUTPUT); 641 | digitalWrite(_enablePin, HIGH ^ _enableInverted); 642 | } 643 | } 644 | 645 | void AccelStepperEncoder::setMinPulseWidth(unsigned int minWidth) 646 | { 647 | _minPulseWidth = minWidth; 648 | } 649 | 650 | void AccelStepperEncoder::setEnablePin(uint8_t enablePin) 651 | { 652 | _enablePin = enablePin; 653 | 654 | // This happens after construction, so init pin now. 655 | if (_enablePin != 0xff) 656 | { 657 | pinMode(_enablePin, OUTPUT); 658 | digitalWrite(_enablePin, HIGH ^ _enableInverted); 659 | } 660 | } 661 | 662 | void AccelStepperEncoder::setPinsInverted(bool directionInvert, bool stepInvert, bool enableInvert) 663 | { 664 | _pinInverted[0] = stepInvert; 665 | _pinInverted[1] = directionInvert; 666 | _enableInverted = enableInvert; 667 | } 668 | 669 | void AccelStepperEncoder::setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert) 670 | { 671 | _pinInverted[0] = pin1Invert; 672 | _pinInverted[1] = pin2Invert; 673 | _pinInverted[2] = pin3Invert; 674 | _pinInverted[3] = pin4Invert; 675 | _enableInverted = enableInvert; 676 | } 677 | 678 | // Blocks until the target position is reached and stopped 679 | void AccelStepperEncoder::runToPosition() 680 | { 681 | while (run()) 682 | ; 683 | } 684 | 685 | boolean AccelStepperEncoder::runSpeedToPosition() 686 | { 687 | if (_targetPos == _currentPos) 688 | return false; 689 | if (_targetPos >_currentPos) 690 | _direction = DIRECTION_CW; 691 | else 692 | _direction = DIRECTION_CCW; 693 | return runSpeed(); 694 | } 695 | 696 | // Blocks until the new target position is reached 697 | void AccelStepperEncoder::runToNewPosition(long position) 698 | { 699 | moveTo(position); 700 | runToPosition(); 701 | } 702 | 703 | void AccelStepperEncoder::stop() 704 | { 705 | if (_speed != 0.0) 706 | { 707 | long stepsToStop = (long)((_speed * _speed) / (2.0 * _acceleration)) + 1; // Equation 16 (+integer rounding) 708 | if (_speed > 0) 709 | move(stepsToStop); 710 | else 711 | move(-stepsToStop); 712 | } 713 | } 714 | 715 | void AccelStepperEncoder::addEncoder(Encoder *enc, float ratio) 716 | { 717 | _motorToEncoderRatio = ratio; 718 | _enc = enc; 719 | writeEnc(0); 720 | } 721 | Encoder* AccelStepperEncoder::getEncoder() 722 | { 723 | return _enc; 724 | } 725 | long AccelStepperEncoder::readEnc() 726 | { 727 | return _enc->read(); 728 | } 729 | void AccelStepperEncoder::writeEnc(long value) 730 | { 731 | _enc->write(value); 732 | } -------------------------------------------------------------------------------- /AccelStepperEncoder.h: -------------------------------------------------------------------------------- 1 | // AccelStepperEncoder.h 2 | // 3 | /// \mainpage AccelStepperEncoder library for Arduino 4 | /// 5 | /// This is the Arduino AccelStepperEncoder library. 6 | /// It provides an object-oriented interface for 2, 3 or 4 pin stepper motors. 7 | /// 8 | /// The standard Arduino IDE includes the Stepper library 9 | /// (http://arduino.cc/en/Reference/Stepper) for stepper motors. It is 10 | /// perfectly adequate for simple, single motor applications. 11 | /// 12 | /// AccelStepperEncoder significantly improves on the standard Arduino Stepper library in several ways: 13 | /// \li Supports acceleration and deceleration 14 | /// \li Supports multiple simultaneous steppers, with independent concurrent stepping on each stepper 15 | /// \li API functions never delay() or block 16 | /// \li Supports 2, 3 and 4 wire steppers, plus 3 and 4 wire half steppers. 17 | /// \li Supports alternate stepping functions to enable support of AFMotor (https://github.com/adafruit/Adafruit-Motor-Shield-library) 18 | /// \li Supports stepper drivers such as the Sparkfun EasyDriver (based on 3967 driver chip) 19 | /// \li Very slow speeds are supported 20 | /// \li Extensive API 21 | /// \li Subclass support 22 | /// 23 | /// The latest version of this documentation can be downloaded from 24 | /// http://www.airspayce.com/mikem/arduino/AccelStepperEncoder 25 | /// The version of the package that this documentation refers to can be downloaded 26 | /// from http://www.airspayce.com/mikem/arduino/AccelStepperEncoder/AccelStepperEncoder-1.38.zip 27 | /// 28 | /// Example Arduino programs are included to show the main modes of use. 29 | /// 30 | /// You can also find online help and discussion at http://groups.google.com/group/AccelStepperEncoder 31 | /// Please use that group for all questions and discussions on this topic. 32 | /// Do not contact the author directly, unless it is to discuss commercial licensing. 33 | /// 34 | /// Tested on Arduino Diecimila and Mega with arduino-0018 & arduino-0021 35 | /// on OpenSuSE 11.1 and avr-libc-1.6.1-1.15, 36 | /// cross-avr-binutils-2.19-9.1, cross-avr-gcc-4.1.3_20080612-26.5. 37 | /// 38 | /// \par Installation 39 | /// Install in the usual way: unzip the distribution zip file to the libraries 40 | /// sub-folder of your sketchbook. 41 | /// 42 | /// \par Theory 43 | /// This code uses speed calculations as described in 44 | /// "Generate stepper-motor speed profiles in real time" by David Austin 45 | /// http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf 46 | /// with the exception that AccelStepperEncoder uses steps per second rather than radians per second 47 | /// (because we dont know the step angle of the motor) 48 | /// An initial step interval is calculated for the first step, based on the desired acceleration 49 | /// On subsequent steps, shorter step intervals are calculated based 50 | /// on the previous step until max speed is achieved. 51 | /// 52 | /// This software is Copyright (C) 2010 Mike McCauley. Use is subject to license 53 | /// conditions. The main licensing options available are GPL V2 or Commercial: 54 | /// 55 | /// \par Open Source Licensing GPL V2 56 | /// This is the appropriate option if you want to share the source code of your 57 | /// application with everyone you distribute it to, and you also want to give them 58 | /// the right to share who uses it. If you wish to use this software under Open 59 | /// Source Licensing, you must contribute all your source code to the open source 60 | /// community in accordance with the GPL Version 2 when your application is 61 | /// distributed. See http://www.gnu.org/copyleft/gpl.html 62 | /// 63 | /// \par Commercial Licensing 64 | /// This is the appropriate option if you are creating proprietary applications 65 | /// and you are not prepared to distribute and share the source code of your 66 | /// application. Contact info@airspayce.com for details. 67 | /// 68 | /// \par Revision History 69 | /// \version 1.0 Initial release 70 | /// 71 | /// \version 1.1 Added speed() function to get the current speed. 72 | /// \version 1.2 Added runSpeedToPosition() submitted by Gunnar Arndt. 73 | /// \version 1.3 Added support for stepper drivers (ie with Step and Direction inputs) with _pins == 1 74 | /// \version 1.4 Added functional contructor to support AFMotor, contributed by Limor, with example sketches. 75 | /// \version 1.5 Improvements contributed by Peter Mousley: Use of microsecond steps and other speed improvements 76 | /// to increase max stepping speed to about 4kHz. New option for user to set the min allowed pulse width. 77 | /// Added checks for already running at max speed and skip further calcs if so. 78 | /// \version 1.6 Fixed a problem with wrapping of microsecond stepping that could cause stepping to hang. 79 | /// Reported by Sandy Noble. 80 | /// Removed redundant _lastRunTime member. 81 | /// \version 1.7 Fixed a bug where setCurrentPosition() did not always work as expected. 82 | /// Reported by Peter Linhart. 83 | /// \version 1.8 Added support for 4 pin half-steppers, requested by Harvey Moon 84 | /// \version 1.9 setCurrentPosition() now also sets motor speed to 0. 85 | /// \version 1.10 Builds on Arduino 1.0 86 | /// \version 1.11 Improvments from Michael Ellison: 87 | /// Added optional enable line support for stepper drivers 88 | /// Added inversion for step/direction/enable lines for stepper drivers 89 | /// \version 1.12 Announce Google Group 90 | /// \version 1.13 Improvements to speed calculation. Cost of calculation is now less in the worst case, 91 | /// and more or less constant in all cases. This should result in slightly beter high speed performance, and 92 | /// reduce anomalous speed glitches when other steppers are accelerating. 93 | /// However, its hard to see how to replace the sqrt() required at the very first step from 0 speed. 94 | /// \version 1.14 Fixed a problem with compiling under arduino 0021 reported by EmbeddedMan 95 | /// \version 1.15 Fixed a problem with runSpeedToPosition which did not correctly handle 96 | /// running backwards to a smaller target position. Added examples 97 | /// \version 1.16 Fixed some cases in the code where abs() was used instead of fabs(). 98 | /// \version 1.17 Added example ProportionalControl 99 | /// \version 1.18 Fixed a problem: If one calls the funcion runSpeed() when Speed is zero, it makes steps 100 | /// without counting. reported by Friedrich, Klappenbach. 101 | /// \version 1.19 Added MotorInterfaceType and symbolic names for the number of pins to use 102 | /// for the motor interface. Updated examples to suit. 103 | /// Replaced individual pin assignment variables _pin1, _pin2 etc with array _pin[4]. 104 | /// _pins member changed to _interface. 105 | /// Added _pinInverted array to simplify pin inversion operations. 106 | /// Added new function setOutputPins() which sets the motor output pins. 107 | /// It can be overridden in order to provide, say, serial output instead of parallel output 108 | /// Some refactoring and code size reduction. 109 | /// \version 1.20 Improved documentation and examples to show need for correctly 110 | /// specifying AccelStepperEncoder::FULL4WIRE and friends. 111 | /// \version 1.21 Fixed a problem where desiredSpeed could compute the wrong step acceleration 112 | /// when _speed was small but non-zero. Reported by Brian Schmalz. 113 | /// Precompute sqrt_twoa to improve performance and max possible stepping speed 114 | /// \version 1.22 Added Bounce.pde example 115 | /// Fixed a problem where calling moveTo(), setMaxSpeed(), setAcceleration() more 116 | /// frequently than the step time, even 117 | /// with the same values, would interfere with speed calcs. Now a new speed is computed 118 | /// only if there was a change in the set value. Reported by Brian Schmalz. 119 | /// \version 1.23 Rewrite of the speed algorithms in line with 120 | /// http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf 121 | /// Now expect smoother and more linear accelerations and decelerations. The desiredSpeed() 122 | /// function was removed. 123 | /// \version 1.24 Fixed a problem introduced in 1.23: with runToPosition, which did never returned 124 | /// \version 1.25 Now ignore attempts to set acceleration to 0.0 125 | /// \version 1.26 Fixed a problem where certina combinations of speed and accelration could cause 126 | /// oscillation about the target position. 127 | /// \version 1.27 Added stop() function to stop as fast as possible with current acceleration parameters. 128 | /// Also added new Quickstop example showing its use. 129 | /// \version 1.28 Fixed another problem where certain combinations of speed and accelration could cause 130 | /// oscillation about the target position. 131 | /// Added support for 3 wire full and half steppers such as Hard Disk Drive spindle. 132 | /// Contributed by Yuri Ivatchkovitch. 133 | /// \version 1.29 Fixed a problem that could cause a DRIVER stepper to continually step 134 | /// with some sketches. Reported by Vadim. 135 | /// \version 1.30 Fixed a problem that could cause stepper to back up a few steps at the end of 136 | /// accelerated travel with certain speeds. Reported and patched by jolo. 137 | /// \version 1.31 Updated author and distribution location details to airspayce.com 138 | /// \version 1.32 Fixed a problem with enableOutputs() and setEnablePin on Arduino Due that 139 | /// prevented the enable pin changing stae correctly. Reported by Duane Bishop. 140 | /// \version 1.33 Fixed an error in example AFMotor_ConstantSpeed.pde did not setMaxSpeed(); 141 | /// Fixed a problem that caused incorrect pin sequencing of FULL3WIRE and HALF3WIRE. 142 | /// Unfortunately this meant changing the signature for all step*() functions. 143 | /// Added example MotorShield, showing how to use AdaFruit Motor Shield to control 144 | /// a 3 phase motor such as a HDD spindle motor (and without using the AFMotor library. 145 | /// \version 1.34 Added setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert) 146 | /// to allow inversion of 2, 3 and 4 wire stepper pins. Requested by Oleg. 147 | /// \version 1.35 Removed default args from setPinsInverted(bool, bool, bool, bool, bool) to prevent ambiguity with 148 | /// setPinsInverted(bool, bool, bool). Reported by Mac Mac. 149 | /// \version 1.36 Changed enableOutputs() and disableOutputs() to be virtual so can be overridden. 150 | /// Added new optional argument 'enable' to constructor, which allows you toi disable the 151 | /// automatic enabling of outputs at construction time. Suggested by Guido. 152 | /// \version 1.37 Fixed a problem with step1 that could cause a rogue step in the 153 | /// wrong direction (or not, 154 | /// depending on the setup-time requirements of the connected hardware). 155 | /// Reported by Mark Tillotson. 156 | /// \version 1.38 run() function incorrectly always returned true. Updated function and doc so it returns true 157 | /// if the motor is still running to the target position. 158 | /// 159 | /// \author Mike McCauley (mikem@airspayce.com) DO NOT CONTACT THE AUTHOR DIRECTLY: USE THE LISTS 160 | // Copyright (C) 2009-2013 Mike McCauley 161 | // $Id: AccelStepperEncoder.h,v 1.19 2013/08/02 01:53:21 mikem Exp mikem $ 162 | 163 | #ifndef AccelStepperEncoder_h 164 | #define AccelStepperEncoder_h 165 | 166 | #include 167 | #include "Encoder.h" 168 | 169 | #include 170 | #if ARDUINO >= 100 171 | #include 172 | #else 173 | #include 174 | #include 175 | #endif 176 | 177 | // These defs cause trouble on some versions of Arduino 178 | #undef round 179 | 180 | ///////////////////////////////////////////////////////////////////// 181 | /// \class AccelStepperEncoder AccelStepperEncoder.h 182 | /// \brief Support for stepper motors with acceleration etc. 183 | /// 184 | /// This defines a single 2 or 4 pin stepper motor, or stepper moter with fdriver chip, with optional 185 | /// acceleration, deceleration, absolute positioning commands etc. Multiple 186 | /// simultaneous steppers are supported, all moving 187 | /// at different speeds and accelerations. 188 | /// 189 | /// \par Operation 190 | /// This module operates by computing a step time in microseconds. The step 191 | /// time is recomputed after each step and after speed and acceleration 192 | /// parameters are changed by the caller. The time of each step is recorded in 193 | /// microseconds. The run() function steps the motor once if a new step is due. 194 | /// The run() function must be called frequently until the motor is in the 195 | /// desired position, after which time run() will do nothing. 196 | /// 197 | /// \par Positioning 198 | /// Positions are specified by a signed long integer. At 199 | /// construction time, the current position of the motor is consider to be 0. Positive 200 | /// positions are clockwise from the initial position; negative positions are 201 | /// anticlockwise. The curent position can be altered for instance after 202 | /// initialization positioning. 203 | /// 204 | /// \par Caveats 205 | /// This is an open loop controller: If the motor stalls or is oversped, 206 | /// AccelStepperEncoder will not have a correct 207 | /// idea of where the motor really is (since there is no feedback of the motor's 208 | /// real position. We only know where we _think_ it is, relative to the 209 | /// initial starting point). 210 | /// 211 | /// \par Performance 212 | /// The fastest motor speed that can be reliably supported is about 4000 steps per 213 | /// second at a clock frequency of 16 MHz on Arduino such as Uno etc. 214 | /// Faster processors can support faster stepping speeds. 215 | /// However, any speed less than that 216 | /// down to very slow speeds (much less than one per second) are also supported, 217 | /// provided the run() function is called frequently enough to step the motor 218 | /// whenever required for the speed set. 219 | /// Calling setAcceleration() is expensive, 220 | /// since it requires a square root to be calculated. 221 | class AccelStepperEncoder 222 | { 223 | public: 224 | /// \brief Symbolic names for number of pins. 225 | /// Use this in the pins argument the AccelStepperEncoder constructor to 226 | /// provide a symbolic name for the number of pins 227 | /// to use. 228 | typedef enum 229 | { 230 | FUNCTION = 0, ///< Use the functional interface, implementing your own driver functions (internal use only) 231 | DRIVER = 1, ///< Stepper Driver, 2 driver pins required 232 | FULL2WIRE = 2, ///< 2 wire stepper, 2 motor pins required 233 | FULL3WIRE = 3, ///< 3 wire stepper, such as HDD spindle, 3 motor pins required 234 | FULL4WIRE = 4, ///< 4 wire full stepper, 4 motor pins required 235 | HALF3WIRE = 6, ///< 3 wire half stepper, such as HDD spindle, 3 motor pins required 236 | HALF4WIRE = 8 ///< 4 wire half stepper, 4 motor pins required 237 | } MotorInterfaceType; 238 | 239 | /// Constructor. You can have multiple simultaneous steppers, all moving 240 | /// at different speeds and accelerations, provided you call their run() 241 | /// functions at frequent enough intervals. Current Position is set to 0, target 242 | /// position is set to 0. MaxSpeed and Acceleration default to 1.0. 243 | /// The motor pins will be initialised to OUTPUT mode during the 244 | /// constructor by a call to enableOutputs(). 245 | /// \param[in] interface Number of pins to interface to. 1, 2, 4 or 8 are 246 | /// supported, but it is preferred to use the \ref MotorInterfaceType symbolic names. 247 | /// AccelStepperEncoder::DRIVER (1) means a stepper driver (with Step and Direction pins). 248 | /// If an enable line is also needed, call setEnablePin() after construction. 249 | /// You may also invert the pins using setPinsInverted(). 250 | /// AccelStepperEncoder::FULL2WIRE (2) means a 2 wire stepper (2 pins required). 251 | /// AccelStepperEncoder::FULL3WIRE (3) means a 3 wire stepper, such as HDD spindle (3 pins required). 252 | /// AccelStepperEncoder::FULL4WIRE (4) means a 4 wire stepper (4 pins required). 253 | /// AccelStepperEncoder::HALF3WIRE (6) means a 3 wire half stepper, such as HDD spindle (3 pins required) 254 | /// AccelStepperEncoder::HALF4WIRE (8) means a 4 wire half stepper (4 pins required) 255 | /// Defaults to AccelStepperEncoder::FULL4WIRE (4) pins. 256 | /// \param[in] pin1 Arduino digital pin number for motor pin 1. Defaults 257 | /// to pin 2. For a AccelStepperEncoder::DRIVER (pins==1), 258 | /// this is the Step input to the driver. Low to high transition means to step) 259 | /// \param[in] pin2 Arduino digital pin number for motor pin 2. Defaults 260 | /// to pin 3. For a AccelStepperEncoder::DRIVER (pins==1), 261 | /// this is the Direction input the driver. High means forward. 262 | /// \param[in] pin3 Arduino digital pin number for motor pin 3. Defaults 263 | /// to pin 4. 264 | /// \param[in] pin4 Arduino digital pin number for motor pin 4. Defaults 265 | /// to pin 5. 266 | /// \param[in] enable If this is true (the default), enableOutpuys() will be called to enable 267 | /// the output pins at construction time. 268 | AccelStepperEncoder(uint8_t interface = AccelStepperEncoder::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t pin3 = 4, uint8_t pin4 = 5, bool enable = true); 269 | 270 | /// Alternate Constructor which will call your own functions for forward and backward steps. 271 | /// You can have multiple simultaneous steppers, all moving 272 | /// at different speeds and accelerations, provided you call their run() 273 | /// functions at frequent enough intervals. Current Position is set to 0, target 274 | /// position is set to 0. MaxSpeed and Acceleration default to 1.0. 275 | /// Any motor initialization should happen before hand, no pins are used or initialized. 276 | /// \param[in] forward void-returning procedure that will make a forward step 277 | /// \param[in] backward void-returning procedure that will make a backward step 278 | AccelStepperEncoder(void (*forward)(), void (*backward)()); 279 | 280 | /// Set the target position. The run() function will try to move the motor (at most one step per call) 281 | /// from the current position to the target position set by the most 282 | /// recent call to this function. Caution: moveTo() also recalculates the speed for the next step. 283 | /// If you are trying to use constant speed movements, you should call setSpeed() after calling moveTo(). 284 | /// \param[in] absolute The desired absolute position. Negative is 285 | /// anticlockwise from the 0 position. 286 | virtual void moveTo(long absolute); 287 | 288 | /// Set the target position relative to the current position 289 | /// \param[in] relative The desired position relative to the current position. Negative is 290 | /// anticlockwise from the current position. 291 | void move(long relative); 292 | 293 | /// Poll the motor and step it if a step is due, implementing 294 | /// accelerations and decelerations to acheive the target position. You must call this as 295 | /// frequently as possible, but at least once per minimum step time interval, 296 | /// preferably in your main loop. Note that each call to run() will make at most one step, and then only when a step is due, 297 | /// based on the current speed and the time since the last step. 298 | /// \return true if the motor is still running to the target position. 299 | boolean run(); 300 | 301 | /// Poll the motor and step it if a step is due, implementing a constant 302 | /// speed as set by the most recent call to setSpeed(). You must call this as 303 | /// frequently as possible, but at least once per step interval, 304 | /// \return true if the motor was stepped. 305 | virtual boolean runSpeed(); 306 | 307 | /// Sets the maximum permitted speed. The run() function will accelerate 308 | /// up to the speed set by this function. 309 | /// \param[in] speed The desired maximum speed in steps per second. Must 310 | /// be > 0. Caution: Speeds that exceed the maximum speed supported by the processor may 311 | /// Result in non-linear accelerations and decelerations. 312 | void setMaxSpeed(float speed); 313 | 314 | /// Sets the acceleration/deceleration rate. 315 | /// \param[in] acceleration The desired acceleration in steps per second 316 | /// per second. Must be > 0.0. This is an expensive call since it requires a square 317 | /// root to be calculated. Dont call more ofthen than needed 318 | void setAcceleration(float acceleration); 319 | 320 | /// Sets the desired constant speed for use with runSpeed(). 321 | /// \param[in] speed The desired constant speed in steps per 322 | /// second. Positive is clockwise. Speeds of more than 1000 steps per 323 | /// second are unreliable. Very slow speeds may be set (eg 0.00027777 for 324 | /// once per hour, approximately. Speed accuracy depends on the Arduino 325 | /// crystal. Jitter depends on how frequently you call the runSpeed() function. 326 | void setSpeed(float speed); 327 | 328 | /// The most recently set speed 329 | /// \return the most recent speed in steps per second 330 | float speed(); 331 | 332 | /// The distance from the current position to the target position. 333 | /// \return the distance from the current position to the target position 334 | /// in steps. Positive is clockwise from the current position. 335 | long distanceToGo(); 336 | 337 | /// The most recently set target position. 338 | /// \return the target position 339 | /// in steps. Positive is clockwise from the 0 position. 340 | long targetPosition(); 341 | 342 | /// The currently motor position. 343 | /// \return the current motor position 344 | /// in steps. Positive is clockwise from the 0 position. 345 | long currentPosition(); 346 | 347 | /// Resets the current position of the motor, so that wherever the motor 348 | /// happens to be right now is considered to be the new 0 position. Useful 349 | /// for setting a zero position on a stepper after an initial hardware 350 | /// positioning move. 351 | /// Has the side effect of setting the current motor speed to 0. 352 | /// \param[in] position The position in steps of wherever the motor 353 | /// happens to be right now. 354 | void setCurrentPosition(long position); 355 | 356 | /// Moves the motor at the currently selected constant speed (forward or reverse) 357 | /// to the target position and blocks until it is at 358 | /// position. Dont use this in event loops, since it blocks. 359 | void runToPosition(); 360 | 361 | /// Runs at the currently selected speed until the target position is reached 362 | /// Does not implement accelerations. 363 | /// \return true if it stepped 364 | virtual boolean runSpeedToPosition(); 365 | 366 | /// Moves the motor to the new target position and blocks until it is at 367 | /// position. Dont use this in event loops, since it blocks. 368 | /// \param[in] position The new target position. 369 | void runToNewPosition(long position); 370 | 371 | /// Sets a new target position that causes the stepper 372 | /// to stop as quickly as possible, using to the current speed and acceleration parameters. 373 | void stop(); 374 | 375 | /// Disable motor pin outputs by setting them all LOW 376 | /// Depending on the design of your electronics this may turn off 377 | /// the power to the motor coils, saving power. 378 | /// This is useful to support Arduino low power modes: disable the outputs 379 | /// during sleep and then reenable with enableOutputs() before stepping 380 | /// again. 381 | virtual void disableOutputs(); 382 | 383 | /// Enable motor pin outputs by setting the motor pins to OUTPUT 384 | /// mode. Called automatically by the constructor. 385 | virtual void enableOutputs(); 386 | 387 | /// Sets the minimum pulse width allowed by the stepper driver. The minimum practical pulse width is 388 | /// approximately 20 microseconds. Times less than 20 microseconds 389 | /// will usually result in 20 microseconds or so. 390 | /// \param[in] minWidth The minimum pulse width in microseconds. 391 | void setMinPulseWidth(unsigned int minWidth); 392 | 393 | /// Sets the enable pin number for stepper drivers. 394 | /// 0xFF indicates unused (default). 395 | /// Otherwise, if a pin is set, the pin will be turned on when 396 | /// enableOutputs() is called and switched off when disableOutputs() 397 | /// is called. 398 | /// \param[in] enablePin Arduino digital pin number for motor enable 399 | /// \sa setPinsInverted 400 | void setEnablePin(uint8_t enablePin = 0xff); 401 | 402 | /// Sets the inversion for stepper driver pins 403 | /// \param[in] directionInvert True for inverted direction pin, false for non-inverted 404 | /// \param[in] stepInvert True for inverted step pin, false for non-inverted 405 | /// \param[in] enableInvert True for inverted enable pin, false (default) for non-inverted 406 | void setPinsInverted(bool directionInvert = false, bool stepInvert = false, bool enableInvert = false); 407 | 408 | /// Sets the inversion for 2, 3 and 4 wire stepper pins 409 | /// \param[in] pin1Invert True for inverted pin1, false for non-inverted 410 | /// \param[in] pin2Invert True for inverted pin2, false for non-inverted 411 | /// \param[in] pin3Invert True for inverted pin3, false for non-inverted 412 | /// \param[in] pin4Invert True for inverted pin4, false for non-inverted 413 | /// \param[in] enableInvert True for inverted enable pin, false (default) for non-inverted 414 | void setPinsInverted(bool pin1Invert, bool pin2Invert, bool pin3Invert, bool pin4Invert, bool enableInvert); 415 | 416 | void addEncoder(Encoder *enc, float ratio); 417 | Encoder* getEncoder(); 418 | long readEnc(); 419 | void writeEnc(long value); 420 | float encoderPositionForMotor(long motorPos); 421 | float computeDeviation(); 422 | float maxDeviation = 0.0f; 423 | float minDeviation = 0.0f; 424 | float acceptableDeviation = 10.0; 425 | 426 | float correctDeviation(); 427 | void synchroniseMotorWithEncoder(); 428 | 429 | protected: 430 | 431 | /// \brief Direction indicator 432 | /// Symbolic names for the direction the motor is turning 433 | typedef enum 434 | { 435 | DIRECTION_CCW = 0, ///< Clockwise 436 | DIRECTION_CW = 1 ///< Counter-Clockwise 437 | } Direction; 438 | 439 | /// Forces the library to compute a new instantaneous speed and set that as 440 | /// the current speed. It is called by 441 | /// the library: 442 | /// \li after each step 443 | /// \li after change to maxSpeed through setMaxSpeed() 444 | /// \li after change to acceleration through setAcceleration() 445 | /// \li after change to target position (relative or absolute) through 446 | /// move() or moveTo() 447 | void computeNewSpeed(); 448 | 449 | /// Low level function to set the motor output pins 450 | /// bit 0 of the mask corresponds to _pin[0] 451 | /// bit 1 of the mask corresponds to _pin[1] 452 | /// You can override this to impment, for example serial chip output insted of using the 453 | /// output pins directly 454 | virtual void setOutputPins(uint8_t mask); 455 | 456 | /// Called to execute a step. Only called when a new step is 457 | /// required. Subclasses may override to implement new stepping 458 | /// interfaces. The default calls step1(), step2(), step4() or step8() depending on the 459 | /// number of pins defined for the stepper. 460 | /// \param[in] step The current step phase number (0 to 7) 461 | virtual void step(long step); 462 | 463 | /// Called to execute a step using stepper functions (pins = 0) Only called when a new step is 464 | /// required. Calls _forward() or _backward() to perform the step 465 | /// \param[in] step The current step phase number (0 to 7) 466 | virtual void step0(long step); 467 | 468 | /// Called to execute a step on a stepper driver (ie where pins == 1). Only called when a new step is 469 | /// required. Subclasses may override to implement new stepping 470 | /// interfaces. The default sets or clears the outputs of Step pin1 to step, 471 | /// and sets the output of _pin2 to the desired direction. The Step pin (_pin1) is pulsed for 1 microsecond 472 | /// which is the minimum STEP pulse width for the 3967 driver. 473 | /// \param[in] step The current step phase number (0 to 7) 474 | virtual void step1(long step); 475 | 476 | /// Called to execute a step on a 2 pin motor. Only called when a new step is 477 | /// required. Subclasses may override to implement new stepping 478 | /// interfaces. The default sets or clears the outputs of pin1 and pin2 479 | /// \param[in] step The current step phase number (0 to 7) 480 | virtual void step2(long step); 481 | 482 | /// Called to execute a step on a 3 pin motor, such as HDD spindle. Only called when a new step is 483 | /// required. Subclasses may override to implement new stepping 484 | /// interfaces. The default sets or clears the outputs of pin1, pin2, 485 | /// pin3 486 | /// \param[in] step The current step phase number (0 to 7) 487 | virtual void step3(long step); 488 | 489 | /// Called to execute a step on a 4 pin motor. Only called when a new step is 490 | /// required. Subclasses may override to implement new stepping 491 | /// interfaces. The default sets or clears the outputs of pin1, pin2, 492 | /// pin3, pin4. 493 | /// \param[in] step The current step phase number (0 to 7) 494 | virtual void step4(long step); 495 | 496 | /// Called to execute a step on a 3 pin motor, such as HDD spindle. Only called when a new step is 497 | /// required. Subclasses may override to implement new stepping 498 | /// interfaces. The default sets or clears the outputs of pin1, pin2, 499 | /// pin3 500 | /// \param[in] step The current step phase number (0 to 7) 501 | virtual void step6(long step); 502 | 503 | /// Called to execute a step on a 4 pin half-steper motor. Only called when a new step is 504 | /// required. Subclasses may override to implement new stepping 505 | /// interfaces. The default sets or clears the outputs of pin1, pin2, 506 | /// pin3, pin4. 507 | /// \param[in] step The current step phase number (0 to 7) 508 | virtual void step8(long step); 509 | 510 | //private: 511 | /// Number of pins on the stepper motor. Permits 2 or 4. 2 pins is a 512 | /// bipolar, and 4 pins is a unipolar. 513 | uint8_t _interface; // 0, 1, 2, 4, 8, See MotorInterfaceType 514 | 515 | /// Arduino pin number assignments for the 2 or 4 pins required to interface to the 516 | /// stepper motor or driver 517 | uint8_t _pin[4]; 518 | 519 | /// Whether the _pins is inverted or not 520 | uint8_t _pinInverted[4]; 521 | 522 | /// The current absolution position in steps. 523 | long _currentPos; // Steps 524 | 525 | /// The target position in steps. The AccelStepperEncoder library will move the 526 | /// motor from the _currentPos to the _targetPos, taking into account the 527 | /// max speed, acceleration and deceleration 528 | long _targetPos; // Steps 529 | 530 | /// The current motos speed in steps per second 531 | /// Positive is clockwise 532 | float _speed; // Steps per second 533 | 534 | /// The maximum permitted speed in steps per second. Must be > 0. 535 | float _maxSpeed; 536 | 537 | /// The acceleration to use to accelerate or decelerate the motor in steps 538 | /// per second per second. Must be > 0 539 | float _acceleration; 540 | float _sqrt_twoa; // Precomputed sqrt(2*_acceleration) 541 | 542 | /// The current interval between steps in microseconds. 543 | /// 0 means the motor is currently stopped with _speed == 0 544 | unsigned long _stepInterval; 545 | 546 | /// The last step time in microseconds 547 | unsigned long _lastStepTime; 548 | 549 | /// The minimum allowed pulse width in microseconds 550 | unsigned int _minPulseWidth; 551 | 552 | /// Is the direction pin inverted? 553 | ///bool _dirInverted; /// Moved to _pinInverted[1] 554 | 555 | /// Is the step pin inverted? 556 | ///bool _stepInverted; /// Moved to _pinInverted[0] 557 | 558 | /// Is the enable pin inverted? 559 | bool _enableInverted; 560 | 561 | /// Enable pin for stepper driver, or 0xFF if unused. 562 | uint8_t _enablePin; 563 | 564 | /// The pointer to a forward-step procedure 565 | void (*_forward)(); 566 | 567 | /// The pointer to a backward-step procedure 568 | void (*_backward)(); 569 | 570 | /// The step counter for speed calculations 571 | long _n; 572 | 573 | /// Initial step size in microseconds 574 | float _c0; 575 | 576 | /// Last step size in microseconds 577 | float _cn; 578 | 579 | /// Min step size in microseconds based on maxSpeed 580 | float _cmin; // at max speed 581 | 582 | /// Current direction motor is spinning in 583 | boolean _direction; // 1 == CW 584 | 585 | /// instance of encoder 586 | Encoder *_enc; 587 | 588 | /// we still keep track of the motor position so that we can work out which 589 | /// pattern to step with next. 590 | long _currentMotorPos; 591 | 592 | float _encoderTargetPos; 593 | float _motorToEncoderRatio; 594 | 595 | 596 | }; 597 | 598 | /// @example Random.pde 599 | /// Make a single stepper perform random changes in speed, position and acceleration 600 | 601 | /// @example Overshoot.pde 602 | /// Check overshoot handling 603 | /// which sets a new target position and then waits until the stepper has 604 | /// achieved it. This is used for testing the handling of overshoots 605 | 606 | /// @example MultiStepper.pde 607 | /// Shows how to multiple simultaneous steppers 608 | /// Runs one stepper forwards and backwards, accelerating and decelerating 609 | /// at the limits. Runs other steppers at the same time 610 | 611 | /// @example ConstantSpeed.pde 612 | /// Shows how to run AccelStepperEncoder in the simplest, 613 | /// fixed speed mode with no accelerations 614 | 615 | /// @example Blocking.pde 616 | /// Shows how to use the blocking call runToNewPosition 617 | /// Which sets a new target position and then waits until the stepper has 618 | /// achieved it. 619 | 620 | /// @example AFMotor_MultiStepper.pde 621 | /// Control both Stepper motors at the same time with different speeds 622 | /// and accelerations. 623 | 624 | /// @example AFMotor_ConstantSpeed.pde 625 | /// Shows how to run AccelStepperEncoder in the simplest, 626 | /// fixed speed mode with no accelerations 627 | 628 | /// @example ProportionalControl.pde 629 | /// Make a single stepper follow the analog value read from a pot or whatever 630 | /// The stepper will move at a constant speed to each newly set posiiton, 631 | /// depending on the value of the pot. 632 | 633 | /// @example Bounce.pde 634 | /// Make a single stepper bounce from one limit to another, observing 635 | /// accelrations at each end of travel 636 | 637 | /// @example Quickstop.pde 638 | /// Check stop handling. 639 | /// Calls stop() while the stepper is travelling at full speed, causing 640 | /// the stepper to stop as quickly as possible, within the constraints of the 641 | /// current acceleration. 642 | 643 | /// @example MotorShield.pde 644 | /// Shows how to use AccelStepperEncoder to control a 3-phase motor, such as a HDD spindle motor 645 | /// using the Adafruit Motor Shield http://www.ladyada.net/make/mshield/index.html. 646 | 647 | #endif 648 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | This software is Copyright (C) 2008 Mike McCauley. Use is subject to license 2 | conditions. The main licensing options available are GPL V2 or Commercial: 3 | 4 | Open Source Licensing GPL V2 5 | 6 | This is the appropriate option if you want to share the source code of your 7 | application with everyone you distribute it to, and you also want to give them 8 | the right to share who uses it. If you wish to use this software under Open 9 | Source Licensing, you must contribute all your source code to the open source 10 | community in accordance with the GPL Version 2 when your application is 11 | distributed. See http://www.gnu.org/copyleft/gpl.html 12 | 13 | Commercial Licensing 14 | 15 | This is the appropriate option if you are creating proprietary applications 16 | and you are not prepared to distribute and share the source code of your 17 | application. Contact info@open.com.au for details. 18 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | AccelStepperEncoder 2 | =================== 3 | 4 | A subclass of Mike McCauley's excellent AccelStepper library (http://www.airspayce.com/mikem/arduino/AccelStepper) that uses an encoder to record and correct motion. 5 | 6 | This is intended to be used where an encoder (such as http://www.coolcomponents.co.uk/rotary-encoder-200-p-r-quadrature.html) is coupled to the output of a stepper motor. In my case, as part of my Polargraph drawing machine project, I have a stepper motor that is pulling a length of beaded cord using a sprocket. This cord also passes over an idler sprocket attached to the encoder. 7 | 8 | From time to time (such as if a toddler begins to devour the cords), a motor step does not result in the cord moving, either because the motor is held still (dropping steps), or when the motor still manages to move, but the cord jumps through the teeth of the sprocket. 9 | 10 | This subclass of AccelStepper rolls the encoder object into it, and will allow AccelStepper to spot failed steps and retry. At first, it will be as simple as that, because that's about as clever as I am, but I think a smarter guy might be able to do some PIDdy stuff to make it work a bit less naively. 11 | 12 | The use case for this is: 13 | ========================= 14 | Polargraph machine is half way through a 20 minute drawing, when a baby with preturnaturally long arms reaches out from its papoose, and grasps the shiny wriggling pen. Normally this means game over, and the rest of the drawing is painfully offset and distorted. With AccelStepperEncoder the machine spots that an instruction to move 1 step has resulted in a 10,000 step change in actual position, and concludes that there has been a "drawing exception", lifts the pen off the paper and works to return it to it's proper place, picking up where it left off. 15 | 16 | Second use case: 17 | ================ 18 | I've just set off a portrait, but forgotten to put a new sheet of paper up, or it's fallen off as soon as I started, or something like that. Normally, pausing the machine locks the motors so that the position is not lost. This causes the motors to heat up, and means the pen is fixed right in the way of the paper. With AccelStepperEncoder, a pause will halt the command queue AND turn off the motors, so the pen can be moved safely out of the way. 19 | 20 | Third use case: 21 | =============== 22 | The machine is set up to draw a small feature once an hour (a la Matt Venn's Cursive Data energy monitor https://github.com/mattvenn/cursivedata). Currently, to stay calibrated, the motors must never be turned off, and that leads to all kinds of problems (heat mostly), as well as being a waste of lecky. Matt's solution to this is to turn the power of the motors down so that they are still just locked enough to stay in place, but there seem to be no off-the-shelf motor drivers that facilitate this kind of programmatic power control. This encoder approach is a completely over-engineered solution (as well as being MUCH more expensive). But, with AccelStepperEncoder, the motors can simply be turned off after each feature, and re-enabled before the next one, and it will pick up where it left off. 23 | 24 | At least, that's the theory. Have to actually write it now. 25 | 26 | I would be surprised if this problem is not already extremely well-solved in industrial contexts, with a proper hardware feedback loop. 27 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For AccelStepper 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | AccelStepperEncoder KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | moveTo KEYWORD2 16 | move KEYWORD2 17 | run KEYWORD2 18 | runSpeed KEYWORD2 19 | setMaxSpeed KEYWORD2 20 | setAcceleration KEYWORD2 21 | setSpeed KEYWORD2 22 | speed KEYWORD2 23 | distanceToGo KEYWORD2 24 | targetPosition KEYWORD2 25 | currentPosition KEYWORD2 26 | steCurrentPosition KEYWORD2 27 | runToPosition KEYWORD2 28 | runSpeedToPosition KEYWORD2 29 | runToNewPosition KEYWORD2 30 | disableOutputs KEYWORD2 31 | enableOutputs KEYWORD2 32 | setMinPulseWidth KEYWORD2 33 | 34 | ####################################### 35 | # Constants (LITERAL1) 36 | ####################################### 37 | 38 | --------------------------------------------------------------------------------