From 0948754e79c9507fe43732f81b0a8efe522e9f1a Mon Sep 17 00:00:00 2001 From: Elizabeth Moore Date: Tue, 21 Aug 2018 10:26:10 -0700 Subject: [PATCH] Initial Commit First release of the library --- LICENSE.md | 116 ++++++++++++ PrecisionServo/PrecisionServo.h | 238 +++++++++++++++++++++++++ PrecisionServo/ServoManager.cpp | 152 ++++++++++++++++ PrecisionServo/ServoManager.h | 30 ++++ PrecisionServo/ServoManagerold.cpp.txt | 162 +++++++++++++++++ README.md | 71 ++++++++ examples/single_servo_sweep.ino | 24 +++ examples/two_servo_settings_sweep.ino | 56 ++++++ 8 files changed, 849 insertions(+) create mode 100644 LICENSE.md create mode 100644 PrecisionServo/PrecisionServo.h create mode 100644 PrecisionServo/ServoManager.cpp create mode 100644 PrecisionServo/ServoManager.h create mode 100644 PrecisionServo/ServoManagerold.cpp.txt create mode 100644 examples/single_servo_sweep.ino create mode 100644 examples/two_servo_settings_sweep.ino diff --git a/LICENSE.md b/LICENSE.md new file mode 100644 index 0000000..3bbbc1e --- /dev/null +++ b/LICENSE.md @@ -0,0 +1,116 @@ +CC0 1.0 Universal + +Statement of Purpose + +The laws of most jurisdictions throughout the world automatically confer +exclusive Copyright and Related Rights (defined below) upon the creator and +subsequent owner(s) (each and all, an "owner") of an original work of +authorship and/or a database (each, a "Work"). + +Certain owners wish to permanently relinquish those rights to a Work for the +purpose of contributing to a commons of creative, cultural and scientific +works ("Commons") that the public can reliably and without fear of later +claims of infringement build upon, modify, incorporate in other works, reuse +and redistribute as freely as possible in any form whatsoever and for any +purposes, including without limitation commercial purposes. These owners may +contribute to the Commons to promote the ideal of a free culture and the +further production of creative, cultural and scientific works, or to gain +reputation or greater distribution for their Work in part through the use and +efforts of others. + +For these and/or other purposes and motivations, and without any expectation +of additional consideration or compensation, the person associating CC0 with a +Work (the "Affirmer"), to the extent that he or she is an owner of Copyright +and Related Rights in the Work, voluntarily elects to apply CC0 to the Work +and publicly distribute the Work under its terms, with knowledge of his or her +Copyright and Related Rights in the Work and the meaning and intended legal +effect of CC0 on those rights. + +1. Copyright and Related Rights. A Work made available under CC0 may be +protected by copyright and related or neighboring rights ("Copyright and +Related Rights"). Copyright and Related Rights include, but are not limited +to, the following: + + i. the right to reproduce, adapt, distribute, perform, display, communicate, + and translate a Work; + + ii. moral rights retained by the original author(s) and/or performer(s); + + iii. publicity and privacy rights pertaining to a person's image or likeness + depicted in a Work; + + iv. rights protecting against unfair competition in regards to a Work, + subject to the limitations in paragraph 4(a), below; + + v. rights protecting the extraction, dissemination, use and reuse of data in + a Work; + + vi. database rights (such as those arising under Directive 96/9/EC of the + European Parliament and of the Council of 11 March 1996 on the legal + protection of databases, and under any national implementation thereof, + including any amended or successor version of such directive); and + + vii. other similar, equivalent or corresponding rights throughout the world + based on applicable law or treaty, and any national implementations thereof. + +2. Waiver. To the greatest extent permitted by, but not in contravention of, +applicable law, Affirmer hereby overtly, fully, permanently, irrevocably and +unconditionally waives, abandons, and surrenders all of Affirmer's Copyright +and Related Rights and associated claims and causes of action, whether now +known or unknown (including existing as well as future claims and causes of +action), in the Work (i) in all territories worldwide, (ii) for the maximum +duration provided by applicable law or treaty (including future time +extensions), (iii) in any current or future medium and for any number of +copies, and (iv) for any purpose whatsoever, including without limitation +commercial, advertising or promotional purposes (the "Waiver"). Affirmer makes +the Waiver for the benefit of each member of the public at large and to the +detriment of Affirmer's heirs and successors, fully intending that such Waiver +shall not be subject to revocation, rescission, cancellation, termination, or +any other legal or equitable action to disrupt the quiet enjoyment of the Work +by the public as contemplated by Affirmer's express Statement of Purpose. + +3. Public License Fallback. Should any part of the Waiver for any reason be +judged legally invalid or ineffective under applicable law, then the Waiver +shall be preserved to the maximum extent permitted taking into account +Affirmer's express Statement of Purpose. In addition, to the extent the Waiver +is so judged Affirmer hereby grants to each affected person a royalty-free, +non transferable, non sublicensable, non exclusive, irrevocable and +unconditional license to exercise Affirmer's Copyright and Related Rights in +the Work (i) in all territories worldwide, (ii) for the maximum duration +provided by applicable law or treaty (including future time extensions), (iii) +in any current or future medium and for any number of copies, and (iv) for any +purpose whatsoever, including without limitation commercial, advertising or +promotional purposes (the "License"). The License shall be deemed effective as +of the date CC0 was applied by Affirmer to the Work. Should any part of the +License for any reason be judged legally invalid or ineffective under +applicable law, such partial invalidity or ineffectiveness shall not +invalidate the remainder of the License, and in such case Affirmer hereby +affirms that he or she will not (i) exercise any of his or her remaining +Copyright and Related Rights in the Work or (ii) assert any associated claims +and causes of action with respect to the Work, in either case contrary to +Affirmer's express Statement of Purpose. + +4. Limitations and Disclaimers. + + a. No trademark or patent rights held by Affirmer are waived, abandoned, + surrendered, licensed or otherwise affected by this document. + + b. Affirmer offers the Work as-is and makes no representations or warranties + of any kind concerning the Work, express, implied, statutory or otherwise, + including without limitation warranties of title, merchantability, fitness + for a particular purpose, non infringement, or the absence of latent or + other defects, accuracy, or the present or absence of errors, whether or not + discoverable, all to the greatest extent permissible under applicable law. + + c. Affirmer disclaims responsibility for clearing rights of other persons + that may apply to the Work or any use thereof, including without limitation + any person's Copyright and Related Rights in the Work. Further, Affirmer + disclaims responsibility for obtaining any necessary consents, permissions + or other rights required for any use of the Work. + + d. Affirmer understands and acknowledges that Creative Commons is not a + party to this document and has no duty or obligation with respect to this + CC0 or use of the Work. + +For more information, please see + \ No newline at end of file diff --git a/PrecisionServo/PrecisionServo.h b/PrecisionServo/PrecisionServo.h new file mode 100644 index 0000000..b280215 --- /dev/null +++ b/PrecisionServo/PrecisionServo.h @@ -0,0 +1,238 @@ +#ifndef PrecisionServo_h +#define PrecisionServo_h + +#define SERVO_SLOW_SMOOTH 0 +#define SERVO_SLOW_PRECISE 1 +#define SERVO_SLOW_ENDPOINT 2 + +#define SERVO_MEDIUM_SMOOTH 3 +#define SERVO_MEDIUM_PRECISE 4 +#define SERVO_MEDIUM_ENDPOINT 5 + +#define SERVO_FAST_SMOOTH 6 +#define SERVO_FAST_PRECISE 7 +#define SERVO_FAST_ENDPOINT 8 + +// The maximum number of degrees that we ask the servo to move at once +// higher values -> faster, less accurate movement +#define SLOW_SMOOTH_MAX_MOVEMENT 3 +#define SLOW_PRECISE_MAX_MOVEMENT 10 +#define SLOW_ENDPOINT_MAX_MOVEMENT 10 + +#define MEDIUM_SMOOTH_MAX_MOVEMENT 5 +#define MEDIUM_PRECISE_MAX_MOVEMENT 12 +#define MEDIUM_ENDPOINT_MAX_MOVEMENT 12 + +#define FAST_SMOOTH_MAX_MOVEMENT 8 +#define FAST_PRECISE_MAX_MOVEMENT 15 +#define FAST_ENDPOINT_MAX_MOVEMENT 15 + +// The number of times we send our orders to the servo when we change its +// angle by max movement. Each iteration is roughly 20ms +#define SLOW_SMOOTH_MOVEMENT_ITERATIONS 3 +#define SLOW_PRECISE_MOVEMENT_ITERATIONS 2 +#define SLOW_ENDPOINT_MOVEMENT_ITERATIONS 2 + +#define MEDIUM_SMOOTH_MOVEMENT_ITERATIONS 2 +#define MEDIUM_PRECISE_MOVEMENT_ITERATIONS 1 +#define MEDIUM_ENDPOINT_MOVEMENT_ITERATIONS 1 + +#define FAST_SMOOTH_MOVEMENT_ITERATIONS 1 +#define FAST_PRECISE_MOVEMENT_ITERATIONS 1 +#define FAST_ENDPOINT_MOVEMENT_ITERATIONS 1 + +// The extra iterations we do when changing directions (each is roughly 20ms) +// These are at our old position; by sending extra it prepares the servo +// If the new direction additional iterations and max movement is high while +// movement iterations is low, you get full-speed movement to endpoints followed +// by a pause. +#define SLOW_SMOOTH_NEW_DIR_ADD_ITERS 0 +#define SLOW_PRECISE_NEW_DIR_ADD_ITERS 3 +#define SLOW_ENDPOINT_NEW_DIR_ADD_ITERS 25 + +#define MEDIUM_SMOOTH_NEW_DIR_ADD_ITERS 0 +#define MEDIUM_PRECISE_NEW_DIR_ADD_ITERS 2 +#define MEDIUM_ENDPOINT_NEW_DIR_ADD_ITERS 35 + +#define FAST_SMOOTH_NEW_DIR_ADD_ITERS 0 +#define FAST_PRECISE_NEW_DIR_ADD_ITERS 1 +#define FAST_ENDPOINT_NEW_DIR_ADD_ITERS 45 + +// The extra iterations we do when we've been given a new target, regardless +// of if it's in a new direction. +#define SLOW_SMOOTH_NEW_TAR_ADD_ITERS 0 +#define SLOW_PRECISE_NEW_TAR_ADD_ITERS 25 +#define SLOW_ENDPOINT_NEW_TAR_ADD_ITERS 0 + +#define MEDIUM_SMOOTH_NEW_TAR_ADD_ITERS 0 +#define MEDIUM_PRECISE_NEW_TAR_ADD_ITERS 35 +#define MEDIUM_ENDPOINT_NEW_TAR_ADD_ITERS 0 + +#define FAST_SMOOTH_NEW_TAR_ADD_ITERS 0 +#define FAST_PRECISE_NEW_TAR_ADD_ITERS 45 +#define FAST_ENDPOINT_NEW_TAR_ADD_ITERS 0 + +#include +#include + +/** + * Contains information about a servo that can be sent + * to the servo manager + * + * @author Timothy Moore + * @author Elizabeth Moore + */ +struct PrecisionServo +{ + /** + * Which pin this servo is operating on. The pin + * can be assumed to have been set to output mode + * (done manually or via servo manager setup) + */ + uint8_t pin; + + /** + * The last angle that we told the servo to go to + * in degrees. Starts at 0 + */ + uint8_t angleDegrees; + + /** + * How many more times we should ask the servo to go to angleDegrees + * before moving on. + */ + int8_t iterationsLeftOnCurrentIncrement; + + /** + * Which angle we are trying to get to on this servo. + */ + uint8_t targetAngleDegrees; + + /** + * If the target has recently changed. + */ + bool targetChanged; + + /** + * The sign (-1, 0, +1) for the last direction we tried to move + * the servo in. The servo often loses accuracy when changing + * direction, so if we try to change direction we add extra + * delay. + */ + int8_t lastDirection; + + /** + * Set the target for the servo to the given degrees. + * @param angle the direction to go to, in degrees + */ + void setTarget(uint8_t angle) { + if(angle != targetAngleDegrees) { + targetAngleDegrees = angle; + targetChanged = true; + } + } + + /** Configure this servo to the given settings. + * + * @details This switch might be taking a lot of memory space. It could + * perhaps be avoided using a compiler macro instead of a setSetting + * function? + * + * @param setting The setting that this servo is using. + * + + * Valid Options: + * SERVO_SLOW_SMOOTH + * SERVO_SLOW_PRECISE + * SERVO_SLOW_ENDPOINT + * SERVO_MEDIUM_SMOOTH + * SERVO_MEDIUM_PRECISE + * SERVO_MEDIUM_ENDPOINT + * SERVO_FAST_SMOOTH + * SERVO_FAST_PRECISE + * SERVO_FAST_ENDPOINT + * + * SLOW/MEDIUM/FAST controls how rapidly the servo is going to the target + * + * SMOOTH - No additional pauses are used; the servo chases the target but + * often won't quite reach it. This is great for if you are trying + * to follow a sensor and the angles your asking for are inherently + * inprecise. + * + * PRECISE - The servo pauses at the *old* target whenever the target changes. + * This helps ensure it makes it exactly to the requested angles, + * assuming that you are using waitUntilFinished or sufficient delays + * between changing targets. This is ideal if you are hardcoding a + * sequence of movements and want the most consistent behavior possible. + * + * ENDPOINT - Acts very similiarly to PRECISE, except skips the pause when given + * orders that continue in the same direction (the servo is often + * less accurate when changing directions) + */ + void setSetting(uint8_t setting) { + switch(setting) { + case SERVO_SLOW_SMOOTH: + this->maxMovement = SLOW_SMOOTH_MAX_MOVEMENT; + this->movementIters = SLOW_SMOOTH_MOVEMENT_ITERATIONS; + this->newDirAddIters = SLOW_SMOOTH_NEW_DIR_ADD_ITERS; + this->newTarAddIters = SLOW_SMOOTH_NEW_TAR_ADD_ITERS; + break; + case SERVO_SLOW_PRECISE: + this->maxMovement = SLOW_PRECISE_MAX_MOVEMENT; + this->movementIters = SLOW_PRECISE_MOVEMENT_ITERATIONS; + this->newDirAddIters = SLOW_PRECISE_NEW_DIR_ADD_ITERS; + this->newTarAddIters = SLOW_PRECISE_NEW_TAR_ADD_ITERS; + break; + case SERVO_SLOW_ENDPOINT: + this->maxMovement = SLOW_ENDPOINT_MAX_MOVEMENT; + this->movementIters = SLOW_ENDPOINT_MOVEMENT_ITERATIONS; + this->newDirAddIters = SLOW_ENDPOINT_NEW_DIR_ADD_ITERS; + this->newTarAddIters = SLOW_ENDPOINT_NEW_TAR_ADD_ITERS; + break; + case SERVO_MEDIUM_SMOOTH: + this->maxMovement = MEDIUM_SMOOTH_MAX_MOVEMENT; + this->movementIters = MEDIUM_SMOOTH_MOVEMENT_ITERATIONS; + this->newDirAddIters = MEDIUM_SMOOTH_NEW_DIR_ADD_ITERS; + this->newTarAddIters = MEDIUM_SMOOTH_NEW_TAR_ADD_ITERS; + break; + case SERVO_MEDIUM_PRECISE: + this->maxMovement = MEDIUM_PRECISE_MAX_MOVEMENT; + this->movementIters = MEDIUM_PRECISE_MOVEMENT_ITERATIONS; + this->newDirAddIters = MEDIUM_PRECISE_NEW_DIR_ADD_ITERS; + this->newTarAddIters = MEDIUM_PRECISE_NEW_TAR_ADD_ITERS; + break; + case SERVO_MEDIUM_ENDPOINT: + this->maxMovement = MEDIUM_ENDPOINT_MAX_MOVEMENT; + this->movementIters = MEDIUM_ENDPOINT_MOVEMENT_ITERATIONS; + this->newDirAddIters = MEDIUM_ENDPOINT_NEW_DIR_ADD_ITERS; + this->newTarAddIters = MEDIUM_ENDPOINT_NEW_TAR_ADD_ITERS; + break; + case SERVO_FAST_SMOOTH: + this->maxMovement = FAST_SMOOTH_MAX_MOVEMENT; + this->movementIters = FAST_SMOOTH_MOVEMENT_ITERATIONS; + this->newDirAddIters = FAST_SMOOTH_NEW_DIR_ADD_ITERS; + this->newTarAddIters = FAST_SMOOTH_NEW_TAR_ADD_ITERS; + break; + case SERVO_FAST_PRECISE: + this->maxMovement = FAST_PRECISE_MAX_MOVEMENT; + this->movementIters = FAST_PRECISE_MOVEMENT_ITERATIONS; + this->newDirAddIters = FAST_PRECISE_NEW_DIR_ADD_ITERS; + this->newTarAddIters = FAST_PRECISE_NEW_TAR_ADD_ITERS; + break; + case SERVO_FAST_ENDPOINT: + this->maxMovement = FAST_ENDPOINT_MAX_MOVEMENT; + this->movementIters = FAST_ENDPOINT_MOVEMENT_ITERATIONS; + this->newDirAddIters = FAST_ENDPOINT_NEW_DIR_ADD_ITERS; + this->newTarAddIters = FAST_ENDPOINT_NEW_TAR_ADD_ITERS; + break; + } + } + + // servo settings; see defines at the top. Only change + // these directly if none of the presets are satisfactory. + uint8_t maxMovement; + uint8_t movementIters; + uint8_t newDirAddIters; + uint8_t newTarAddIters; +}; +#endif \ No newline at end of file diff --git a/PrecisionServo/ServoManager.cpp b/PrecisionServo/ServoManager.cpp new file mode 100644 index 0000000..67b3715 --- /dev/null +++ b/PrecisionServo/ServoManager.cpp @@ -0,0 +1,152 @@ +#include + +// The number of microseconds of ON per 20ms to get the servo to go to 180 +#define MICROSECONDS_FOR_180 2000 + +// The number of microseconds of ON per 20ms to get the servo to go to 0 +#define MICROSECONDS_FOR_0 400 + +int8_t sign(int16_t num) { + if(num < 0) { + return -1; + }else if(num == 0) { + return 0; + }else { + return 1; + } +} + +/** + * Stores how many microseconds before we should send information + * to the servos + */ +static uint32_t microsecondsUntilNextCycle = 0; +static bool currentlyUpdatingServos = false; +static PrecisionServo* servos = NULL; +static uint8_t numServos = 0; +static uint8_t totalServos = 0; + +void ServoManager::setup(uint8_t totServos) { + // TIMSK - Timer/Counter Interrupt Mask Register + // OCIE0A - Timer/Counter0 Output Compare Match A Interrupt Enable + // TOIE0 - Timer/Counter0 Overflow Interrupt Enable + TIMSK |= _BV(TOIE1); + + // TCCR1 - Timer/Counter1 Control Register + // Bits CS1[3:0] -> control prescaling + TCCR1 = _BV(CS12) | _BV(CS11) | _BV(CS10) | _BV(CTC1); // 1/64 prescaling with reset on match with OCR1C + + // 159 is the desired overflow time; this comes from the + // table on page 88 for a 50kHz clock. That's not what + // we have but somehow it works + OCR1C = 159; + + PLLCSR |= _BV(PCKE); // use the fast peripheral clock (64Mhz) + + if(totServos <= 0) { + // ideally we'd raise an error + totalServos = -1; + return; + } + + servos = (PrecisionServo*)malloc(sizeof(PrecisionServo) * totServos); + numServos = 0; + totalServos = totServos; + + // enable global interrupts + sei(); +} + +PrecisionServo* ServoManager::createServo(uint8_t pin) { + if(numServos >= totalServos) + return NULL; // ideally we'd raise an error + + PrecisionServo* newServo = (servos + sizeof(PrecisionServo) * numServos); + numServos++; + + pinMode(pin, OUTPUT); + newServo->pin = pin; + newServo->angleDegrees = 0; + newServo->targetAngleDegrees = 0; + newServo->iterationsLeftOnCurrentIncrement = 10; + newServo->lastDirection = 0; + newServo->setSetting(SERVO_SLOW_PRECISE); + return newServo; +} + +void ServoManager::waitUntilFinished() { + bool finished = false; + while(!finished) { + finished = true; + PrecisionServo* servo = servos; + for(uint8_t index = 0; index < numServos; index++) { + if(index != 0) { + servo += sizeof(PrecisionServo); + } + + if(servo->iterationsLeftOnCurrentIncrement != 0 || servo->angleDegrees != servo->targetAngleDegrees) { + finished = false; + break; + } + } + } +} + +int32_t calculateOntimeFor(uint8_t degrees) { + return (int32_t)(MICROSECONDS_FOR_0 + (MICROSECONDS_FOR_180 - MICROSECONDS_FOR_0) * (degrees / 180.0)); +} + +int32_t sendServoSignal(PrecisionServo* servo) { + int32_t ontime = calculateOntimeFor(servo->angleDegrees); + + digitalWrite(servo->pin, HIGH); + delayMicroseconds(ontime); + digitalWrite(servo->pin, LOW); + + + return ontime; +} + +void interpolateServo(PrecisionServo* servo) { + if(servo->iterationsLeftOnCurrentIncrement > 0) { + servo->iterationsLeftOnCurrentIncrement--; + }else if(servo->targetChanged && servo->newTarAddIters != 0) { + servo->iterationsLeftOnCurrentIncrement = servo->newTarAddIters; + servo->targetChanged = false; + }else if(servo->angleDegrees != servo->targetAngleDegrees) { + int32_t towardAmt = servo->targetAngleDegrees - servo->angleDegrees; + int8_t towardSign = sign(towardAmt); + + if(towardSign != servo->lastDirection && servo->newDirAddIters != 0) { + servo->iterationsLeftOnCurrentIncrement = servo->newDirAddIters; + }else { + if(towardAmt >= -servo->maxMovement && towardAmt <= servo->maxMovement) { + servo->angleDegrees = servo->targetAngleDegrees; + }else { + servo->angleDegrees += sign(towardAmt) * servo->maxMovement; + } + + servo->iterationsLeftOnCurrentIncrement = servo->movementIters; + } + servo->lastDirection = towardSign; + } +} + +void updateServos() { + PrecisionServo* servo = servos; + + int32_t lastSleep = 0; + for(int i = 0; i < numServos; i++) { + if(i != 0) { + servo += sizeof(PrecisionServo); + delayMicroseconds(MICROSECONDS_FOR_180 - lastSleep); + } + + lastSleep = sendServoSignal(servo); + interpolateServo(servo); + } +} + +ISR(TIM1_OVF_vect) { + updateServos(); +} diff --git a/PrecisionServo/ServoManager.h b/PrecisionServo/ServoManager.h new file mode 100644 index 0000000..362ade4 --- /dev/null +++ b/PrecisionServo/ServoManager.h @@ -0,0 +1,30 @@ +#ifndef ServoManager_h +#define ServoManager_h + +#include +#include + +class ServoManager { + public: + /** + * Initial setup required for all servo related + * calls. Must be called prior to any other servo + * manager functions. + * @param numServos how many servos are going to be set up + */ + static void setup(uint8_t numServos); + + /** + * Create a new servo at the specified pin + * @param pin the pin which the servo will function on + * @return a new servo attached to that pin + */ + static PrecisionServo* createServo(uint8_t pin); + + /** + * Wait until the servos are at the target locations + */ + static void waitUntilFinished(); +}; + +#endif \ No newline at end of file diff --git a/PrecisionServo/ServoManagerold.cpp.txt b/PrecisionServo/ServoManagerold.cpp.txt new file mode 100644 index 0000000..74b5558 --- /dev/null +++ b/PrecisionServo/ServoManagerold.cpp.txt @@ -0,0 +1,162 @@ +#include + +#define MICROSECONDS_PER_TCNT0_CYCLE 256 +#define MICROSECONDS_PER_DELAY_1 3 +#define MICROSECONDS_PER_DELAY_2 4 + +#define MICROSECONDS_PER_SERVO_UPDATE 20000 + +// The maximum number of degrees that we ask the servo to move at once +// higher values -> faster, less accurate movement +#define MAX_MOVEMENT 2 + +// The number of times we send our orders to the servo when we change its +// angle by max movement. Each iteration is roughly 20ms +#define MOVEMENT_ITERATIONS 2 + +// The extra iterations we do when changing directions (each is roughly 20ms) +#define NEW_DIRECTION_ADDITIONAL_ITERATIONS 2 + +// The number of microseconds of ON per 20ms to get the servo to go to 180 +#define MICROSECONDS_FOR_180 2040 + +// The number of microseconds of ON per 20ms to get the servo to go to 0 +#define MICROSECONDS_FOR_0 200 + +int8_t sign(int16_t num) { + if(num < 0) { + return -1; + }else if(num == 0) { + return 0; + }else { + return 1; + } +} + +/** + * Stores how many microseconds before we should send information + * to the servos + */ +static uint32_t microsecondsUntilNextCycle = 0; +static bool currentlyUpdatingServos = false; +static PrecisionServo* servos = NULL; +static uint8_t numServos = 0; +static uint8_t totalServos = 0; + +// TODO REMOVE +static int8_t blueLed = 1; +static int8_t greenLed = 2; +static int8_t redLed = 3; +// END TODO + +void ServoManager::setup(uint8_t totServos) { + // TIMSK - Timer/Counter Interrupt Mask Register + // OCIE0A - Timer/Counter0 Output Compare Match A Interrupt Enable + // TOIE0 - Timer/Counter0 Overflow Interrupt Enable + TIMSK |= _BV(TOIE1); + + if(totServos <= 0) { + // ideally we'd raise an error + totalServos = -1; + return; + } + + servos = (PrecisionServo*)malloc(sizeof(PrecisionServo) * totServos); + numServos = 0; + totalServos = totServos; + + pinMode(blueLed, OUTPUT); + pinMode(greenLed, OUTPUT); + pinMode(redLed, OUTPUT); + // enable global interrupts + sei(); +} + +PrecisionServo* ServoManager::createServo(uint8_t pin) { + if(numServos >= totalServos) + return NULL; // ideally we'd raise an error + + PrecisionServo* newServo = (servos + sizeof(PrecisionServo) * numServos); + numServos++; + + pinMode(pin, OUTPUT); + newServo->pin = pin; + newServo->angleDegrees = 0; + newServo->targetAngleDegrees = 0; + newServo->iterationsLeftOnCurrentIncrement = 10; + newServo->lastDirection = 0; + return newServo; +} + +void ServoManager::waitUntilFinished() { + // todo +} + +void updateServos() { + // first we busywait until we use up the remaining microseconds + delayMicroseconds(microsecondsUntilNextCycle); + + uint32_t usedMicroseconds = 0; + // now we update the servos one at a time + for(int8_t servoIndex = 0; servoIndex < numServos; servoIndex++) { + PrecisionServo* servo = (servos + sizeof(PrecisionServo) * servoIndex); + if(servo->iterationsLeftOnCurrentIncrement == 0) { + if(servo->angleDegrees != servo->targetAngleDegrees) { + int8_t directionRaw = (int8_t)((int16_t)servo->targetAngleDegrees - (int16_t)servo->angleDegrees); + int8_t direction = sign(directionRaw); + uint8_t newAngle; + if(directionRaw > -MAX_MOVEMENT && directionRaw < MAX_MOVEMENT) { + newAngle = servo->targetAngleDegrees; + }else { + newAngle = servo->angleDegrees + direction * MAX_MOVEMENT; + } + + uint8_t iters = MOVEMENT_ITERATIONS; + if(direction != servo->lastDirection) { + iters += NEW_DIRECTION_ADDITIONAL_ITERATIONS; + servo->lastDirection = direction; + } + + servo->angleDegrees = newAngle; + servo->iterationsLeftOnCurrentIncrement = iters; + } + } + + if(servo->iterationsLeftOnCurrentIncrement > 0) { + // this servo needs to set its position but we might not have + // reached the start of our cycle yet + int32_t targetMicroseconds = servoIndex * MICROSECONDS_FOR_180; + int32_t busyWaitMicroseconds = targetMicroseconds - usedMicroseconds; + delayMicroseconds(busyWaitMicroseconds); + usedMicroseconds += busyWaitMicroseconds; + + // now we're definitely at the start of our cycle + double percentOf180 = servo->angleDegrees / 180.0; + + busyWaitMicroseconds = (int32_t)(MICROSECONDS_FOR_0 + percentOf180 * (MICROSECONDS_FOR_180 - MICROSECONDS_FOR_0)); + digitalWrite(servo->pin, HIGH); + digitalWrite(greenLed, HIGH); + delayMicroseconds(busyWaitMicroseconds); + usedMicroseconds += busyWaitMicroseconds; + digitalWrite(servo->pin, LOW); + digitalWrite(greenLed, LOW); + servo->iterationsLeftOnCurrentIncrement--; + } + } + + microsecondsUntilNextCycle = MICROSECONDS_PER_SERVO_UPDATE - usedMicroseconds; +} + +ISR(TIM1_OVF_vect) { + if(microsecondsUntilNextCycle > MICROSECONDS_PER_TCNT0_CYCLE) { + digitalWrite(blueLed, LOW); + microsecondsUntilNextCycle -= MICROSECONDS_PER_TCNT0_CYCLE; + } + + if(microsecondsUntilNextCycle <= MICROSECONDS_PER_TCNT0_CYCLE && !currentlyUpdatingServos) { + digitalWrite(blueLed, HIGH); + currentlyUpdatingServos = true; + updateServos(); + currentlyUpdatingServos = false; + } +} diff --git a/README.md b/README.md index 31c8cd8..89a2ab6 100644 --- a/README.md +++ b/README.md @@ -1 +1,72 @@ # PrecisionServo + +PrecisionServo provides an alternative to [SoftwareServo](https://github.com/nicolaskruchten/arduino/tree/master/libraries/SoftwareServo) for ATTiny's which do not work with the Arduino built-in servo library. This library was tested with an ATTiny85 via a usb programmer on an Arduino IDE with the Tower Pro Micro Servo 9g (an SG90 servo). The servos positive power should first go through a voltage regulator (I use L7805CV - a 5 volt regulator). + +This library uses the Timer/Counter1 in order to control the servo. Thus, when using this library, you cannot edit TIMSK bit TOIE1, TCCR1, OCR1C, or PLLCSR bits related to timer1, or any other settings that would modify the behavior of those registry masks. + +## Simple example + +This is from single_server_sweep in the examples: + +```cpp +/** + * Sweeps a single servo on the medium endoint setting from 0 to 180. + */ + +#include + +const int servoPin = 0; + +PrecisionServo* servo; + +void setup() { + ServoManager::setup(1); + + servo = ServoManager::createServo(servoPin); + servo->setSetting(SERVO_MEDIUM_ENDPOINT); +} + +void loop() { + servo->setTarget(0); + ServoManager::waitUntilFinished(); + + servo->setTarget(180); + ServoManager::waitUntilFinished(); +} +``` + +## Usage explained + +The servo manager should be setup with the number of unique servos on the circuit first, with `ServoManager::setup`. Then, create each servo one at a time by calling `ServoManager::createServo` with the corresponding pin. The returned servo is ready to go, however you might want to change the setting (see Settings below) using `PrecisionServo->setSetting`. Every servo may have its own setting. + +From there, call setTarget on the servo with the target direction in degrees. Immediately after this function call the servo manager will start to move the servo in that direction, interrupting any running code in order to do so (so it acts kind-of asynchronously from your perspective). You might want to wait until the servo actually reaches its target before moving on, which can be done by calling `ServoManager::waitUntilFinished`. You can change the target at any time, even prior to it reaching its old target, and the servo manager will automatically start moving the servo in that direction. + +Targets are *not* queued, so if you change the target before it reaches the destination, the servo range will appear to be clipped. + +Since the servo manager works in the background, you can set a different servos target while a servo is moving. See examples/two_servos_settings_sweep.ino + +## Settings + +There are many different modes of operation that you can set on the servos via `PrecisionServo->setSetting`. + +### Constants + +- `SERVO_SLOW_SMOOTH` +- `SERVO_SLOW_PRECISE` +- `SERVO_SLOW_ENDPOINT` +- `SERVO_MEDIUM_SMOOTH` +- `SERVO_MEDIUM_PRECISE` +- `SERVO_MEDIUM_ENDPOINT` +- `SERVO_FAST_SMOOTH` +- `SERVO_FAST_PRECISE` +- `SERVO_FAST_ENDPOINT` + +SLOW/MEDIUM/FAST controls how rapidly the servo is going to the target + +- **SMOOTH**: No additional pauses are used; the servo chases the target but often won't quite reach it. This is great for if you are trying to follow a sensor and the angles your asking for are inherently inprecise. +- **PRECISE**: The servo pauses at the *old* target whenever the target changes. This helps ensure it makes it exactly to the requested angles, assuming that you are using waitUntilFinished or sufficient delays between changing targets. This is ideal if you are hardcoding a sequence of movements and want the most consistent behavior possible. +- **ENDPOINT**: Acts very similiarly to PRECISE, except skips the pause when given orders that continue in the same direction (the servo is often less accurate when changing directions) + +## Calibration + +Depending on the exact amount of resistance on your circuit and the imperfections in the servos themself, they might not act exactly like you expect on the default settings. If the range seems off, verify no wires are loose, that a voltage regulator is in place, and that your batteries are charged (if powered off batteries) and then modify the MICROSECONDS_FOR_180 and MICROSECONDS_FOR_0 definitions at the top of ServoManager.cpp. \ No newline at end of file diff --git a/examples/single_servo_sweep.ino b/examples/single_servo_sweep.ino new file mode 100644 index 0000000..4236377 --- /dev/null +++ b/examples/single_servo_sweep.ino @@ -0,0 +1,24 @@ +/** + * Sweeps a single servo on the medium endoint setting from 0 to 180. + */ + +#include + +const int servoPin = 0; + +PrecisionServo* servo; + +void setup() { + ServoManager::setup(1); + + servo = ServoManager::createServo(servoPin); + servo->setSetting(SERVO_MEDIUM_ENDPOINT); +} + +void loop() { + servo->setTarget(0); + ServoManager::waitUntilFinished(); + + servo->setTarget(180); + ServoManager::waitUntilFinished(); +} diff --git a/examples/two_servo_settings_sweep.ino b/examples/two_servo_settings_sweep.ino new file mode 100644 index 0000000..59b9dc1 --- /dev/null +++ b/examples/two_servo_settings_sweep.ino @@ -0,0 +1,56 @@ +/* + * Runs two servos of the servo manager. + * + * Servos should be connected to pins 0 and 1 and should + * be run through a voltage regulator. + * + * Cycles each servo through each of the 9 settings at the + * same rate, except the servo on pin 1 is 4 settings "ahead" + * of the servo on pin 0. + * + * For showing the setting, the servos go to 0, then to 180, + * then to 90, then pause and increment their setting. + * + * When the servo seems to pause at the edges it's likely at + * either a precise or endpoint setting. When it seems to skip + * the endpoint (often seeming to lose some range of motion) + * it is on one of the smooth settings. + */ + +#include + +const int servoPin1 = 0; +const int servoPin2 = 1; + +int setting = 0; +PrecisionServo* servo1; +PrecisionServo* servo2; + +void setup() { + ServoManager::setup(2); + + servo1 = ServoManager::createServo(servoPin1); + servo1->setSetting(setting); + + servo2 = ServoManager::createServo(servoPin2); + servo2->setSetting((setting + 4) % 9); +} + +void loop() { + servo1->setTarget(0); + servo2->setTarget(0); + ServoManager::waitUntilFinished(); + + servo1->setTarget(180); + servo2->setTarget(180); + ServoManager::waitUntilFinished(); + + servo1->setTarget(90); + servo2->setTarget(90); + ServoManager::waitUntilFinished(); + delay(500); + + setting = (setting + 1) % 9; + servo1->setSetting(setting); + servo2->setSetting((setting + 4) % 9); +}