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

servo.cpp 13KB

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