diff --git a/Marlin/Servo.cpp b/Marlin/Servo.cpp index 5f8c7efe3..27a7d3cf3 100644 --- a/Marlin/Servo.cpp +++ b/Marlin/Servo.cpp @@ -44,6 +44,7 @@ #include "Configuration.h" #ifdef NUM_SERVOS + #include #include @@ -52,7 +53,6 @@ #define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009 #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds - #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009 //#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER) @@ -74,24 +74,23 @@ uint8_t ServoCount = 0; // the total number /************ static functions common to all instances ***********************/ -static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) -{ - if( Channel[timer] < 0 ) +static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) { + if (Channel[timer] < 0) *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer - else{ - if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true ) + else { + if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive) digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated } Channel[timer]++; // increment to the next channel - if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { + if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks; - if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated + if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high } else { // finished all channels so wait for the refresh period to expire before starting over - if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed + if ( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); else *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed @@ -100,142 +99,126 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t } #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform -// Interrupt handlers for Arduino -#if defined(_useTimer1) -SIGNAL (TIMER1_COMPA_vect) -{ - handle_interrupts(_timer1, &TCNT1, &OCR1A); -} -#endif - -#if defined(_useTimer3) -SIGNAL (TIMER3_COMPA_vect) -{ - handle_interrupts(_timer3, &TCNT3, &OCR3A); -} -#endif -#if defined(_useTimer4) -SIGNAL (TIMER4_COMPA_vect) -{ - handle_interrupts(_timer4, &TCNT4, &OCR4A); + // Interrupt handlers for Arduino + #if defined(_useTimer1) + SIGNAL (TIMER1_COMPA_vect) { handle_interrupts(_timer1, &TCNT1, &OCR1A); } + #endif + + #if defined(_useTimer3) + SIGNAL (TIMER3_COMPA_vect) { handle_interrupts(_timer3, &TCNT3, &OCR3A); } + #endif + + #if defined(_useTimer4) + SIGNAL (TIMER4_COMPA_vect) { handle_interrupts(_timer4, &TCNT4, &OCR4A); } + #endif + + #if defined(_useTimer5) + SIGNAL (TIMER5_COMPA_vect) { handle_interrupts(_timer5, &TCNT5, &OCR5A); } + #endif + +#else //!WIRING + + // Interrupt handlers for Wiring + #if defined(_useTimer1) + void Timer1Service() { handle_interrupts(_timer1, &TCNT1, &OCR1A); } + #endif + #if defined(_useTimer3) + void Timer3Service() { handle_interrupts(_timer3, &TCNT3, &OCR3A); } + #endif + +#endif //!WIRING + + +static void initISR(timer16_Sequence_t timer) { + #if defined(_useTimer1) + if (timer == _timer1) { + TCCR1A = 0; // normal counting mode + TCCR1B = _BV(CS11); // set prescaler of 8 + TCNT1 = 0; // clear the timer count + #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__) + TIFR |= _BV(OCF1A); // clear any pending interrupts; + TIMSK |= _BV(OCIE1A); // enable the output compare interrupt + #else + // here if not ATmega8 or ATmega128 + TIFR1 |= _BV(OCF1A); // clear any pending interrupts; + TIMSK1 |= _BV(OCIE1A); // enable the output compare interrupt + #endif + #if defined(WIRING) + timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); + #endif + } + #endif + + #if defined(_useTimer3) + if (timer == _timer3) { + TCCR3A = 0; // normal counting mode + TCCR3B = _BV(CS31); // set prescaler of 8 + TCNT3 = 0; // clear the timer count + #if defined(__AVR_ATmega128__) + TIFR |= _BV(OCF3A); // clear any pending interrupts; + ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt + #else + TIFR3 = _BV(OCF3A); // clear any pending interrupts; + TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt + #endif + #if defined(WIRING) + timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only + #endif + } + #endif + + #if defined(_useTimer4) + if (timer == _timer4) { + TCCR4A = 0; // normal counting mode + TCCR4B = _BV(CS41); // set prescaler of 8 + TCNT4 = 0; // clear the timer count + TIFR4 = _BV(OCF4A); // clear any pending interrupts; + TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt + } + #endif + + #if defined(_useTimer5) + if (timer == _timer5) { + TCCR5A = 0; // normal counting mode + TCCR5B = _BV(CS51); // set prescaler of 8 + TCNT5 = 0; // clear the timer count + TIFR5 = _BV(OCF5A); // clear any pending interrupts; + TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt + } + #endif } -#endif -#if defined(_useTimer5) -SIGNAL (TIMER5_COMPA_vect) -{ - handle_interrupts(_timer5, &TCNT5, &OCR5A); +static void finISR(timer16_Sequence_t timer) { + // Disable use of the given timer + #if defined(WIRING) + if (timer == _timer1) { + #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) + TIMSK1 + #else + TIMSK + #endif + &= ~_BV(OCIE1A); // disable timer 1 output compare interrupt + timerDetach(TIMER1OUTCOMPAREA_INT); + } + else if (timer == _timer3) { + #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) + TIMSK3 + #else + ETIMSK + #endif + &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt + timerDetach(TIMER3OUTCOMPAREA_INT); + } + #else //!WIRING + // For arduino - in future: call here to a currently undefined function to reset the timer + #endif } -#endif -#elif defined WIRING -// Interrupt handlers for Wiring -#if defined(_useTimer1) -void Timer1Service() -{ - handle_interrupts(_timer1, &TCNT1, &OCR1A); -} -#endif -#if defined(_useTimer3) -void Timer3Service() -{ - handle_interrupts(_timer3, &TCNT3, &OCR3A); -} -#endif -#endif - - -static void initISR(timer16_Sequence_t timer) -{ -#if defined (_useTimer1) - if(timer == _timer1) { - TCCR1A = 0; // normal counting mode - TCCR1B = _BV(CS11); // set prescaler of 8 - TCNT1 = 0; // clear the timer count -#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__) - TIFR |= _BV(OCF1A); // clear any pending interrupts; - TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt -#else - // here if not ATmega8 or ATmega128 - TIFR1 |= _BV(OCF1A); // clear any pending interrupts; - TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt -#endif -#if defined(WIRING) - timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); -#endif - } -#endif - -#if defined (_useTimer3) - if(timer == _timer3) { - TCCR3A = 0; // normal counting mode - TCCR3B = _BV(CS31); // set prescaler of 8 - TCNT3 = 0; // clear the timer count -#if defined(__AVR_ATmega128__) - TIFR |= _BV(OCF3A); // clear any pending interrupts; - ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt -#else - TIFR3 = _BV(OCF3A); // clear any pending interrupts; - TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt -#endif -#if defined(WIRING) - timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only -#endif - } -#endif - -#if defined (_useTimer4) - if(timer == _timer4) { - TCCR4A = 0; // normal counting mode - TCCR4B = _BV(CS41); // set prescaler of 8 - TCNT4 = 0; // clear the timer count - TIFR4 = _BV(OCF4A); // clear any pending interrupts; - TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt - } -#endif - -#if defined (_useTimer5) - if(timer == _timer5) { - TCCR5A = 0; // normal counting mode - TCCR5B = _BV(CS51); // set prescaler of 8 - TCNT5 = 0; // clear the timer count - TIFR5 = _BV(OCF5A); // clear any pending interrupts; - TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt - } -#endif -} - -static void finISR(timer16_Sequence_t timer) -{ - //disable use of the given timer -#if defined WIRING // Wiring - if(timer == _timer1) { - #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) - TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt - #else - TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt - #endif - timerDetach(TIMER1OUTCOMPAREA_INT); - } - else if(timer == _timer3) { - #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__) - TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt - #else - ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt - #endif - timerDetach(TIMER3OUTCOMPAREA_INT); - } -#else - //For arduino - in future: call here to a currently undefined function to reset the timer -#endif -} - -static boolean isTimerActive(timer16_Sequence_t timer) -{ +static boolean isTimerActive(timer16_Sequence_t timer) { // returns true if any servo is active on this timer for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) { - if(SERVO(timer,channel).Pin.isActive == true) + if (SERVO(timer,channel).Pin.isActive) return true; } return false; @@ -244,70 +227,59 @@ static boolean isTimerActive(timer16_Sequence_t timer) /****************** end of static functions ******************************/ -Servo::Servo() -{ - if( ServoCount < MAX_SERVOS) { +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 - 12 Aug 2009 + servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009 } else - this->servoIndex = INVALID_SERVO ; // too many servos + this->servoIndex = INVALID_SERVO; // too many servos } -uint8_t Servo::attach(int pin) -{ +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) -{ - if(this->servoIndex < MAX_SERVOS ) { -#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) +uint8_t Servo::attach(int pin, int min, int max) { + if (this->servoIndex < MAX_SERVOS ) { + #if defined(ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) if (pin > 0) this->pin = pin; else pin = this->pin; -#endif - pinMode( pin, OUTPUT) ; // set servo pin to output + #endif + pinMode(pin, OUTPUT); // set servo pin to output servos[this->servoIndex].Pin.nbr = pin; // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 - this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS - this->max = (MAX_PULSE_WIDTH - max)/4; + this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS + this->max = (MAX_PULSE_WIDTH - max) / 4; // initialize the timer if it has not already been initialized timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if(isTimerActive(timer) == false) - initISR(timer); + if (!isTimerActive(timer)) initISR(timer); servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive } - return this->servoIndex ; + return this->servoIndex; } -void Servo::detach() -{ +void Servo::detach() { servos[this->servoIndex].Pin.isActive = false; timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if(isTimerActive(timer) == false) { - finISR(timer); - } + if (!isTimerActive(timer)) finISR(timer); } -void Servo::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 > 180) value = 180; +void Servo::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 > 180) value = 180; value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX()); } this->writeMicroseconds(value); } -void Servo::writeMicroseconds(int value) -{ +void Servo::writeMicroseconds(int value) { // 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 + 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() ) + else if (value > SERVO_MAX()) value = SERVO_MAX(); value = value - TRIM_DURATION; @@ -320,25 +292,13 @@ void Servo::writeMicroseconds(int value) } } -int Servo::read() // return the value as degrees -{ - return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); -} +// return the value as degrees +int Servo::read() { return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); } -int Servo::readMicroseconds() -{ - unsigned int pulsewidth; - if( this->servoIndex != INVALID_SERVO ) - pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009 - else - pulsewidth = 0; - - return pulsewidth; +int Servo::readMicroseconds() { + return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION; } -bool Servo::attached() -{ - return servos[this->servoIndex].Pin.isActive ; -} +bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; } #endif diff --git a/Marlin/Servo.h b/Marlin/Servo.h index 204497a4a..bbdf6bf0a 100644 --- a/Marlin/Servo.h +++ b/Marlin/Servo.h @@ -58,35 +58,36 @@ // Say which 16 bit timers can be used and in what order #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) -#define _useTimer5 -//#define _useTimer1 -#define _useTimer3 -#define _useTimer4 -//typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; -typedef enum { _timer5, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; + #define _useTimer5 + //#define _useTimer1 + #define _useTimer3 + #define _useTimer4 + //typedef enum { _timer5, _timer1, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; + typedef enum { _timer5, _timer3, _timer4, _Nbr_16timers } timer16_Sequence_t ; #elif defined(__AVR_ATmega32U4__) -//#define _useTimer1 -#define _useTimer3 -//typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; -typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; + //#define _useTimer1 + #define _useTimer3 + //typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; + typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; #elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) -#define _useTimer3 -//#define _useTimer1 -//typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; -typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; + #define _useTimer3 + //#define _useTimer1 + //typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; + typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; #elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) ||defined(__AVR_ATmega2561__) -#define _useTimer3 -//#define _useTimer1 -//typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; -typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; + #define _useTimer3 + //#define _useTimer1 + //typedef enum { _timer3, _timer1, _Nbr_16timers } timer16_Sequence_t ; + typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t ; #else // everything else -//#define _useTimer1 -//typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; -typedef enum { _Nbr_16timers } timer16_Sequence_t ; + //#define _useTimer1 + //typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t ; + typedef enum { _Nbr_16timers } timer16_Sequence_t ; + #endif #define Servo_VERSION 2 // software version of this library @@ -101,35 +102,34 @@ typedef enum { _Nbr_16timers } timer16_Sequence_t ; #define INVALID_SERVO 255 // flag indicating an invalid servo index -typedef struct { +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 -} ServoPin_t ; +} ServoPin_t; typedef struct { ServoPin_t Pin; unsigned int ticks; } servo_t; -class Servo -{ -public: - Servo(); - uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure - 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 it is 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 -#if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) - int pin; // store the hardware pin of the servo -#endif -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 +class Servo { + public: + Servo(); + uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure + 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 it is 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 + #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0) + int pin; // store the hardware pin of the servo + #endif + 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 }; #endif