From 6f3c24815de035a730ef2b4f773dc65e73ed50c9 Mon Sep 17 00:00:00 2001 From: Brian Schmalz Date: Tue, 18 Sep 2012 21:44:57 -0500 Subject: [PATCH 1/2] Fixed math problem with Servo library that was incorrectly computing the time between the last falling edge and the first rising edge of the next cycle. Also tweaked timing so that pulses come out more accurately now. Also changed code to support up to pin 127 (since MAX32 has 83 IOs, we needed to go above 63 which was the previous limit.) --- hardware/pic32/libraries/Servo/Servo.cpp | 94 +++++++++++++----------- hardware/pic32/libraries/Servo/Servo.h | 79 ++++++++++---------- 2 files changed, 92 insertions(+), 81 deletions(-) diff --git a/hardware/pic32/libraries/Servo/Servo.cpp b/hardware/pic32/libraries/Servo/Servo.cpp index 5eed8a487..fa9ff80c5 100644 --- a/hardware/pic32/libraries/Servo/Servo.cpp +++ b/hardware/pic32/libraries/Servo/Servo.cpp @@ -1,6 +1,6 @@ //************************************************************************ /* - Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + Servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers Copyright (c) 2009 Michael Margolis. All right reserved. Revision date: 08/18/2011(Michelle Yu) @@ -20,13 +20,19 @@ */ //************************************************************************ -/* - +/* A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. The servos are pulsed in the background using the value most recently written using the write() method Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached. - Timers are seized as needed in groups of 8 servos - 16 servos use two timers, 32 servos will use four. + Timers are seized as needed in groups of 8 servos - 16 servos use two timers, 24 servos will use three. + + For the PIC32, the three timers that are used are (in order): + Timer4 (for 1 through 8 servos) + Timer5 (for 9 through 16 servos) + Timer3 (for 17 through 24 servos) + + Be careful that other libraries do not use any of the timers that you need for your servos. The methods are: @@ -53,6 +59,9 @@ //* Sep 1, 2011 issue #112, changed assigment to compare in finISR //* Sep 5, 2011 added include of plib.h to fix compile errors //* introduced when plib.h was removed from HardwareSerial.h +//* Sep 18, 2012 Fixed math problem with refresh interval, and +//* expanded nbr to 7 bits for pins above 63 (MAX32 has 83) +//* And adjusted trim to 1 tick (measured and adjusted) //************************************************************************ #include @@ -65,16 +74,13 @@ extern "C"{ #define usToTicks(_us) (((_us)*5)/4) // converts microseconds to tick #define ticksToUs(_ticks) ((((unsigned)(_ticks))*4)/5) // converts from ticks back to microseconds - -#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 200 - +#define TRIM_DURATION 1 // compensation ticks to trim adjust for digitalWrite delays // 12 August 200 static servo_t servos[MAX_SERVOS]; // static array of servo structures int channel[3]; // channel for the current servo uint8_t ServoCount = 0; // the total number of attached servos - #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel @@ -83,50 +89,60 @@ uint8_t ServoCount = 0; // the total number of attached servos /************ static functions common to all instances ***********************/ //************************************************************************ -void handle_interrupts(int timer, volatile unsigned int *TMRn, volatile unsigned int *PR) +// Note: PIC32 timers (TMRn) reset to zero when they match PRn. +void handle_interrupts(int timer, volatile unsigned int *TMRn, volatile unsigned int *PRn) { - - + static uint32_t AccumulatedTicks[3] = {0,0,0}; // Store the number of ticks since the first rising edge for this timer + + // Test for invalid timer number + if (timer >= 3) + { + return; + } + + // If this value is -1, then we have just finished with the time from the end of the last pulse + // and need to start with the first servo index on this timer again. if ( channel[timer] < 0 ) { - *TMRn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer + AccumulatedTicks[timer] = 0; // Clear the accumulated time for this timer on first rising edge } else { + // If this is not the first pulse, then set the old pin low if ( SERVO_INDEX(timer,channel[timer]) < ServoCount && SERVO(timer,channel[timer]).Pin.isActive == true ) { - digitalWrite( SERVO(timer,channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated + digitalWrite( SERVO(timer,channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated } } channel[timer]++; // increment to the next channel + // If we have not run out of channels (on this timer), if ( SERVO_INDEX(timer,channel[timer]) < ServoCount && channel[timer] < SERVOS_PER_TIMER) { - *PR = *TMRn + SERVO(timer,channel[timer]).ticks; + // Then set the time we want to fire next (the width for this channel) + *PRn = SERVO(timer,channel[timer]).ticks; + // Set this channel's pin high if its active if (SERVO(timer,channel[timer]).Pin.isActive == true) // check if activated { - digitalWrite( SERVO(timer,channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high + digitalWrite( SERVO(timer,channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high } + AccumulatedTicks[timer] += *PRn; // Add the time we are about to spend on this channel } else { - *TMRn = channel[timer] * usToTicks(MAX_PULSE_WIDTH); - // finished all channels so wait for the refresh period to expire before starting over - if ( (unsigned)*TMRn < (usToTicks(REFRESH_INTERVAL) + 4) ) // allow a few ticks to ensure the next OCR1A not missed + // Otherwise, finished all channels so next fire needs to be at REFRESH_INTERVAL - AccumulatedTicks + if (AccumulatedTicks[timer] < (usToTicks(REFRESH_INTERVAL) - 4)) { - *PR = (unsigned int)usToTicks(REFRESH_INTERVAL); + *PRn = (unsigned int)(usToTicks(REFRESH_INTERVAL)) - AccumulatedTicks[timer]; } else { - *PR = *TMRn + 4; // at least REFRESH_INTERVAL has elapsed + *PRn = *TMRn + 4; // at least REFRESH_INTERVAL has elapsed } channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel } } - - - //************************************************************************ static void finISR(int timer) { @@ -157,21 +173,24 @@ static boolean isTimerActive(int timer) return false; } - /****************** end of static functions ******************************/ Servo::Servo() { - if ( ServoCount < MAX_SERVOS) + if (ServoCount < MAX_SERVOS) { + /// TODO: Really, we need to search through servos[] for the first one + /// that is not being used, rather than always use ServoCount as the + /// index. What happens if you create 24 servos, then destroy one + /// and create another. Right now that would fail, even though you + /// weren't actively using 24 servos. this->servoIndex = ServoCount++; // assign a servo index to this instance servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default to neutral position } else - this->servoIndex = INVALID_SERVO ; // too many servos + this->servoIndex = INVALID_SERVO ; // too many servos } - //************************************************************************ uint8_t Servo::attach(int pin) { @@ -181,16 +200,9 @@ uint8_t Servo::attach(int pin) //************************************************************************ uint8_t Servo::attach(int pin, int min, int max) { - if (this->servoIndex < MAX_SERVOS ) + if (this->servoIndex < ServoCount) { - if (pin == 0 | pin == 1) - { - // disable UART - U1MODE = 0x0; - - } - - pinMode( pin, OUTPUT) ; // set servo pin to output + pinMode(pin, OUTPUT); // set servo pin to output servos[this->servoIndex].Pin.nbr = pin; this->min = (MIN_PULSE_WIDTH - min)/4; // resolution of min/max is 4 uS this->max = (MAX_PULSE_WIDTH - max)/4; @@ -204,10 +216,13 @@ uint8_t Servo::attach(int pin, int min, int max) return this->servoIndex; } + else + { + // return bogus value if this->ServoIndex is invalid + return(INVALID_SERVO); + } } - - //************************************************************************ void Servo::detach() { @@ -248,12 +263,9 @@ void Servo::writeMicroseconds(int value) value = value - TRIM_DURATION; value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - - unsigned int status; status = INTDisableInterrupts(); servos[channel].ticks = value; - INTRestoreInterrupts(status); } } diff --git a/hardware/pic32/libraries/Servo/Servo.h b/hardware/pic32/libraries/Servo/Servo.h index 86452e73a..aaf9546ba 100755 --- a/hardware/pic32/libraries/Servo/Servo.h +++ b/hardware/pic32/libraries/Servo/Servo.h @@ -1,25 +1,24 @@ -/* - Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 - Copyright (c) 2009 Michael Margolis. All right reserved. - Revision date: 08/18/2011(Michelle Yu) - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +/* + Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers + Copyright (c) 2009 Michael Margolis. All right reserved. + Revision date: 08/18/2011(Michelle Yu) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ /* - A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. The servos are pulsed in the background using the value most recently written using the write() method @@ -41,8 +40,24 @@ readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) attached() - Returns true if there is a servo attached. detach() - Stops an attached servos from pulsing its i/o pin. + + This library has been modified to support the PIC32 architecture. + + NOTE: On 09/18/2012 the code to disable a UART on pins 0 and 1 when + assigning a servo to those pins was removed, since not all boards have + UARTs on pins 0 and 1. You must be careful to disable any other function + on a given pin before you start using a servo output on that pin. For + example, if you have the standard UART on pins 1 and 0 and you want to use + those pins for servo output, you would need to disable the UART first. + Example: + Serial.end(); + Servo.attach(0); + + Version history: + See Servo.cpp */ + #ifndef Servo_h #define Servo_h @@ -53,33 +68,17 @@ #include -/* - * Defines for 16 bit timers used with Servo library - * - * If _useTimerX is defined then TimerX is a 16 bit timer on the curent board - * timer16_Sequence_t enumerates the sequence that the timers should be allocated - * _Nbr_16timers indicates how many 16 bit timers are available. - * - */ - -// Say which 16 bit timers can be used and in what order - - -#define Servo_VERSION 3 // software version of this library - -#define SERVOS_PER_TIMER 8 // the maximum number of servos controlled by one timer -#define MAX_SERVOS 24 - - -#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo (us) +#define Servo_VERSION 3 // software version of this library +#define SERVOS_PER_TIMER 8 // the maximum number of servos controlled by one timer +#define MAX_SERVOS 24 // Maximum number of servos this library can support at once +#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) #define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds - #define INVALID_SERVO 255 // flag indicating an invalid servo index typedef struct { - uint8_t nbr :6 ; // a pin number from 0 to 63 + uint8_t nbr :7 ; // a pin number from 0 to 127 uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false } ServoPin_t ; From 94612a251641388b746c7dcd94c07e8ec1f865be Mon Sep 17 00:00:00 2001 From: Brian Schmalz Date: Thu, 20 Sep 2012 22:02:13 -0500 Subject: [PATCH 2/2] Added readme.mb file for Servo library. --- hardware/pic32/libraries/Servo/readme.mb | 41 ++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 hardware/pic32/libraries/Servo/readme.mb diff --git a/hardware/pic32/libraries/Servo/readme.mb b/hardware/pic32/libraries/Servo/readme.mb new file mode 100644 index 000000000..306fd3220 --- /dev/null +++ b/hardware/pic32/libraries/Servo/readme.mb @@ -0,0 +1,41 @@ +# readme.mb : Readme file for chipKIT/MPIDE Servo library + +## Overview: + +The Servo library allows a sketch to output RC servo pulses on up to 24 of the I/O pins +at a time. These servo pulses are all controlled by hardware timers and interrupts, so that +once they are started, they continue to output pulses with no sketch code interaction. + +Use this library to control up to 24 RC servos at a time, using either degree (0 to 179) +parameter or microsecond pulse durations (from 544us to 2400us). + +## Usage: + +See the Servo.cpp and Servo.h files for detailed usage information. + +## Operation: + +The way the library works is to keep a list of all possible servos. Each time you create +a new Servo object and attach it to a pin, the next servo in the list is populated with +the pin number, the duration (in uS) and which of three timers will control that servo. + +The three timers are dynamically turned on/off as needed to support the number of servos +used, with up to 8 servos on each of the three timers. Each time the timer fires, it +clears the current servo pin and set the next one in the list, then sets itself to fire +again in the future the duration of the new servo high period. After all servos have fired, +the timer then sets itself to fire such that the entire process repeats every 20ms or so. + +Even though timers are used, the actual pin setting and clearing is done in software in +the timer's ISR. Thus these servos will have jitter based upon what other interrupts +are enabled and running on the system at the time. + +## Notes: + +See Servo.cpp and Servo.h for more information on special precautions when using the +library. + +## History: + +This library was adatped for the PIC32 architecture from the Arduino Servo library. See +Servo.cpp for more history information +