Initial porting to mbed cores

Author: facchinm

library.properties

@@ -6,4 +6,4 @@ sentence=Allows Arduino/Genuino boards to control a variety of servo motors.
 paragraph=This library can control a great number of servos.<br />It makes careful use of timers: the library can control 12 servos using only 1 timer.<br />On the Arduino Due you can control up to 60 servos.<br />
 category=Device Control
 url=http://www.arduino.cc/en/Reference/Servo
-architectures=avr,megaavr,sam,samd,nrf52,stm32f4
+architectures=avr,megaavr,sam,samd,nrf52,stm32f4,mbed

src/Servo.h

@@ -71,6 +71,8 @@
 #include "nrf52/ServoTimers.h"
 #elif defined(ARDUINO_ARCH_MEGAAVR)
 #include "megaavr/ServoTimers.h"
+#elif defined(ARDUINO_ARCH_MBED)
+#include "mbed/ServoTimers.h"
 #else
 #error "This library only supports boards with an AVR, SAM, SAMD, NRF52 or STM32F4 processor."
 #endif

src/mbed/Servo.cpp

@@ -0,0 +1,132 @@
+#if defined(ARDUINO_ARCH_MBED)
+
+#include <Arduino.h>
+#include <Servo.h>
+
+class ServoImpl {
+    mbed::DigitalOut   *pin;
+    mbed::Timeout      timeout;  // calls a callback once when a timeout expires
+    mbed::Ticker       ticker;   // calls a callback repeatedly with a timeout
+    uint32_t           duration;
+
+public:
+    ServoImpl(PinName _pin) {
+      pin = new mbed::DigitalOut(_pin);
+    }
+
+    ~ServoImpl() {
+        ticker.detach();
+        timeout.detach();
+        delete pin;
+    }
+
+    void start(uint32_t duration_us) {
+      duration = duration_us;
+      ticker.attach(mbed::callback(this, &ServoImpl::call), 0.02f);
+    }
+
+    void call() {
+        timeout.attach(mbed::callback(this, &ServoImpl::toggle), duration / 1e6);
+        toggle();
+    }
+
+    void toggle() {
+      *pin = !*pin;
+    }
+
+    uint32_t read() {
+      return duration;
+    }
+};
+
+static ServoImpl* servos[MAX_SERVOS];                      // static array of servo structures
+uint8_t ServoCount = 0;                                    // the total number of attached servos
+
+#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min)   // minimum value in uS for this servo
+#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max)   // maximum value in uS for this servo
+
+#define TRIM_DURATION   15                           //callback overhead (35 uS) -> 15uS if toggle() is called after starting the timeout
+
+Servo::Servo()
+{
+  if (ServoCount < MAX_SERVOS) {
+    this->servoIndex = ServoCount++;
+  } else {
+    this->servoIndex = INVALID_SERVO;  // too many servos
+  }
+}
+
+uint8_t Servo::attach(int pin)
+{
+  return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+}
+
+uint8_t Servo::attach(int pin, int min, int max)
+{
+  pinMode(pin, OUTPUT);                                   // set servo pin to output
+  servos[this->servoIndex] = new ServoImpl(digitalPinToPinName(pin));
+
+  this->min  = (MIN_PULSE_WIDTH - min);
+  this->max  = (MAX_PULSE_WIDTH - max);
+  return this->servoIndex;
+}
+
+void Servo::detach()
+{
+  delete servos[this->servoIndex];
+  servos[this->servoIndex] = NULL;
+}
+
+void Servo::write(int value)
+{
+  // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+  if (value < MIN_PULSE_WIDTH)
+  {
+    if (value < 0)
+      value = 0;
+    else if (value > 180)
+      value = 180;
+
+    value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
+  }
+  writeMicroseconds(value);
+}
+
+void Servo::writeMicroseconds(int value)
+{
+  if (!servos[this->servoIndex]) {
+    return;
+  }
+  // calculate and store the values for the given channel
+  byte channel = this->servoIndex;
+  if( (channel < MAX_SERVOS) )   // ensure channel is valid
+  {
+    if (value < SERVO_MIN())          // ensure pulse width is valid
+      value = SERVO_MIN();
+    else if (value > SERVO_MAX())
+      value = SERVO_MAX();
+
+    value = value - TRIM_DURATION;
+    servos[this->servoIndex]->start(value);
+  }
+}
+
+int Servo::read() // return the value as degrees
+{
+  return map(readMicroseconds(), SERVO_MIN(), SERVO_MAX(), 0, 180);
+}
+
+int Servo::readMicroseconds()
+{
+  if (!servos[this->servoIndex]) {
+    return 0;
+  }
+  return servos[this->servoIndex]->read();
+}
+
+bool Servo::attached()
+{
+  return servos[this->servoIndex] != NULL;
+}
+
+#endif

src/mbed/ServoTimers.h

@@ -0,0 +1 @@
+#define _Nbr_16timers 32