My Marlin configs for Fabrikator Mini and CTC i3 Pro B
選択できるのは25トピックまでです。 トピックは、先頭が英数字で、英数字とダッシュ('-')を使用した35文字以内のものにしてください。

servo.cpp 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312
  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. move(pin, angle) - Sequence of attach(pin), write(angle).
  29. With DEACTIVATE_SERVOS_AFTER_MOVE it waits SERVO_DEACTIVATION_DELAY and detaches.
  30. read() - Gets the last written servo pulse width as an angle between 0 and 180.
  31. readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
  32. attached() - Returns true if there is a servo attached.
  33. detach() - Stops an attached servos from pulsing its i/o pin.
  34. */
  35. #include "Configuration.h"
  36. #if HAS_SERVOS
  37. #include <avr/interrupt.h>
  38. #include <Arduino.h>
  39. #include "servo.h"
  40. #define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009
  41. #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
  42. #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
  43. //#define NBR_TIMERS ((MAX_SERVOS) / (SERVOS_PER_TIMER))
  44. static ServoInfo_t servo_info[MAX_SERVOS]; // static array of servo info structures
  45. static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
  46. uint8_t ServoCount = 0; // the total number of attached servos
  47. // convenience macros
  48. #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo
  49. #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // returns the index of the servo on this timer
  50. #define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // macro to access servo index by timer and channel
  51. #define SERVO(_timer,_channel) (servo_info[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
  52. #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo
  53. #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo
  54. /************ static functions common to all instances ***********************/
  55. static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
  56. if (Channel[timer] < 0)
  57. *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
  58. else {
  59. if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
  60. digitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
  61. }
  62. Channel[timer]++; // increment to the next channel
  63. if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
  64. *OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks;
  65. if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated
  66. digitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high
  67. }
  68. else {
  69. // finished all channels so wait for the refresh period to expire before starting over
  70. if (((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
  71. *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
  72. else
  73. *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
  74. Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
  75. }
  76. }
  77. #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
  78. // Interrupt handlers for Arduino
  79. #if ENABLED(_useTimer1)
  80. SIGNAL (TIMER1_COMPA_vect) { handle_interrupts(_timer1, &TCNT1, &OCR1A); }
  81. #endif
  82. #if ENABLED(_useTimer3)
  83. SIGNAL (TIMER3_COMPA_vect) { handle_interrupts(_timer3, &TCNT3, &OCR3A); }
  84. #endif
  85. #if ENABLED(_useTimer4)
  86. SIGNAL (TIMER4_COMPA_vect) { handle_interrupts(_timer4, &TCNT4, &OCR4A); }
  87. #endif
  88. #if ENABLED(_useTimer5)
  89. SIGNAL (TIMER5_COMPA_vect) { handle_interrupts(_timer5, &TCNT5, &OCR5A); }
  90. #endif
  91. #else //!WIRING
  92. // Interrupt handlers for Wiring
  93. #if ENABLED(_useTimer1)
  94. void Timer1Service() { handle_interrupts(_timer1, &TCNT1, &OCR1A); }
  95. #endif
  96. #if ENABLED(_useTimer3)
  97. void Timer3Service() { handle_interrupts(_timer3, &TCNT3, &OCR3A); }
  98. #endif
  99. #endif //!WIRING
  100. static void initISR(timer16_Sequence_t timer) {
  101. #if ENABLED(_useTimer1)
  102. if (timer == _timer1) {
  103. TCCR1A = 0; // normal counting mode
  104. TCCR1B = _BV(CS11); // set prescaler of 8
  105. TCNT1 = 0; // clear the timer count
  106. #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
  107. SBI(TIFR, OCF1A); // clear any pending interrupts;
  108. SBI(TIMSK, OCIE1A); // enable the output compare interrupt
  109. #else
  110. // here if not ATmega8 or ATmega128
  111. SBI(TIFR1, OCF1A); // clear any pending interrupts;
  112. SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
  113. #endif
  114. #ifdef WIRING
  115. timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
  116. #endif
  117. }
  118. #endif
  119. #if ENABLED(_useTimer3)
  120. if (timer == _timer3) {
  121. TCCR3A = 0; // normal counting mode
  122. TCCR3B = _BV(CS31); // set prescaler of 8
  123. TCNT3 = 0; // clear the timer count
  124. #ifdef __AVR_ATmega128__
  125. SBI(TIFR, OCF3A); // clear any pending interrupts;
  126. SBI(ETIMSK, OCIE3A); // enable the output compare interrupt
  127. #else
  128. TIFR3 = _BV(OCF3A); // clear any pending interrupts;
  129. TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
  130. #endif
  131. #ifdef WIRING
  132. timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
  133. #endif
  134. }
  135. #endif
  136. #if ENABLED(_useTimer4)
  137. if (timer == _timer4) {
  138. TCCR4A = 0; // normal counting mode
  139. TCCR4B = _BV(CS41); // set prescaler of 8
  140. TCNT4 = 0; // clear the timer count
  141. TIFR4 = _BV(OCF4A); // clear any pending interrupts;
  142. TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
  143. }
  144. #endif
  145. #if ENABLED(_useTimer5)
  146. if (timer == _timer5) {
  147. TCCR5A = 0; // normal counting mode
  148. TCCR5B = _BV(CS51); // set prescaler of 8
  149. TCNT5 = 0; // clear the timer count
  150. TIFR5 = _BV(OCF5A); // clear any pending interrupts;
  151. TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
  152. }
  153. #endif
  154. }
  155. static void finISR(timer16_Sequence_t timer) {
  156. // Disable use of the given timer
  157. #ifdef WIRING
  158. if (timer == _timer1) {
  159. CBI(
  160. #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
  161. TIMSK1
  162. #else
  163. TIMSK
  164. #endif
  165. , OCIE1A); // disable timer 1 output compare interrupt
  166. timerDetach(TIMER1OUTCOMPAREA_INT);
  167. }
  168. else if (timer == _timer3) {
  169. CBI(
  170. #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
  171. TIMSK3
  172. #else
  173. ETIMSK
  174. #endif
  175. , OCIE3A); // disable the timer3 output compare A interrupt
  176. timerDetach(TIMER3OUTCOMPAREA_INT);
  177. }
  178. #else //!WIRING
  179. // For arduino - in future: call here to a currently undefined function to reset the timer
  180. #endif
  181. }
  182. static boolean isTimerActive(timer16_Sequence_t timer) {
  183. // returns true if any servo is active on this timer
  184. for (uint8_t channel = 0; channel < SERVOS_PER_TIMER; channel++) {
  185. if (SERVO(timer, channel).Pin.isActive)
  186. return true;
  187. }
  188. return false;
  189. }
  190. /****************** end of static functions ******************************/
  191. Servo::Servo() {
  192. if (ServoCount < MAX_SERVOS) {
  193. this->servoIndex = ServoCount++; // assign a servo index to this instance
  194. servo_info[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
  195. }
  196. else
  197. this->servoIndex = INVALID_SERVO; // too many servos
  198. }
  199. int8_t Servo::attach(int pin) {
  200. return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  201. }
  202. int8_t Servo::attach(int pin, int min, int max) {
  203. if (this->servoIndex >= MAX_SERVOS) return -1;
  204. if (pin > 0) servo_info[this->servoIndex].Pin.nbr = pin;
  205. pinMode(servo_info[this->servoIndex].Pin.nbr, OUTPUT); // set servo pin to output
  206. // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
  207. this->min = (MIN_PULSE_WIDTH - min) / 4; //resolution of min/max is 4 uS
  208. this->max = (MAX_PULSE_WIDTH - max) / 4;
  209. // initialize the timer if it has not already been initialized
  210. timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
  211. if (!isTimerActive(timer)) initISR(timer);
  212. servo_info[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
  213. return this->servoIndex;
  214. }
  215. void Servo::detach() {
  216. servo_info[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. value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
  223. }
  224. this->writeMicroseconds(value);
  225. }
  226. void Servo::writeMicroseconds(int value) {
  227. // calculate and store the values for the given channel
  228. byte channel = this->servoIndex;
  229. if (channel < MAX_SERVOS) { // ensure channel is valid
  230. // ensure pulse width is valid
  231. value = constrain(value, SERVO_MIN(), SERVO_MAX()) - (TRIM_DURATION);
  232. value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
  233. CRITICAL_SECTION_START;
  234. servo_info[channel].ticks = value;
  235. CRITICAL_SECTION_END;
  236. }
  237. }
  238. // return the value as degrees
  239. int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
  240. int Servo::readMicroseconds() {
  241. return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servo_info[this->servoIndex].ticks) + TRIM_DURATION;
  242. }
  243. bool Servo::attached() { return servo_info[this->servoIndex].Pin.isActive; }
  244. void Servo::move(int value) {
  245. if (this->attach(0) >= 0) {
  246. this->write(value);
  247. #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
  248. delay(SERVO_DEACTIVATION_DELAY);
  249. this->detach();
  250. #endif
  251. }
  252. }
  253. #endif