3 files changed, 231 insertions, 141 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 1c75618..f80339e 100644
--- a/libraries/Servo/Servo.h
+++ b/libraries/Servo/Servo.h
@@ -25,92 +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 */
-
-/* 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))