My Marlin configs for Fabrikator Mini and CTC i3 Pro B
您最多选择25个主题 主题必须以字母或数字开头,可以包含连字符 (-),并且长度不得超过35个字符

servo.cpp 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304
  1. /*
  2. servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2
  3. Copyright (c) 2009 Michael Margolis. All right reserved.
  4. This library is free software; you can redistribute it and/or
  5. modify it under the terms of the GNU Lesser General Public
  6. License as published by the Free Software Foundation; either
  7. version 2.1 of the License, or (at your option) any later version.
  8. This library is distributed in the hope that it will be useful,
  9. but WITHOUT ANY WARRANTY; without even the implied warranty of
  10. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  11. Lesser General Public License for more details.
  12. You should have received a copy of the GNU Lesser General Public
  13. License along with this library; if not, write to the Free Software
  14. Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  15. */
  16. /*
  17. A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
  18. The servos are pulsed in the background using the value most recently written using the write() method
  19. Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached.
  20. Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four.
  21. The methods are:
  22. Servo - Class for manipulating servo motors connected to Arduino pins.
  23. attach(pin ) - Attaches a servo motor to an i/o pin.
  24. attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds
  25. default min is 544, max is 2400
  26. write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds)
  27. writeMicroseconds() - Sets the servo pulse width in microseconds
  28. read() - Gets the last written servo pulse width as an angle between 0 and 180.
  29. readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
  30. attached() - Returns true if there is a servo attached.
  31. detach() - Stops an attached servos from pulsing its i/o pin.
  32. */
  33. #include "Configuration.h"
  34. #ifdef NUM_SERVOS
  35. #include <avr/interrupt.h>
  36. #include <Arduino.h>
  37. #include "servo.h"
  38. #define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
  39. #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
  40. #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
  41. //#define NBR_TIMERS (MAX_SERVOS / SERVOS_PER_TIMER)
  42. static servo_t servos[MAX_SERVOS]; // static array of servo structures
  43. static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
  44. uint8_t ServoCount = 0; // the total number of attached servos
  45. // convenience macros
  46. #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
  47. #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
  48. #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel
  49. #define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
  50. #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
  51. #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
  52. /************ static functions common to all instances ***********************/
  53. static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA) {
  54. if (Channel[timer] < 0)
  55. *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
  56. else {
  57. if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive)
  58. digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
  59. }
  60. Channel[timer]++; // increment to the next channel
  61. if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
  62. *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
  63. if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated
  64. digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
  65. }
  66. else {
  67. // finished all channels so wait for the refresh period to expire before starting over
  68. if ( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
  69. *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
  70. else
  71. *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
  72. Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
  73. }
  74. }
  75. #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
  76. // Interrupt handlers for Arduino
  77. #if defined(_useTimer1)
  78. SIGNAL (TIMER1_COMPA_vect) { handle_interrupts(_timer1, &TCNT1, &OCR1A); }
  79. #endif
  80. #if defined(_useTimer3)
  81. SIGNAL (TIMER3_COMPA_vect) { handle_interrupts(_timer3, &TCNT3, &OCR3A); }
  82. #endif
  83. #if defined(_useTimer4)
  84. SIGNAL (TIMER4_COMPA_vect) { handle_interrupts(_timer4, &TCNT4, &OCR4A); }
  85. #endif
  86. #if defined(_useTimer5)
  87. SIGNAL (TIMER5_COMPA_vect) { handle_interrupts(_timer5, &TCNT5, &OCR5A); }
  88. #endif
  89. #else //!WIRING
  90. // Interrupt handlers for Wiring
  91. #if defined(_useTimer1)
  92. void Timer1Service() { handle_interrupts(_timer1, &TCNT1, &OCR1A); }
  93. #endif
  94. #if defined(_useTimer3)
  95. void Timer3Service() { handle_interrupts(_timer3, &TCNT3, &OCR3A); }
  96. #endif
  97. #endif //!WIRING
  98. static void initISR(timer16_Sequence_t timer) {
  99. #if defined(_useTimer1)
  100. if (timer == _timer1) {
  101. TCCR1A = 0; // normal counting mode
  102. TCCR1B = _BV(CS11); // set prescaler of 8
  103. TCNT1 = 0; // clear the timer count
  104. #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
  105. TIFR |= _BV(OCF1A); // clear any pending interrupts;
  106. TIMSK |= _BV(OCIE1A); // enable the output compare interrupt
  107. #else
  108. // here if not ATmega8 or ATmega128
  109. TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
  110. TIMSK1 |= _BV(OCIE1A); // enable the output compare interrupt
  111. #endif
  112. #if defined(WIRING)
  113. timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
  114. #endif
  115. }
  116. #endif
  117. #if defined(_useTimer3)
  118. if (timer == _timer3) {
  119. TCCR3A = 0; // normal counting mode
  120. TCCR3B = _BV(CS31); // set prescaler of 8
  121. TCNT3 = 0; // clear the timer count
  122. #if defined(__AVR_ATmega128__)
  123. TIFR |= _BV(OCF3A); // clear any pending interrupts;
  124. ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
  125. #else
  126. TIFR3 = _BV(OCF3A); // clear any pending interrupts;
  127. TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
  128. #endif
  129. #if defined(WIRING)
  130. timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
  131. #endif
  132. }
  133. #endif
  134. #if defined(_useTimer4)
  135. if (timer == _timer4) {
  136. TCCR4A = 0; // normal counting mode
  137. TCCR4B = _BV(CS41); // set prescaler of 8
  138. TCNT4 = 0; // clear the timer count
  139. TIFR4 = _BV(OCF4A); // clear any pending interrupts;
  140. TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
  141. }
  142. #endif
  143. #if defined(_useTimer5)
  144. if (timer == _timer5) {
  145. TCCR5A = 0; // normal counting mode
  146. TCCR5B = _BV(CS51); // set prescaler of 8
  147. TCNT5 = 0; // clear the timer count
  148. TIFR5 = _BV(OCF5A); // clear any pending interrupts;
  149. TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
  150. }
  151. #endif
  152. }
  153. static void finISR(timer16_Sequence_t timer) {
  154. // Disable use of the given timer
  155. #if defined(WIRING)
  156. if (timer == _timer1) {
  157. #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
  158. TIMSK1
  159. #else
  160. TIMSK
  161. #endif
  162. &= ~_BV(OCIE1A); // disable timer 1 output compare interrupt
  163. timerDetach(TIMER1OUTCOMPAREA_INT);
  164. }
  165. else if (timer == _timer3) {
  166. #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
  167. TIMSK3
  168. #else
  169. ETIMSK
  170. #endif
  171. &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
  172. timerDetach(TIMER3OUTCOMPAREA_INT);
  173. }
  174. #else //!WIRING
  175. // For arduino - in future: call here to a currently undefined function to reset the timer
  176. #endif
  177. }
  178. static boolean isTimerActive(timer16_Sequence_t timer) {
  179. // returns true if any servo is active on this timer
  180. for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
  181. if (SERVO(timer,channel).Pin.isActive)
  182. return true;
  183. }
  184. return false;
  185. }
  186. /****************** end of static functions ******************************/
  187. Servo::Servo() {
  188. if ( ServoCount < MAX_SERVOS) {
  189. this->servoIndex = ServoCount++; // assign a servo index to this instance
  190. servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
  191. }
  192. else
  193. this->servoIndex = INVALID_SERVO; // too many servos
  194. }
  195. uint8_t Servo::attach(int pin) {
  196. return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  197. }
  198. uint8_t Servo::attach(int pin, int min, int max) {
  199. if (this->servoIndex < MAX_SERVOS ) {
  200. #if defined(ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  201. if (pin > 0) this->pin = pin; else pin = this->pin;
  202. #endif
  203. pinMode(pin, OUTPUT); // set servo pin to output
  204. servos[this->servoIndex].Pin.nbr = pin;
  205. // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
  206. this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS
  207. this->max = (MAX_PULSE_WIDTH - max) / 4;
  208. // initialize the timer if it has not already been initialized
  209. timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
  210. if (!isTimerActive(timer)) initISR(timer);
  211. servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
  212. }
  213. return this->servoIndex;
  214. }
  215. void Servo::detach() {
  216. servos[this->servoIndex].Pin.isActive = false;
  217. timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
  218. if (!isTimerActive(timer)) finISR(timer);
  219. }
  220. void Servo::write(int value) {
  221. if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
  222. if (value < 0) value = 0;
  223. if (value > 180) value = 180;
  224. value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
  225. }
  226. this->writeMicroseconds(value);
  227. }
  228. void Servo::writeMicroseconds(int value) {
  229. // calculate and store the values for the given channel
  230. byte channel = this->servoIndex;
  231. if (channel < MAX_SERVOS) { // ensure channel is valid
  232. if (value < SERVO_MIN()) // ensure pulse width is valid
  233. value = SERVO_MIN();
  234. else if (value > SERVO_MAX())
  235. value = SERVO_MAX();
  236. value = value - TRIM_DURATION;
  237. value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
  238. uint8_t oldSREG = SREG;
  239. cli();
  240. servos[channel].ticks = value;
  241. SREG = oldSREG;
  242. }
  243. }
  244. // return the value as degrees
  245. int Servo::read() { return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
  246. int Servo::readMicroseconds() {
  247. return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
  248. }
  249. bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
  250. #endif