My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Du kan inte välja fler än 25 ämnen Ämnen måste starta med en bokstav eller siffra, kan innehålla bindestreck ('-') och vara max 35 tecken långa.

Servo.cpp 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344
  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. {
  55. if( Channel[timer] < 0 )
  56. *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
  57. else{
  58. if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
  59. digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
  60. }
  61. Channel[timer]++; // increment to the next channel
  62. if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
  63. *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
  64. if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
  65. digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
  66. }
  67. else {
  68. // finished all channels so wait for the refresh period to expire before starting over
  69. if( ((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) ) // allow a few ticks to ensure the next OCR1A not missed
  70. *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
  71. else
  72. *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
  73. Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
  74. }
  75. }
  76. #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
  77. // Interrupt handlers for Arduino
  78. #if defined(_useTimer1)
  79. SIGNAL (TIMER1_COMPA_vect)
  80. {
  81. handle_interrupts(_timer1, &TCNT1, &OCR1A);
  82. }
  83. #endif
  84. #if defined(_useTimer3)
  85. SIGNAL (TIMER3_COMPA_vect)
  86. {
  87. handle_interrupts(_timer3, &TCNT3, &OCR3A);
  88. }
  89. #endif
  90. #if defined(_useTimer4)
  91. SIGNAL (TIMER4_COMPA_vect)
  92. {
  93. handle_interrupts(_timer4, &TCNT4, &OCR4A);
  94. }
  95. #endif
  96. #if defined(_useTimer5)
  97. SIGNAL (TIMER5_COMPA_vect)
  98. {
  99. handle_interrupts(_timer5, &TCNT5, &OCR5A);
  100. }
  101. #endif
  102. #elif defined WIRING
  103. // Interrupt handlers for Wiring
  104. #if defined(_useTimer1)
  105. void Timer1Service()
  106. {
  107. handle_interrupts(_timer1, &TCNT1, &OCR1A);
  108. }
  109. #endif
  110. #if defined(_useTimer3)
  111. void Timer3Service()
  112. {
  113. handle_interrupts(_timer3, &TCNT3, &OCR3A);
  114. }
  115. #endif
  116. #endif
  117. static void initISR(timer16_Sequence_t timer)
  118. {
  119. #if defined (_useTimer1)
  120. if(timer == _timer1) {
  121. TCCR1A = 0; // normal counting mode
  122. TCCR1B = _BV(CS11); // set prescaler of 8
  123. TCNT1 = 0; // clear the timer count
  124. #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
  125. TIFR |= _BV(OCF1A); // clear any pending interrupts;
  126. TIMSK |= _BV(OCIE1A) ; // enable the output compare interrupt
  127. #else
  128. // here if not ATmega8 or ATmega128
  129. TIFR1 |= _BV(OCF1A); // clear any pending interrupts;
  130. TIMSK1 |= _BV(OCIE1A) ; // enable the output compare interrupt
  131. #endif
  132. #if defined(WIRING)
  133. timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
  134. #endif
  135. }
  136. #endif
  137. #if defined (_useTimer3)
  138. if(timer == _timer3) {
  139. TCCR3A = 0; // normal counting mode
  140. TCCR3B = _BV(CS31); // set prescaler of 8
  141. TCNT3 = 0; // clear the timer count
  142. #if defined(__AVR_ATmega128__)
  143. TIFR |= _BV(OCF3A); // clear any pending interrupts;
  144. ETIMSK |= _BV(OCIE3A); // enable the output compare interrupt
  145. #else
  146. TIFR3 = _BV(OCF3A); // clear any pending interrupts;
  147. TIMSK3 = _BV(OCIE3A) ; // enable the output compare interrupt
  148. #endif
  149. #if defined(WIRING)
  150. timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
  151. #endif
  152. }
  153. #endif
  154. #if defined (_useTimer4)
  155. if(timer == _timer4) {
  156. TCCR4A = 0; // normal counting mode
  157. TCCR4B = _BV(CS41); // set prescaler of 8
  158. TCNT4 = 0; // clear the timer count
  159. TIFR4 = _BV(OCF4A); // clear any pending interrupts;
  160. TIMSK4 = _BV(OCIE4A) ; // enable the output compare interrupt
  161. }
  162. #endif
  163. #if defined (_useTimer5)
  164. if(timer == _timer5) {
  165. TCCR5A = 0; // normal counting mode
  166. TCCR5B = _BV(CS51); // set prescaler of 8
  167. TCNT5 = 0; // clear the timer count
  168. TIFR5 = _BV(OCF5A); // clear any pending interrupts;
  169. TIMSK5 = _BV(OCIE5A) ; // enable the output compare interrupt
  170. }
  171. #endif
  172. }
  173. static void finISR(timer16_Sequence_t timer)
  174. {
  175. //disable use of the given timer
  176. #if defined WIRING // Wiring
  177. if(timer == _timer1) {
  178. #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
  179. TIMSK1 &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
  180. #else
  181. TIMSK &= ~_BV(OCIE1A) ; // disable timer 1 output compare interrupt
  182. #endif
  183. timerDetach(TIMER1OUTCOMPAREA_INT);
  184. }
  185. else if(timer == _timer3) {
  186. #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
  187. TIMSK3 &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
  188. #else
  189. ETIMSK &= ~_BV(OCIE3A); // disable the timer3 output compare A interrupt
  190. #endif
  191. timerDetach(TIMER3OUTCOMPAREA_INT);
  192. }
  193. #else
  194. //For arduino - in future: call here to a currently undefined function to reset the timer
  195. #endif
  196. }
  197. static boolean isTimerActive(timer16_Sequence_t timer)
  198. {
  199. // returns true if any servo is active on this timer
  200. for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
  201. if(SERVO(timer,channel).Pin.isActive == true)
  202. return true;
  203. }
  204. return false;
  205. }
  206. /****************** end of static functions ******************************/
  207. Servo::Servo()
  208. {
  209. if( ServoCount < MAX_SERVOS) {
  210. this->servoIndex = ServoCount++; // assign a servo index to this instance
  211. servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
  212. }
  213. else
  214. this->servoIndex = INVALID_SERVO ; // too many servos
  215. }
  216. uint8_t Servo::attach(int pin)
  217. {
  218. return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  219. }
  220. uint8_t Servo::attach(int pin, int min, int max)
  221. {
  222. if(this->servoIndex < MAX_SERVOS ) {
  223. #if defined (ENABLE_AUTO_BED_LEVELING) && (PROBE_SERVO_DEACTIVATION_DELAY > 0)
  224. if (pin > 0) this->pin = pin; else pin = this->pin;
  225. #endif
  226. pinMode( pin, OUTPUT) ; // set servo pin to output
  227. servos[this->servoIndex].Pin.nbr = pin;
  228. // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
  229. this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
  230. this->max = (MAX_PULSE_WIDTH - max)/4;
  231. // initialize the timer if it has not already been initialized
  232. timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
  233. if(isTimerActive(timer) == false)
  234. initISR(timer);
  235. servos[this->servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
  236. }
  237. return this->servoIndex ;
  238. }
  239. void Servo::detach()
  240. {
  241. servos[this->servoIndex].Pin.isActive = false;
  242. timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
  243. if(isTimerActive(timer) == false) {
  244. finISR(timer);
  245. }
  246. }
  247. void Servo::write(int value)
  248. {
  249. if(value < MIN_PULSE_WIDTH)
  250. { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
  251. if(value < 0) value = 0;
  252. if(value > 180) value = 180;
  253. value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
  254. }
  255. this->writeMicroseconds(value);
  256. }
  257. void Servo::writeMicroseconds(int value)
  258. {
  259. // calculate and store the values for the given channel
  260. byte channel = this->servoIndex;
  261. if( (channel < MAX_SERVOS) ) // ensure channel is valid
  262. {
  263. if( value < SERVO_MIN() ) // ensure pulse width is valid
  264. value = SERVO_MIN();
  265. else if( value > SERVO_MAX() )
  266. value = SERVO_MAX();
  267. value = value - TRIM_DURATION;
  268. value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009
  269. uint8_t oldSREG = SREG;
  270. cli();
  271. servos[channel].ticks = value;
  272. SREG = oldSREG;
  273. }
  274. }
  275. int Servo::read() // return the value as degrees
  276. {
  277. return map( this->readMicroseconds()+1, SERVO_MIN(), SERVO_MAX(), 0, 180);
  278. }
  279. int Servo::readMicroseconds()
  280. {
  281. unsigned int pulsewidth;
  282. if( this->servoIndex != INVALID_SERVO )
  283. pulsewidth = ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION ; // 12 aug 2009
  284. else
  285. pulsewidth = 0;
  286. return pulsewidth;
  287. }
  288. bool Servo::attached()
  289. {
  290. return servos[this->servoIndex].Pin.isActive ;
  291. }
  292. #endif