Fix for issue chipKIT32#575 : Adding "servo" wrapper class to SoftPMW…

…Servo

* This is needed for the new Firmata support
* Also fixed FubarinoSD analog mapping macro so that the new Firmata would work properly
* Also fixed some subtle bugs in SoftPWMServo that were not exposed until Firmata really exercised it a lot.

hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.cpp

@@ -37,6 +37,7 @@
     * Fixed bug that caused glitches every 107 seconds.
   06/19/2015 <BrianSchmalz>:
     * Fixed bug that crashed library when non-existant pin was passed in (chipKIT32-MAX #572)
+    * Changed default frame time from 2ms to 2.5ms to match Arduino servo better
 */
 
 
@@ -208,6 +209,7 @@ int32_t SoftPWMServoPinDisable(uint32_t Pin)
     // Mark it as unused
     Chan[InactiveBuffer][Pin].SetPort = NULL;
     Chan[InactiveBuffer][Pin].ClearPort = NULL;
+    Chan[InactiveBuffer][Pin].PWMValue = 0;
     restoreInterrupts(intr);    
 
     return SOFTPWMSERVO_OK;
@@ -238,17 +240,7 @@ int32_t SoftPWMServoRawWrite(uint32_t Pin, uint32_t Value, bool PinType)
     {
         Value = FrameTime;
     }
-    if (Pin > SOFTPWMSERVO_MAX_PINS)
-    {
-        return SOFTPWMSERVO_ERROR;
-    }
 
-    // And if this pin already has this PWM Value, then don't do anything.
-    if (Value == Chan[ActiveBuffer][Pin].PWMValue)
-    {
-        return SOFTPWMSERVO_OK;
-    }
-
     // The easy way to prevent the ISR from doing a buffer swap while
     // we're in the middle of this is to disable interrupts during 
     // the time that we're mucking with the list.
@@ -721,3 +713,141 @@ static void CopyBuffers(void)
     }
     InactiveBufferReady = true;
 }
+
+/*************************************************************************/
+/*           Public Servo class member functions                         */
+/*************************************************************************/
+
+/*
+ * Nothing to do in the constructor really
+ */
+SoftServo::SoftServo()
+{
+    // Initialize some values in case we get asked about them
+    this->pin = 255;
+    this->min = MIN_PULSE_WIDTH;
+    this->max = MAX_PULSE_WIDTH;
+    this->isAttached = false;
+}
+
+/*
+ * Set up this object with the pin, min and max pulse widths
+ */
+uint8_t SoftServo::attach(int pin)
+{
+    return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+}
+
+/*
+ * Record the pin, min and max pulse widths for this SoftServo object
+ * For some reason, some sketches think that sending in -1 for min or max
+ * will make them take on their 'default' values. Well, OK then.
+ */
+uint8_t SoftServo::attach(int pin, int min, int max)
+{
+    this->pin = pin;
+    if (min != -1)
+    {
+        this->min = min;
+    }
+    else
+    {
+        this->min = MIN_PULSE_WIDTH;
+    }
+    if (max != -1)
+    {
+        this->max = max;
+    }
+    else
+    {
+        this->max = MAX_PULSE_WIDTH;
+    }
+    // Always start out at the default pulse width and turn on the output
+    this->write(DEFAULT_PULSE_WIDTH);
+    this->isAttached = true;
+    return this->pin;
+}
+
+/*
+ * Turn off the SoftPWMServo for this pin
+ */
+void SoftServo::detach()
+{
+    SoftPWMServoPinDisable(this->pin);
+    this->isAttached = false;
+}
+
+/*
+ * Set the pin to a new pulse width (value).
+ * If value is less than MIN_PULSE_WIDTH, treat it as a angular measurement in degrees
+ * and scale it between min and max (from 0 to 179 degrees).
+ * Otherwise treat it as a microsecond value.
+ */
+void SoftServo::write(int value)
+{
+    if (value < MIN_PULSE_WIDTH)
+    {
+        // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+        if (value < 0) value = 0;
+        if (value > 179) value = 179;
+
+        value = map(value, 0, 179, this->min, this->max);
+    }
+    this->writeMicroseconds(value);
+}
+
+/*
+ * Write a new pulse width down to the SoftPWMServo code
+ * This will turn on (enable) a pin for Servo output if it's not already 
+ * turned on.
+ */
+void SoftServo::writeMicroseconds(int value)
+{
+    if ( value < this->min )          // ensure pulse width is valid
+        value = this->min;
+    else if ( value > this->max )
+        value = this->max;
+
+    SoftPWMServoServoWrite(this->pin, value);
+}
+
+/*
+ * Return the current pulse time as a valule of degrees from 0 to 180
+ */
+int SoftServo::read()
+{
+    return map( this->readMicroseconds() + 1, this->min, this->max, 0, 180);
+}
+
+/*
+ * Return the current pulse time in microseconds
+ */
+int SoftServo::readMicroseconds()
+{
+    unsigned int pulsewidth;
+    if (this->attached())
+    {
+        pulsewidth = SoftPWMServoServoRead(this->pin);
+    }
+    else 
+    {
+        pulsewidth = 0;
+    }
+    return pulsewidth;
+}
+
+/*
+ * Check with SoftPWMServo to see if this pin is being used or not
+ */
+bool SoftServo::attached()
+{   
+    if (this->isAttached)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+

hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.h

@@ -42,24 +42,28 @@
       improvement
 */
 
-#ifndef SoftPWMServo_h
-#define SoftPWMServo_h
+#ifndef SOFT_PWM_SERVO_H
+#define SOFT_PWM_SERVO_H
 
 #ifndef WProgram_h
 	#include	"WProgram.h"
 #endif
 
 #include <inttypes.h>
 
-#define SOFTPWMSERVO_VERSION  1.2      // software version of this library
+#define SOFTPWMSERVO_VERSION   2       // software version of this library
 #define SOFTPWMSERVO_MAX_PINS (NUM_DIGITAL_PINS) // Max number of pins the library can handle (from variant Board_Defs.h)
 #define SOFTPWMSERVO_ERROR    (-1)     // Returned when a function fails
 #define SOFTPWMSERVO_OK       (0)      // Returned when a function passes
 #define SOFTPWMSERVO_SERVO    (1)      // Used to enable a pin for servo operation
 #define SOFTPWMSERVO_PWM      (0)      // Used to enable a pin for PWM operation
 
+#define MIN_PULSE_WIDTH       544      // the shortest pulse sent to a servo (us)
+#define MAX_PULSE_WIDTH       2400     // the longest pulse sent to a servo (us)
+#define DEFAULT_PULSE_WIDTH   1500     // default pulse width when servo is attached (us)
+
 // Used in determining the default FrameTime. How many frames per second do you want?
-#define DEFAULT_FRAMES_PER_SECOND           (500)
+#define DEFAULT_FRAMES_PER_SECOND           (400)           // 2.5ms per frame
 // Number of 40MHz CoreTimer ticks of the default frame time
 #define SOFTPWMSERVO_DEFAULT_FRAME_TIME     (F_CPU / 2 / DEFAULT_FRAMES_PER_SECOND)
 // How many CoreTimer ticks are there per microsecond
@@ -231,4 +235,34 @@ int8_t SoftPWMServoPWMRead(uint32_t Pin);
  *
  */
 int32_t SoftPWMServoSetServoFrames(uint32_t NewFrameCount);
+
+/*
+ * SoftServo class
+ * 
+ * This class acts as a wrapper to translate normal "Arduino Servo"
+ * calls into SoftPWMServo calls, so that sketches that use the
+ * standard "Arduino Servo" library can easily use SoftPWMServo instead
+ * so that no hardware timers are used and an unlimited number of I/O
+ * pins can be used for servo output.
+ */
+class SoftServo
+{
+public:
+  SoftServo();
+  uint8_t attach(int pin);           // Not really used here because we don't have channels
+  uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 
+  void detach();
+  void write(int value);             // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 
+  void writeMicroseconds(int value); // Write pulse width in microseconds 
+  int read();                        // returns current pulse width as an angle between 0 and 180 degrees
+  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 pin;                      // Arduino digital pin that this SoftServo is controlling
+   int32_t min;                      // minimum pulse width in microseconds  
+   int32_t max;                      // maximum pulse width in microseconds
+   bool isAttached;                  // Records the servo's "attached" status
+};
+
 #endif

hardware/pic32/variants/Fubarino_SD/Board_Defs.h

@@ -237,7 +237,7 @@ const static uint8_t SCK  = 102;		// PIC32 SCK2
 //#define	digitalPinToAnalog(P) ( (((P) > 15) && ((P) < 32)) ? (P)-16 : NOT_ANALOG_PIN )
 // This definition can be used for the non-default case where there
 // is a mapping table to go from digital pin to analog pin
-#define	digitalPinToAnalog(P) ( digital_pin_to_analog_PGM[P] )
+#define	digitalPinToAnalog(P) ( ((P) > 14 ) ? digital_pin_to_analog_PGM[P] : digital_pin_to_analog_PGM[(14 - P) + 30] )
 
 // This definition can be used for the default one-to-one mapping
 //#define analogInPinToChannel(P) ( P )