From a5e2e93b65c29216d96a9dee5ecaf691edd4fe55 Mon Sep 17 00:00:00 2001 From: Brian Schmalz Date: Fri, 26 Jun 2015 06:46:30 -0500 Subject: [PATCH] Fix for issue #575 : Adding "servo" wrapper class to SoftPMWServo * 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. --- .../libraries/SoftPWMServo/SoftPWMServo.cpp | 150 ++++++++++++++++-- .../libraries/SoftPWMServo/SoftPWMServo.h | 42 ++++- .../pic32/variants/Fubarino_SD/Board_Defs.h | 2 +- 3 files changed, 179 insertions(+), 15 deletions(-) diff --git a/hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.cpp b/hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.cpp index 227457b59..2fbd7a123 100755 --- a/hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.cpp +++ b/hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.cpp @@ -37,6 +37,7 @@ * Fixed bug that caused glitches every 107 seconds. 06/19/2015 : * 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; + } +} + diff --git a/hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.h b/hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.h index e444413f1..da2b7f909 100755 --- a/hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.h +++ b/hardware/pic32/libraries/SoftPWMServo/SoftPWMServo.h @@ -42,8 +42,8 @@ improvement */ -#ifndef SoftPWMServo_h -#define SoftPWMServo_h +#ifndef SOFT_PWM_SERVO_H +#define SOFT_PWM_SERVO_H #ifndef WProgram_h #include "WProgram.h" @@ -51,15 +51,19 @@ #include -#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 \ No newline at end of file diff --git a/hardware/pic32/variants/Fubarino_SD/Board_Defs.h b/hardware/pic32/variants/Fubarino_SD/Board_Defs.h index 43e3ed465..55fc419ce 100644 --- a/hardware/pic32/variants/Fubarino_SD/Board_Defs.h +++ b/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 )