From 61db54f52f32e63c895d775982fbcdcb67f2dde6 Mon Sep 17 00:00:00 2001 From: Marti Bolivar Date: Tue, 22 Mar 2011 16:59:29 -0400 Subject: Initial timer refactor. Basic PWM works. Had some problems in testing that might be due to USART bugs. HardwareTimer has been removed from the build for now; I will re-implement it in terms of the new libmaple API, but consider it deprecated. Let's come up with something better. Servo is implemented in terms of HardwareTimer, so it also has been temporarily removed from the build. pwmWrite() likely got a little bit less inefficient due to indirection, but the PIN_MAPs shrank by a pointer per PinMapping. --- libraries/Servo/Servo.h | 1 + 1 file changed, 1 insertion(+) (limited to 'libraries') diff --git a/libraries/Servo/Servo.h b/libraries/Servo/Servo.h index 1c75618..583312e 100644 --- a/libraries/Servo/Servo.h +++ b/libraries/Servo/Servo.h @@ -28,6 +28,7 @@ #include #include "wirish.h" /* hack for IDE compile */ +#include "HardwareTimer.h" /* Note on Arduino compatibility: -- cgit v1.2.3 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 (limited to 'libraries') 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