Merge pull request #105 from fprwi6labs/fix_servo_lib

Fix servo lib. 

PR done on official Servo library github: arduino-libraries/Servo#10

Author: fpistm

Diff for: libraries/Servo/library.properties

@@ -1,9 +1,9 @@
 name=Servo
-version=1.1.1
+version=1.1.2
 author=Michael Margolis, Arduino
 maintainer=Arduino <[email protected]>
-sentence=Allows Arduino boards to control a variety of servo motors. For all Arduino boards.
+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=stm32
+architectures=avr,sam,samd,nrf52,stm32f4,stm32

Diff for: libraries/Servo/Servo.h renamed to libraries/Servo/src/Servo.h

@@ -59,20 +59,36 @@
  */
 
 // Architecture specific include
-#include "ServoTimers.h"
+#if defined(ARDUINO_ARCH_AVR)
+#include "avr/ServoTimers.h"
+#elif defined(ARDUINO_ARCH_SAM)
+#include "sam/ServoTimers.h"
+#elif defined(ARDUINO_ARCH_SAMD)
+#include "samd/ServoTimers.h"
+#elif defined(ARDUINO_ARCH_STM32F4)
+#include "stm32f4/ServoTimers.h"
+#elif defined(ARDUINO_ARCH_NRF52)
+#include "nrf52/ServoTimers.h"
+#elif defined(ARDUINO_ARCH_STM32)
+#include "stm32/ServoTimers.h"
+#else
+#error "This library only supports boards with an AVR, SAM, SAMD, NRF52, STM32F4 or STM32 processor."
+#endif
 
 #define Servo_VERSION           2     // software version of this library
 
-#define MIN_PULSE_WIDTH       480     // the shortest pulse sent to a servo
-#define MAX_PULSE_WIDTH      2200     // the longest pulse sent to a servo
-#define DEFAULT_PULSE_WIDTH  1300     // default pulse width when servo is attached
+#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
+#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
+#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
 #define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds
 
-#define SERVOS_PER_TIMER       12      // the maximum number of servos controlled by one timer
+#define SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one timer
 #define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
 
 #define INVALID_SERVO         255     // flag indicating an invalid servo index
 
+#if !defined(ARDUINO_ARCH_STM32F4)
+
 typedef struct  {
   uint8_t nbr        :6 ;             // a pin number from 0 to 63
   uint8_t isActive   :1 ;             // true if this channel is enabled, pin not pulsed if false
@@ -96,10 +112,10 @@ class Servo
   int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
   bool attached();                   // return true if this servo is attached, otherwise false
 private:
-  uint8_t servoIndex;               // index into the channel data for this servo
-  int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH
-  int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH
-  stimer_t _timer;
+   uint8_t servoIndex;               // index into the channel data for this servo
+   int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH
+   int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH
 };
 
 #endif
+#endif

Diff for: libraries/Servo/Servo.cpp renamed to libraries/Servo/src/stm32/Servo.cpp

@@ -1,5 +1,5 @@
 /*
-  Copyright (c) 2015 Arduino LLC. All right reserved.
+  Copyright (c) 2017 Arduino LLC. All right reserved.
 
   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
@@ -16,20 +16,16 @@
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */
 
-//#if defined(ARDUINO_ARCH_SAMD)
+#if defined(ARDUINO_ARCH_STM32)
 
 #include <Arduino.h>
 #include <Servo.h>
 
-#define usToTicks(_us)    (_us * 2)     // converts microseconds to tick
-#define ticksToUs(_ticks) (_ticks / 2)  // converts from ticks back to microseconds
-
-#define TRIM_DURATION  2                                   // compensation ticks to trim adjust for digitalWrite delays
-
 static servo_t servos[MAX_SERVOS];                         // static array of servo structures
 static volatile int8_t timerChannel[_Nbr_16timers ];       // counter for the servo being pulsed for each timer (or -1 if refresh interval)
 
 uint8_t ServoCount = 0;                                    // the total number of attached servos
+stimer_t _timer;
 
 // convenience macros
 #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER))   // returns the timer controlling this servo
@@ -69,8 +65,8 @@ static void ServoIrqHandle(stimer_t *obj, uint32_t channel)
   }
   else {
     // finished all channels so wait for the refresh period to expire before starting over
-    if( getTimerCounter(obj) + 4 < usToTicks(REFRESH_INTERVAL) ) {  // allow a few ticks to ensure the next OCR1A not missed
-      setCCRRegister(obj, channel, (unsigned int)usToTicks(REFRESH_INTERVAL));
+    if( getTimerCounter(obj) + 4 < REFRESH_INTERVAL ) {  // allow a few ticks to ensure the next OCR1A not missed
+      setCCRRegister(obj, channel, (unsigned int)REFRESH_INTERVAL);
     } else {
       setCCRRegister(obj, channel, getTimerCounter(obj) + 4);  // at least REFRESH_INTERVAL has elapsed
     }
@@ -80,6 +76,11 @@ static void ServoIrqHandle(stimer_t *obj, uint32_t channel)
 
 static void initISR(stimer_t *obj)
 {
+  /*
+   * Timer clock set by default at 1us.
+   * Period set to REFRESH_INTERVAL*3
+   * Default pulse width set to DEFAULT_PULSE_WIDTH
+   */
   TimerPulseInit(obj, REFRESH_INTERVAL*3, DEFAULT_PULSE_WIDTH, ServoIrqHandle);
 }
 
@@ -104,7 +105,7 @@ Servo::Servo()
 {
   if (ServoCount < MAX_SERVOS) {
     this->servoIndex = ServoCount++;                    // assign a servo index to this instance
-    servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values
+    servos[this->servoIndex].ticks = DEFAULT_PULSE_WIDTH;   // store default values
   } else {
     this->servoIndex = INVALID_SERVO;  // too many servos
   }
@@ -173,8 +174,6 @@ void Servo::writeMicroseconds(int value)
     else if (value > SERVO_MAX())
       value = SERVO_MAX();
 
-    value = value - TRIM_DURATION;
-    value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead
     servos[channel].ticks = value;
   }
 }
@@ -188,7 +187,7 @@ int Servo::readMicroseconds()
 {
   unsigned int pulsewidth;
   if (this->servoIndex != INVALID_SERVO)
-    pulsewidth = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION;
+    pulsewidth = servos[this->servoIndex].ticks;
   else
     pulsewidth  = 0;
 
@@ -199,3 +198,5 @@ bool Servo::attached()
 {
   return servos[this->servoIndex].Pin.isActive;
 }
+
+#endif // ARDUINO_ARCH_STM32

Diff for: libraries/Servo/ServoTimers.h renamed to libraries/Servo/src/stm32/ServoTimers.h

@@ -1,5 +1,5 @@
 /*
-  Copyright (c) 2015 Arduino LLC. All right reserved.
+  Copyright (c) 2017 Arduino LLC. All right reserved.
 
   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
@@ -27,9 +27,10 @@
 #ifndef __SERVO_TIMERS_H__
 #define __SERVO_TIMERS_H__
 
-typedef enum { 
+// Uses one timer. Allows until 12 servos.
+typedef enum {
   _timer1,
-  _Nbr_16timers 
+  _Nbr_16timers
 } timer16_Sequence_t;
 
 #endif   // __SERVO_TIMERS_H__