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. --- libraries/Servo/Servo.cpp | 148 +++++++++++++++++--------------- libraries/Servo/Servo.h | 213 ++++++++++++++++++++++++++++++++-------------- libraries/Servo/rules.mk | 12 +-- 3 files changed, 231 insertions(+), 142 deletions(-) (limited to 'libraries') 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