From 83e240f25035e34e4a43273122cb17d3f4725dac Mon Sep 17 00:00:00 2001 From: Marti Bolivar Date: Fri, 1 Apr 2011 01:01:22 -0400 Subject: Redid Servo in terms of new timer.h. --- Makefile | 2 +- examples/test-servo.cpp | 152 +++++++++++++++++++++++++++++++++ libraries/Servo/Servo.cpp | 148 +++++++++++++++++--------------- libraries/Servo/Servo.h | 213 ++++++++++++++++++++++++++++++++-------------- libraries/Servo/rules.mk | 12 +-- 5 files changed, 384 insertions(+), 143 deletions(-) create mode 100644 examples/test-servo.cpp diff --git a/Makefile b/Makefile index 109a126..d31c9ed 100644 --- a/Makefile +++ b/Makefile @@ -97,7 +97,7 @@ endif LIBMAPLE_MODULES := $(SRCROOT)/libmaple LIBMAPLE_MODULES += $(SRCROOT)/wirish # Official libraries: -# LIBMAPLE_MODULES += $(SRCROOT)/libraries/Servo +LIBMAPLE_MODULES += $(SRCROOT)/libraries/Servo LIBMAPLE_MODULES += $(SRCROOT)/libraries/LiquidCrystal LIBMAPLE_MODULES += $(SRCROOT)/libraries/Wire diff --git a/examples/test-servo.cpp b/examples/test-servo.cpp new file mode 100644 index 0000000..b6b8cd5 --- /dev/null +++ b/examples/test-servo.cpp @@ -0,0 +1,152 @@ +/* + * Basic Servo library test program. + * + * Setup: + * + * - Connect a potentiometer to POT_PIN (default pin 15) + * - Connect an oscilloscope to SERVO_PIN1 (default pin 5) and + * SERVO_PIN2 (default pin 6). + * - Connect a serial monitor to SerialUSB + * + * The potentiometer controls the target angle for each of two Servo + * objects, one with angles in [-90, 90], and another in [0, 180]. + * Servo pulse width range is [1000, 2000]. + * + * Serial2 will tell you what inputs it's giving to each servo object, + * and some information it gets back. Pressing the button + * detaches/reattaches the Servo objects. + * + * Tests you should perform: + * + * - Check calculated pulse widths for each servo's target angle + * - Check that calculated pulse widths match actual pulse widths + * - Check that the period of the pulse train is roughly 20 ms + * - Check that the pulses stop when detached, and resume when reattached + * - Check that Servo::write() and Servo::read() round-trip properly + * + * This file is released into the public domain. + */ + +#include + +#include "wirish.h" + +#include "libraries/Servo/Servo.h" + +#define POT_PIN 15 + +#define MIN_PW 1000 +#define MAX_PW 2000 + +#define SERVO_PIN1 5 +#define MIN_ANGLE1 0 +#define MAX_ANGLE1 180 + +#define SERVO_PIN2 6 +#define MIN_ANGLE2 (-90) +#define MAX_ANGLE2 90 + +Servo servo1; +Servo servo2; + +#define BUF_SIZE 100 +char buf[BUF_SIZE]; + +#define print_buf(fmt, ...) do { \ + snprintf(buf, BUF_SIZE, fmt, __VA_ARGS__); \ + Serial2.println(buf); } while (0) + +int averageAnalogReads(int); +void attach(); +void detach(); + +void setup() { + pinMode(POT_PIN, INPUT_ANALOG); + pinMode(BOARD_BUTTON_PIN, INPUT); + pinMode(BOARD_LED_PIN, OUTPUT); + + Serial2.begin(9600); + + servo1.attach(SERVO_PIN1, MIN_PW, MAX_PW, MIN_ANGLE1, MAX_ANGLE1); + servo2.attach(SERVO_PIN2, MIN_PW, MAX_PW, MIN_ANGLE2, MAX_ANGLE2); + + ASSERT(servo1.attachedPin() == SERVO_PIN1); + ASSERT(servo2.attachedPin() == SERVO_PIN2); +} + +void loop() { + delay(250); + toggleLED(); + + if (isButtonPressed()) { + if (servo1.attached()) detach(); + else attach(); + } + + if (!servo1.attached()) return; + + int32 average = averageAnalogReads(250); + int16 angle1 = (int16)map(average, 0, 4095, MIN_ANGLE1, MAX_ANGLE1); + int16 angle2 = (int16)map(average, 0, 4095, MIN_ANGLE2, MAX_ANGLE2); + + print_buf("pot reading = %d, angle 1 = %d, angle 2 = %d.", + average, angle1, angle2); + + servo1.write(angle1); + servo2.write(angle2); + + int16 read1 = servo1.read(); + int16 read2 = servo2.read(); + + print_buf("write/read angle 1: %d/%d, angle 2: %d/%d", + angle1, read1, angle2, read2); + + ASSERT(abs(angle1 - read1) <= 1); + ASSERT(abs(angle2 - read2) <= 1); + + print_buf("pulse width 1: %d, pulse width 2: %d", + servo1.readMicroseconds(), servo2.readMicroseconds()); + + Serial2.println("\n--------------------------\n"); +} + +int32 averageAnalogReads(int n) { + uint64 total = 0; + + for (int i = 0; i < n; i++) { + total += analogRead(POT_PIN); + } + + return (int32)(total / n); +} + +void attach() { + Serial2.println("attaching"); + servo1.attach(SERVO_PIN1); + servo2.attach(SERVO_PIN2); + ASSERT(servo1.attachedPin() == SERVO_PIN1); + ASSERT(servo2.attachedPin() == SERVO_PIN2); +} + +void detach() { + Serial2.println("detaching"); + servo1.detach(); + servo2.detach(); + ASSERT(!servo1.attached()); + ASSERT(!servo2.attached()); +} + +// Force init to be called *first*, i.e. before static object allocation. +// Otherwise, statically allocated objects that need libmaple may fail. +__attribute__((constructor)) void premain() { + init(); +} + +int main(void) { + setup(); + + while (true) { + loop(); + } + return 0; +} diff --git a/libraries/Servo/Servo.cpp b/libraries/Servo/Servo.cpp index ae87b63..5122391 100644 --- a/libraries/Servo/Servo.cpp +++ b/libraries/Servo/Servo.cpp @@ -29,110 +29,120 @@ #include "pwm.h" #include "wirish_math.h" -// Configure prescaler and overflow for a 20msec period (could just -// use HardwareTimer::setPeriod(), but this lets conversions below -// happen more statically, in combination with an inlined map() -- a -// premature optimization? TODO profile speed/size tradeoff) -#define CYC_20MSEC (20000 * CYCLES_PER_MICROSECOND) -#define SERVO_PRE ((uint16)((CYC_20MSEC >> 16) + 1)) -#define SERVO_OVF ((uint16)((CYC_20MSEC / SERVO_PRE) - 1)) -#define SERVO_TAU_USEC \ - ((uint32)(((double)SERVO_OVF) * SERVO_PRE / CYCLES_PER_MICROSECOND + 0.5)) - -#define US_TO_COMPARE(us) ((uint16)map(us, 0, SERVO_TAU_USEC, 0, SERVO_OVF)) -#define COMPARE_TO_US(c) ((uint32)map(c, 0, SERVO_OVF, 0, SERVO_TAU_USEC)) - -#define ANGLE_TO_US(a) ((uint16)(map(a, 0, 180, this->min, this->max))) -#define US_TO_ANGLE(us) ((uint8)(map(us, this->min, this->max, 0, 180))) +// 20 millisecond period config. For a 1-based prescaler, +// +// (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec +// => prescaler * overflow = 20 * CYC_MSEC +// +// This picks the smallest prescaler that allows an overflow < 2^16. +#define MAX_OVERFLOW ((1 << 16) - 1) +#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND) +#define TAU_MSEC 20 +#define TAU_USEC (TAU_MSEC * 1000) +#define TAU_CYC (TAU_MSEC * CYC_MSEC) +#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1) +#define SERVO_OVERFLOW ((uint16)round((double)TAU_CYC / SERVO_PRESCALER)) + +// Unit conversions +#define US_TO_COMPARE(us) ((uint16)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW)) +#define COMPARE_TO_US(c) ((uint32)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC)) +#define ANGLE_TO_US(a) ((uint16)(map((a), this->minAngle, this->maxAngle, \ + this->minPW, this->maxPW))) +#define US_TO_ANGLE(us) ((int16)(map((us), this->minPW, this->maxPW, \ + this->minAngle, this->maxAngle))) Servo::Servo() { - this->pin = NOT_ATTACHED; - this->timer = 0; - this->channel = TIMER_INVALID; - this->min = SERVO_DEFAULT_MIN_PW; - this->max = SERVO_DEFAULT_MAX_PW; + this->resetFields(); } -bool Servo::attach(uint8_t pin) { - return this->attach(pin, SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW); -} +bool Servo::attach(uint8 pin, + uint16 minPW, + uint16 maxPW, + int16 minAngle, + int16 maxAngle) { + timer_dev *tdev = PIN_MAP[pin].timer_device; -bool Servo::attach(uint8_t pin, uint16_t min, uint16_t max) { - timer_dev_num timer_num = PIN_MAP[pin].timer_num; - uint32_t channel = PIN_MAP[pin].timer_chan; - if (timer_num == TIMER_INVALID) { - // don't reset any members or ASSERT(0), to keep driving any + if (tdev == NULL) { + // don't reset any fields or ASSERT(0), to keep driving any // previously attach()ed servo. return false; } + + if (this->attached()) { + this->detach(); + } + this->pin = pin; - this->timer = getTimer(timer_num); - this->channel = channel; - this->min = min; - this->max = max; + this->minPW = minPW; + this->maxPW = maxPW; + this->minAngle = minAngle; + this->maxAngle = maxAngle; pinMode(pin, PWM); - this->timer->pause(); - this->timer->setPrescaleFactor(SERVO_PRE); - this->timer->setOverflow(SERVO_OVF); - this->timer->generateUpdate(); - this->timer->resume(); + timer_pause(tdev); + timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based + timer_set_reload(tdev, SERVO_OVERFLOW); + timer_generate_update(tdev); + timer_resume(tdev); + return true; } bool Servo::detach() { - if (this->pin == NOT_ATTACHED) return false; + if (!this->attached()) { + return false; + } - this->timer->setChannelMode(this->channel, TIMER_DISABLED); + timer_dev *tdev = PIN_MAP[this->pin].timer_device; + uint8 tchan = PIN_MAP[this->pin].timer_channel; + timer_set_mode(tdev, tchan, TIMER_DISABLED); - this->pin = NOT_ATTACHED; - this->timer = 0; - this->channel = TIMER_INVALID; - this->min = SERVO_DEFAULT_MIN_PW; - this->max = SERVO_DEFAULT_MAX_PW; + this->resetFields(); return true; } -void Servo::write(unsigned int value) { - if (value < SERVO_MAX_WRITE_ANGLE) { - this->writeMicroseconds(ANGLE_TO_US(value)); - } else { - this->writeMicroseconds(value); - } +void Servo::write(int degrees) { + degrees = constrain(degrees, this->minAngle, this->maxAngle); + this->writeMicroseconds(ANGLE_TO_US(degrees)); } -void Servo::writeMicroseconds(uint16_t pulseWidth) { - if (this->pin == NOT_ATTACHED) { +int Servo::read() const { + int a = US_TO_ANGLE(this->readMicroseconds()); + // map() round-trips in a weird way we mostly correct for here; + // the round-trip is still sometimes off-by-one for write(1) and + // write(179). + return a == this->minAngle || a == this->maxAngle ? a : a + 1; +} + +void Servo::writeMicroseconds(uint16 pulseWidth) { + if (!this->attached()) { ASSERT(0); return; } - pulseWidth = constrain(pulseWidth, this->min, this->max); + pulseWidth = constrain(pulseWidth, this->minPW, this->maxPW); pwmWrite(this->pin, US_TO_COMPARE(pulseWidth)); } -int Servo::read() const { - if (this->pin == NOT_ATTACHED) { +uint16 Servo::readMicroseconds() const { + if (!this->attached()) { ASSERT(0); return 0; } - unsigned int pw = this->readMicroseconds(); - int a = US_TO_ANGLE(pw); - // map() round-trips in a weird way we correct for here - return a == 0 || a == 180 ? a : a + 1; -} + stm32_pin_info pin_info = PIN_MAP[this->pin]; + uint16 compare = timer_get_compare(pin_info.timer_device, + pin_info.timer_channel); -uint16_t Servo::readMicroseconds() const { - if (this->pin == NOT_ATTACHED) { - ASSERT(0); - return 0; - } + return COMPARE_TO_US(compare); +} - unsigned int compare = this->timer->getCompare(this->channel); - uint16_t c = COMPARE_TO_US(compare); - // map() round-trips in a weird way we correct for here - return c == 0 || c == 180 ? c : c + 1; +void Servo::resetFields(void) { + this->pin = NOT_ATTACHED; + this->minAngle = SERVO_DEFAULT_MIN_ANGLE; + this->maxAngle = SERVO_DEFAULT_MAX_ANGLE; + this->minPW = SERVO_DEFAULT_MIN_PW; + this->maxPW = SERVO_DEFAULT_MAX_PW; } diff --git a/libraries/Servo/Servo.h b/libraries/Servo/Servo.h index 583312e..f80339e 100644 --- a/libraries/Servo/Servo.h +++ b/libraries/Servo/Servo.h @@ -25,93 +25,178 @@ #ifndef _SERVO_H_ #define _SERVO_H_ -#include +#include "libmaple_types.h" +#include "timer.h" -#include "wirish.h" /* hack for IDE compile */ -#include "HardwareTimer.h" - -/* Note on Arduino compatibility: - - In the Arduino implementation, PWM is done "by hand" in the sense - that timer channels are hijacked in groups and an ISR is set which - toggles Servo::attach()ed pins using digitalWrite(). +#include "wirish_types.h" - While this scheme allows any pin to drive a servo, it chews up - cycles and complicates the programmer's notion of when a particular - timer channel will be in use. - - This implementation only allows Servo instances to Servo::attach() - to pins that already have a timer channel associated with them, and - just uses pwmWrite() to drive the wave. - - This introduces an incompatibility: while the Arduino - implementation of attach() returns the affected channel on success - and 0 on failure, this one returns true on success and false on - failure. +#ifdef MAPLE_IDE +#include "wirish.h" /* hack for IDE compile */ +#endif - RC Servos expect a pulse every 20ms. Since periods are set for - entire timers, rather than individual channels, attach()ing a Servo - to a pin can interfere with other pins associated with the same - timer. As always, the pin mapping mega table is your friend. +/* + * Note on Arduino compatibility: + * + * In the Arduino implementation, PWM is done "by hand" in the sense + * that timer channels are hijacked in groups and an ISR is set which + * toggles Servo::attach()ed pins using digitalWrite(). + * + * While this scheme allows any pin to drive a servo, it chews up + * cycles and complicates the programmer's notion of when a particular + * timer channel will be in use. + * + * This implementation only allows Servo instances to attach() to pins + * that already have a timer channel associated with them, and just + * uses pwmWrite() to drive the wave. + * + * This introduces an incompatibility: while the Arduino + * implementation of attach() returns the affected channel on success + * and 0 on failure, this one returns true on success and false on + * failure. + * + * RC Servos expect a pulse every 20ms. Since periods are set for + * entire timers, rather than individual channels, attach()ing a Servo + * to a pin can interfere with other pins associated with the same + * timer. As always, your board's pin map is your friend. */ // Pin number of unattached pins -#define NOT_ATTACHED (-1) +#define NOT_ATTACHED (-1) // Maximum angle in degrees you can write(), exclusive. Value chosen -// for Arduino compatibility. -#define SERVO_MAX_WRITE_ANGLE (200) - -// Default min (0 deg)/max(180 deg) pulse widths, in microseconds. -// Value chosen for Arduino compatibility. -#define SERVO_DEFAULT_MIN_PW (544) -#define SERVO_DEFAULT_MAX_PW (2400) +// for Arduino compatibility. This value is part of the public API; +// DO NOT CHANGE IT. +#define SERVO_MAX_WRITE_ANGLE 200 + +// Default min/max pulse widths (in microseconds) and angles (in +// degrees). Values chosen for Arduino compatibility. These values +// are part of the public API; DO NOT CHANGE THEM. +#define SERVO_DEFAULT_MIN_PW 544 +#define SERVO_DEFAULT_MAX_PW 2400 +#define SERVO_DEFAULT_MIN_ANGLE 0 +#define SERVO_DEFAULT_MAX_ANGLE 180 class Servo { public: + /** + * @brief Construct a new Servo instance. + * + * The new instance will not be attached to any pin. + */ Servo(); - /* Pin has to have a timer channel associated with it already; - * sets pinMode to PWM and returns true iff successful (failure - * when pin doesn't support PWM). doesn't detach any ISRs - * associated with timer channel. */ - bool attach(uint8_t pin); - - /* Like attach(int), but with (inclusive) min (0 degree) and max - * (180 degree) pulse widths, in microseconds. + /** + * @brief Associate this instance with a servomotor whose input is + * connected to pin. + * + * If this instance is already attached to a pin, it will be + * detached before being attached to the new pin. This function + * doesn't detach any interrupt attached with the pin's timer + * channel. + * + * @param pin Pin connected to the servo pulse wave input. This + * pin must be capable of PWM output. + * + * @param minPulseWidth Minimum pulse width to write to pin, in + * microseconds. This will be associated + * with a minAngle degree angle. Defaults to + * SERVO_DEFAULT_MIN_PW = 544. + * + * @param maxPulseWidth Maximum pulse width to write to pin, in + * microseconds. This will be associated + * with a maxAngle degree angle. Defaults to + * SERVO_DEFAULT_MAX_PW = 2400. + * + * @param minAngle Target angle (in degrees) associated with + * minPulseWidth. Defaults to + * SERVO_DEFAULT_MIN_ANGLE = 0. + * + * @param maxAngle Target angle (in degrees) associated with + * maxPulseWidth. Defaults to + * SERVO_DEFAULT_MAX_ANGLE = 180. + * + * @sideeffect May set pinMode(pin, PWM). + * + * @return true if successful, false when pin doesn't support PWM. */ - bool attach(uint8_t pin, uint16_t min, uint16_t max); - - /* Return pin number if currently attach()ed to a pin, - NOT_ATTACHED otherwise. */ - int attached() const { return pin; } + bool attach(uint8 pin, + uint16 minPulseWidth=SERVO_DEFAULT_MIN_PW, + uint16 maxPulseWidth=SERVO_DEFAULT_MAX_PW, + int16 minAngle=SERVO_DEFAULT_MIN_ANGLE, + int16 maxAngle=SERVO_DEFAULT_MAX_ANGLE); + + /** + * @brief Check if this instance is attached to a servo. + * @return true if this instance is attached to a servo, false otherwise. + * @see Servo::attachedPin() + */ + bool attached() const { return this->pin != NOT_ATTACHED; } - /* Stop driving the wave by disabling the output compare - interrupt. Returns true if this call did anything. */ + /** + * @brief Get the pin this instance is attached to. + * @return Pin number if currently attached to a pin, NOT_ATTACHED + * otherwise. + * @see Servo::attach() + */ + int attachedPin() const { return this->pin; } + + /** + * @brief Stop driving the servo pulse train. + * + * If not currently attached to a motor, this function has no effect. + * + * @return true if this call did anything, false otherwise. + */ bool detach(); - /* If value < MAX_WRITE_ANGLE, treated as an angle in degrees. - Otherwise, it's treated as a pulse width. */ - void write(unsigned int value); + /** + * @brief Set the servomotor target angle. + * + * @param angle Target angle, in degrees. If the target angle is + * outside the range specified at attach() time, it + * will be clamped to lie in that range. + * + * @see Servo::attach() + */ + void write(int angle); - /* If outside of [min, max] determined by attach(), it is clamped - to lie in that range. */ - void writeMicroseconds(uint16_t pulseWidth); - /* Return servo target angle, in degrees. This will lie between 0 - and 180. */ + /** + * Get the servomotor's target angle, in degrees. This will + * lie inside the range specified at attach() time. + * + * @see Servo::attach() + */ int read() const; - /* Returns the current pulse width, in microseconds. This will - lie within the [min, max] range. */ - uint16_t readMicroseconds() const; + /** + * @brief Set the pulse width, in microseconds. + * + * @param pulseWidth Pulse width to send to the servomotor, in + * microseconds. If outside of the range + * specified at attach() time, it is clamped to + * lie in that range. + * + * @see Servo::attach() + */ + void writeMicroseconds(uint16 pulseWidth); + + /** + * Get the current pulse width, in microseconds. This will + * lie within the range specified at attach() time. + * + * @see Servo::attach() + */ + uint16 readMicroseconds() const; private: - int8_t pin; - HardwareTimer *timer; - int channel; - uint16_t min; - uint16_t max; + int16 pin; + uint16 minPW; + uint16 maxPW; + int16 minAngle; + int16 maxAngle; + + void resetFields(void); }; #endif /* _SERVO_H_ */ diff --git a/libraries/Servo/rules.mk b/libraries/Servo/rules.mk index 13cd364..e013754 100644 --- a/libraries/Servo/rules.mk +++ b/libraries/Servo/rules.mk @@ -5,27 +5,21 @@ d := $(dir) BUILDDIRS += $(BUILD_PATH)/$(d) # Local flags -CFLAGS_$(d) := $(WIRISH_INCLUDES) $(LIBMAPLE_INCLUDES) +CXXFLAGS_$(d) := $(WIRISH_INCLUDES) $(LIBMAPLE_INCLUDES) # Local rules and targets cSRCS_$(d) := -# examples/UDPApp/udpapp.c \ -# examples/SocketApp/socketapp.c \ -# examples/WebClient/webclient.c \ -# examples/WebServer/webserver.c \ -# examples/Flash/webserver.c \ - cppSRCS_$(d) := Servo.cpp cFILES_$(d) := $(cSRCS_$(d):%=$(d)/%) cppFILES_$(d) := $(cppSRCS_$(d):%=$(d)/%) OBJS_$(d) := $(cFILES_$(d):%.c=$(BUILD_PATH)/%.o) \ - $(cppFILES_$(d):%.cpp=$(BUILD_PATH)/%.o) + $(cppFILES_$(d):%.cpp=$(BUILD_PATH)/%.o) DEPS_$(d) := $(OBJS_$(d):%.o=%.d) -$(OBJS_$(d)): TGT_CFLAGS := $(CFLAGS_$(d)) +$(OBJS_$(d)): TGT_CXXFLAGS := $(CXXFLAGS_$(d)) TGT_BIN += $(OBJS_$(d)) -- cgit v1.2.3