aboutsummaryrefslogtreecommitdiffstats
path: root/libraries
diff options
context:
space:
mode:
Diffstat (limited to 'libraries')
-rw-r--r--libraries/Servo/Servo.cpp148
-rw-r--r--libraries/Servo/Servo.h213
-rw-r--r--libraries/Servo/rules.mk12
3 files changed, 231 insertions, 142 deletions
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 <stdint.h>
+#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))