Added set and get functions to Servo

libraries/Servo/Servo.cpp

@@ -118,6 +118,13 @@ int Servo::read() const {
     return a == this->minAngle || a == this->maxAngle ? a : a + 1;
 }
 
+void Servo::set(uint16 pulseWidth) {
+    if (!this->attached()) {
+        ASSERT(0);
+        return;
+    }
+    pwmWrite(this->pin, pulseWidth);
+}
 void Servo::writeMicroseconds(uint16 pulseWidth) {
     if (!this->attached()) {
         ASSERT(0);
@@ -128,6 +135,10 @@ void Servo::writeMicroseconds(uint16 pulseWidth) {
     pwmWrite(this->pin, US_TO_COMPARE(pulseWidth));
 }
 
+uint16 Servo::get() const {
+    stm32_pin_info pin_info = PIN_MAP[this->pin];
+    return timer_get_compare(pin_info.timer_device, pin_info.timer_channel);
+}
 uint16 Servo::readMicroseconds() const {
     if (!this->attached()) {
         ASSERT(0);

libraries/Servo/Servo.h

@@ -79,6 +79,49 @@ class Servo {
      */
     Servo();
 
+    /**
+     * @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.
+     */
+    Servo(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)
+    {
+        attach(pin, minPulseWidth, maxPulseWidth, minAngle, maxAngle);
+    };
+
     /**
      * @brief Associate this instance with a servomotor whose input is
      *        connected to pin.
@@ -183,6 +226,22 @@ class Servo {
      */
     uint16 readMicroseconds() const;
 
+    /**
+     * @brief Set the pulse width, raw value.
+     *
+     * @param pulseWidth Raw pulse width to send to the servomotor.
+     *
+     * @see Servo::attach()
+     */
+    void set(uint16 pulseWidth);
+
+    /**
+     * Get the current pulse width, raw value.
+     *
+     * @see Servo::attach()
+     */
+    uint16 get() const;
+
 private:
     int16 pin;
     uint16 minPW;