Halloween Mr. Scary Pumpkin

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   
5 This library is free software; you can redistribute it and/or
6 modify it   under the terms of the GNU Lesser General Public
7 License as published by the   Free Software Foundation; either
8 version 2.1 of the License, or (at your option)   any later version.
9 
10 This library is distributed in the hope that it will   be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13 Lesser General   Public License for more details.
14 
15 You should have received a copy of the   GNU Lesser General Public
16 License along with this library; if not, write to   the Free Software
17 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA   02110-1301  USA
18 */
19
20/*
21  Function slowmove and supporting code added   2010 by Korman. Above limitations apply
22  to all added code, except for the official   maintainer of the Servo library. If he,
23  and only he deems the enhancment a   good idea to add to the official Servo library,
24  he may add it without the requirement   to name the author of the parts original to
25  this version of the library.
26*/
27
28/*
29   Updated 2013 by Philip van Allen (pva), 
30  -- updated for Arduino 1.0 +
31   -- consolidated slowmove into the write command (while keeping slowmove() for   compatibility
32     with Korman's version)
33  -- added wait parameter to allow   write command to block until move is complete
34  -- added sequence playing ability   to asynchronously move the servo through a series of positions, must be called in   a loop
35
36  A servo is activated by creating an instance of the Servo class   passing the desired pin to the attach() method.
37  The servos are pulsed in the   background using the value most recently written using the write() method
38
39   Note that analogWrite of PWM on pins associated with the timer are disabled when   the first servo is attached.
40  Timers are seized as needed in groups of 12 servos   - 24 servos use two timers, 48 servos will use four.
41  The sequence used to sieze   timers is defined in timers.h
42
43  The methods are:
44
45   VarSpeedServo   - Class for manipulating servo motors connected to Arduino pins.
46
47   attach(pin   )  - Attaches a servo motor to an i/o pin.
48   attach(pin, min, max  ) - Attaches   to a pin setting min and max values in microseconds
49   default min is 544, max   is 2400  
50 
51   write(value)     - Sets the servo angle in degrees.  (invalid   angle that is valid as pulse in microseconds is treated as microseconds)
52   write(value,   speed) - speed varies the speed of the move to new position 0=full speed, 1-255   slower to faster
53   write(value, speed, wait) - wait is a boolean that, if true,   causes the function call to block until move is complete
54
55   writeMicroseconds()   - Sets the servo pulse width in microseconds 
56   read()      - Gets the last   written servo pulse width as an angle between 0 and 180. 
57   readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first   release)
58   attached()  - Returns true if there is a servo attached. 
59   detach()     - Stops an attached servos from pulsing its i/o pin. 
60
61   slowmove(value,   speed) - The same as write(value, speed), retained for compatibility with Korman's   version
62
63   stop() - stops the servo at the current position
64
65   sequencePlay(sequence,   sequencePositions); // play a looping sequence starting at position 0
66   sequencePlay(sequence,   sequencePositions, loop, startPosition); // play sequence with number of positions,   loop if true, start at position
67   sequenceStop(); // stop sequence at current   position
68
69 */
70
71#include <avr/interrupt.h>
72#include <Arduino.h> //   updated from WProgram.h to Arduino.h for Arduino 1.0+, pva
73
74#include "VarSpeedServo.h"
75
76#define   usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds   to tick (assumes prescale of 8)  // 12 Aug 2009
77#define ticksToUs(_ticks) ((   (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back   to microseconds
78
79
80#define TRIM_DURATION       2                               //   compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
81
82//#define   NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)
83
84static servo_t servos[MAX_SERVOS];                           // static array of servo structures
85static volatile   int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed   for each timer (or -1 if refresh interval)
86
87uint8_t ServoCount = 0;                                     //   the total number of attached servos
88
89// sequence vars
90
91servoSequencePoint   initSeq[] = {{0,100},{45,100}};
92
93//sequence_t sequences[MAX_SEQUENCE];
94
95//   convenience macros
96#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr   / SERVOS_PER_TIMER)) // returns the timer controlling this servo
97#define SERVO_INDEX_TO_CHANNEL(_servo_nbr)   (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this   timer
98#define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)      // macro to access servo index by timer and channel
99#define SERVO(_timer,_channel)   (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class   by timer and channel
100
101#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)   // minimum value in uS for this servo
102#define SERVO_MAX() (MAX_PULSE_WIDTH   - this->max * 4)  // maximum value in uS for this servo 
103
104/************ static   functions common to all instances ***********************/
105
106static inline   void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile   uint16_t* OCRnA)
107{
108  if( Channel[timer] < 0 )
109    *TCNTn = 0; // channel   set to -1 indicated that refresh interval completed so reset the timer 
110  else{
111     if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive   == true )  
112      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); //   pulse this channel low if activated   
113  }
114
115  Channel[timer]++;    // increment   to the next channel
116  if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer]   < SERVOS_PER_TIMER) {
117
118	// Extension for slowmove
119	if (SERVO(timer,Channel[timer]).speed)   {
120		// Increment ticks by speed until we reach the target.
121		// When the   target is reached, speed is set to 0 to disable that code.
122		if (SERVO(timer,Channel[timer]).target   > SERVO(timer,Channel[timer]).ticks) {
123			SERVO(timer,Channel[timer]).ticks   += SERVO(timer,Channel[timer]).speed;
124			if (SERVO(timer,Channel[timer]).target   <= SERVO(timer,Channel[timer]).ticks) {
125				SERVO(timer,Channel[timer]).ticks   = SERVO(timer,Channel[timer]).target;
126				SERVO(timer,Channel[timer]).speed   = 0;
127			}
128		}
129		else {
130			SERVO(timer,Channel[timer]).ticks   -= SERVO(timer,Channel[timer]).speed;
131			if (SERVO(timer,Channel[timer]).target   >= SERVO(timer,Channel[timer]).ticks) {
132				SERVO(timer,Channel[timer]).ticks   = SERVO(timer,Channel[timer]).target;
133				SERVO(timer,Channel[timer]).speed   = 0;
134			}
135		}
136	}
137	// End of Extension for slowmove
138
139	//   Todo
140	
141    *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
142    if(SERVO(timer,Channel[timer]).Pin.isActive   == true)     // check if activated
143      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH);   // its an active channel so pulse it high   
144  }  
145  else { 
146    // finished   all channels so wait for the refresh period to expire before starting over 
147     if( (unsigned)*TCNTn <  (usToTicks(REFRESH_INTERVAL) + 4) )  // allow a few   ticks to ensure the next OCR1A not missed
148      *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);   
149    else 
150      *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has   elapsed
151    Channel[timer] = -1; // this will get incremented at the end of the   refresh period to start again at the first channel
152  }
153}
154
155#ifndef WIRING   // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring   platform
156// Interrupt handlers for Arduino 
157#if defined(_useTimer1)
158SIGNAL   (TIMER1_COMPA_vect) 
159{ 
160  handle_interrupts(_timer1, &TCNT1, &OCR1A); 
161}
162#endif
163
164#if   defined(_useTimer3)
165SIGNAL (TIMER3_COMPA_vect) 
166{ 
167  handle_interrupts(_timer3,   &TCNT3, &OCR3A); 
168}
169#endif
170
171#if defined(_useTimer4)
172SIGNAL (TIMER4_COMPA_vect)   
173{
174  handle_interrupts(_timer4, &TCNT4, &OCR4A); 
175}
176#endif
177
178#if   defined(_useTimer5)
179SIGNAL (TIMER5_COMPA_vect) 
180{
181  handle_interrupts(_timer5,   &TCNT5, &OCR5A); 
182}
183#endif
184
185#elif defined WIRING
186// Interrupt handlers   for Wiring 
187#if defined(_useTimer1)
188void Timer1Service() 
189{ 
190  handle_interrupts(_timer1,   &TCNT1, &OCR1A); 
191}
192#endif
193#if defined(_useTimer3)
194void Timer3Service()   
195{ 
196  handle_interrupts(_timer3, &TCNT3, &OCR3A); 
197}
198#endif
199#endif
200
201
202static   void initISR(timer16_Sequence_t timer)
203{  
204#if defined (_useTimer1)
205  if(timer   == _timer1) {
206    TCCR1A = 0;             // normal counting mode 
207    TCCR1B   = _BV(CS11);     // set prescaler of 8 
208    TCNT1 = 0;              // clear   the timer count 
209#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
210     TIFR |= _BV(OCF1A);      // clear any pending interrupts; 
211    TIMSK |=  _BV(OCIE1A)   ;  // enable the output compare interrupt  
212#else
213    // here if not ATmega8   or ATmega128
214    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts; 
215     TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt 
216#endif    
217#if   defined(WIRING)       
218    timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);   
219#endif	
220  } 
221#endif  
222
223#if defined (_useTimer3)
224  if(timer ==   _timer3) {
225    TCCR3A = 0;             // normal counting mode 
226    TCCR3B   = _BV(CS31);     // set prescaler of 8  
227    TCNT3 = 0;              // clear   the timer count 
228#if defined(__AVR_ATmega128__)
229    TIFR |= _BV(OCF3A);     //   clear any pending interrupts;   
230	ETIMSK |= _BV(OCIE3A);  // enable the output   compare interrupt     
231#else  
232    TIFR3 = _BV(OCF3A);     // clear any pending   interrupts; 
233    TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt       
234#endif
235#if defined(WIRING)    
236    timerAttach(TIMER3OUTCOMPAREA_INT,   Timer3Service);  // for Wiring platform only	
237#endif  
238  }
239#endif
240
241#if   defined (_useTimer4)
242  if(timer == _timer4) {
243    TCCR4A = 0;             //   normal counting mode 
244    TCCR4B = _BV(CS41);     // set prescaler of 8  
245     TCNT4 = 0;              // clear the timer count 
246    TIFR4 = _BV(OCF4A);      // clear any pending interrupts; 
247    TIMSK4 =  _BV(OCIE4A) ; // enable   the output compare interrupt
248  }    
249#endif
250
251#if defined (_useTimer5)
252   if(timer == _timer5) {
253    TCCR5A = 0;             // normal counting mode   
254    TCCR5B = _BV(CS51);     // set prescaler of 8  
255    TCNT5 = 0;              //   clear the timer count 
256    TIFR5 = _BV(OCF5A);     // clear any pending interrupts;   
257    TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt      
258   }
259#endif
260} 
261
262static void finISR(timer16_Sequence_t timer)
263{
264     //disable use of the given timer
265#if defined WIRING   // Wiring
266  if(timer   == _timer1) {
267    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
268     TIMSK1 &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
269    #else   
270    TIMSK &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt   
271     #endif
272    timerDetach(TIMER1OUTCOMPAREA_INT); 
273  }
274  else if(timer   == _timer3) {     
275    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
276     TIMSK3 &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
277     #else
278    ETIMSK &= ~_BV(OCIE3A);    // disable the timer3 output compare   A interrupt
279    #endif
280    timerDetach(TIMER3OUTCOMPAREA_INT);
281  }
282#else
283     //For arduino - in future: call here to a currently undefined function to reset   the timer
284#endif
285}
286
287static boolean isTimerActive(timer16_Sequence_t   timer)
288{
289  // returns true if any servo is active on this timer
290  for(uint8_t   channel=0; channel < SERVOS_PER_TIMER; channel++) {
291    if(SERVO(timer,channel).Pin.isActive   == true)
292      return true;
293  }
294  return false;
295}
296
297
298/******************   end of static functions ******************************/
299
300VarSpeedServo::VarSpeedServo()
301{
302   if( ServoCount < MAX_SERVOS) {
303    this->servoIndex = ServoCount++;                    //   assign a servo index to this instance
304	  servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);    // store default values  - 12 Aug 2009
305    this->curSeqPosition = 0;
306    this->curSequence   = initSeq;
307  }
308  else
309    this->servoIndex = INVALID_SERVO ;  // too many   servos 
310}
311
312uint8_t VarSpeedServo::attach(int pin)
313{
314  return this->attach(pin,   MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
315}
316
317uint8_t VarSpeedServo::attach(int   pin, int min, int max)
318{
319  if(this->servoIndex < MAX_SERVOS ) {
320    pinMode(   pin, OUTPUT) ;                                   // set servo pin to output
321     servos[this->servoIndex].Pin.nbr = pin;  
322    // todo min/max check: abs(min   - MIN_PULSE_WIDTH) /4 < 128 
323    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution   of min/max is 4 uS
324    this->max  = (MAX_PULSE_WIDTH - max)/4; 
325    // initialize   the timer if it has not already been initialized 
326    timer16_Sequence_t timer   = SERVO_INDEX_TO_TIMER(servoIndex);
327    if(isTimerActive(timer) == false)
328       initISR(timer);    
329    servos[this->servoIndex].Pin.isActive = true;  //   this must be set after the check for isTimerActive
330  } 
331  return this->servoIndex   ;
332}
333
334void VarSpeedServo::detach()  
335{
336  servos[this->servoIndex].Pin.isActive   = false;  
337  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
338   if(isTimerActive(timer) == false) {
339    finISR(timer);
340  }
341}
342
343void   VarSpeedServo::write(int value)
344{  
345  if(value < MIN_PULSE_WIDTH)
346  {  //   treat values less than 544 as angles in degrees (valid values in microseconds are   handled as microseconds)
347    // updated to use constrain() instead of if(), pva
348     value = constrain(value, 0, 180);
349    value = map(value, 0, 180, SERVO_MIN(),   SERVO_MAX());      
350  }
351  this->writeMicroseconds(value);
352}
353
354void   VarSpeedServo::writeMicroseconds(int value)
355{
356  // calculate and store the   values for the given channel
357  byte channel = this->servoIndex;
358  if( (channel   >= 0) && (channel < MAX_SERVOS) )   // ensure channel is valid
359  {  
360    if(   value < SERVO_MIN() )          // ensure pulse width is valid
361      value = SERVO_MIN();
362     else if( value > SERVO_MAX() )
363      value = SERVO_MAX();   
364    
365   	value -= TRIM_DURATION;
366    value = usToTicks(value);  // convert to ticks   after compensating for interrupt overhead - 12 Aug 2009
367
368    uint8_t oldSREG   = SREG;
369    cli();
370    servos[channel].ticks = value;  
371    SREG = oldSREG;    
372
373	// Extension for slowmove
374	// Disable slowmove logic.
375	servos[channel].speed   = 0;  
376	// End of Extension for slowmove
377  } 
378}
379
380// Extension for   slowmove
381/*
382  write(value, speed) - Just like write but at reduced speed.
383
384   value - Target position for the servo. Identical use as value of the function   write.
385  speed - Speed at which to move the servo.
386          speed=0 - Full   speed, identical to write
387          speed=1 - Minimum speed
388          speed=255   - Maximum speed
389*/
390void VarSpeedServo::write(int value, uint8_t speed) {
391	//   This fuction is a copy of write and writeMicroseconds but value will be saved
392	//   in target instead of in ticks in the servo structure and speed will be save
393	//   there too.
394
395  int degrees = value;
396
397	if (speed) {
398		if (value   < MIN_PULSE_WIDTH) {
399			// treat values less than 544 as angles in degrees   (valid values in microseconds are handled as microseconds)
400      		// updated   to use constrain instead of if, pva
401      		value = constrain(value, 0, 180);
402       		value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());    
403		}
404		//   calculate and store the values for the given channel
405		byte channel = this->servoIndex;
406		if(   (channel >= 0) && (channel < MAX_SERVOS) ) {   // ensure channel is valid
407      		//   updated to use constrain instead of if, pva
408      		value = constrain(value,   SERVO_MIN(), SERVO_MAX());
409
410			value = value - TRIM_DURATION;
411			value   = usToTicks(value);  // convert to ticks after compensating for interrupt overhead   - 12 Aug 2009
412
413			// Set speed and direction
414			uint8_t oldSREG =   SREG;
415			cli();
416			servos[channel].target = value;  
417			servos[channel].speed   = speed;  
418			SREG = oldSREG;   
419		}
420	} 
421	else {
422		write   (value);
423	}
424}
425
426void VarSpeedServo::write(int value, uint8_t speed,   bool wait) {
427  write(value, speed);
428  if (wait) { // block until the servo   is at its new position
429    if (value < MIN_PULSE_WIDTH) {
430      while (read()   != value) {
431        delay(5);
432      }
433    } else {
434      while (readMicroseconds()   != value) {
435        delay(5);
436      }
437    }
438  }
439}
440
441void VarSpeedServo::stop()   {
442  write(read());
443}
444
445void VarSpeedServo::slowmove(int value, uint8_t   speed) {
446  // legacy function to support original version of VarSpeedServo
447   write(value, speed);
448}
449
450// End of Extension for slowmove
451
452
453int   VarSpeedServo::read() // return the value as degrees
454{
455  return  map( this->readMicroseconds()+1,   SERVO_MIN(), SERVO_MAX(), 0, 180);     
456}
457
458int VarSpeedServo::readMicroseconds()
459{
460   unsigned int pulsewidth;
461  if( this->servoIndex != INVALID_SERVO )
462    pulsewidth   = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION ;   // 12 aug 2009
463   else 
464    pulsewidth  = 0;
465
466  return pulsewidth;   
467}
468
469bool   VarSpeedServo::attached()
470{
471  return servos[this->servoIndex].Pin.isActive   ;
472}
473
474uint8_t VarSpeedServo::sequencePlay(servoSequencePoint sequenceIn[],   uint8_t numPositions, bool loop, uint8_t startPos) {
475  uint8_t oldSeqPosition   = this->curSeqPosition;
476
477  if( this->curSequence != sequenceIn) {
478    //Serial.println("newSeq");
479     this->curSequence = sequenceIn;
480    this->curSeqPosition = startPos;
481     oldSeqPosition = 255;
482  }
483  
484  if (read() == sequenceIn[this->curSeqPosition].position   && this->curSeqPosition != CURRENT_SEQUENCE_STOP) {
485    this->curSeqPosition++;
486     
487    if (this->curSeqPosition >= numPositions) { // at the end of the loop
488       if (loop) { // reset to the beginning of the loop
489        this->curSeqPosition   = 0;
490      } else { // stop the loop
491        this->curSeqPosition = CURRENT_SEQUENCE_STOP;
492       }
493    }
494  }
495
496  if (this->curSeqPosition != oldSeqPosition && this->curSeqPosition   != CURRENT_SEQUENCE_STOP) { 
497    // CURRENT_SEQUENCE_STOP position means the   animation has ended, and should no longer be played
498    // otherwise move to   the next position
499    write(sequenceIn[this->curSeqPosition].position, sequenceIn[this->curSeqPosition].speed);
500     //Serial.println(this->seqCurPosition);
501  }
502  
503  return this->curSeqPosition;
504}
505
506uint8_t   VarSpeedServo::sequencePlay(servoSequencePoint sequenceIn[], uint8_t numPositions)   {
507  return sequencePlay(sequenceIn, numPositions, true, 0);
508}
509
510void   VarSpeedServo::sequenceStop() {
511  write(read());
512  this->curSeqPosition =   CURRENT_SEQUENCE_STOP;
513}
514
515/*
516	To do
517int VarSpeedServo::targetPosition()   {
518	byte channel = this->servoIndex;
519	return map( servos[channel].target+1,   SERVO_MIN(), SERVO_MAX(), 0, 180);
520}
521
522int VarSpeedServo::targetPositionMicroseconds()   {
523	byte channel = this->servoIndex;
524	return servos[channel].target;
525}
526
527bool   VarSpeedServo::isMoving() {
528	byte channel = this->servoIndex;
529	int servos[channel].target;
530}
531*/
532

1/*
2  VarSpeedServo.h - Interrupt driven Servo library for Arduino using  16 bit timers- Version 2
3  Copyright (c) 2009 Michael Margolis.  All right reserved.
4
5  This library is free software; you can redistribute it and/or
6  modify it under  the terms of the GNU Lesser General Public
7  License as published by the Free  Software Foundation; either
8  version 2.1 of the License, or (at your option)  any later version.
9
10  This library is distributed in the hope that it will  be useful,
11  but WITHOUT ANY WARRANTY; without even the implied warranty of
12  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13  Lesser  General Public License for more details.
14
15  You should have received a copy  of the GNU Lesser General Public
16  License along with this library; if not, write  to the Free Software
17  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,  MA  02110-1301  USA
18*/
19
20
21/*
22  Function slowmove and supporting code  added 2010 by Korman. Above limitations apply
23  to all added code, except for  the official maintainer of the Servo library. If he,
24  and only he deems the  enhancment a good idea to add to the official Servo library,
25  he may add it  without the requirement to name the author of the parts original to
26  this version  of the library.
27*/
28
29/*
30  Updated 2013 by Philip van Allen (pva), 
31  -- updated for Arduino 1.0 +
32  -- consolidated slowmove into the write command  (while keeping slowmove() for compatibility
33     with Korman's version)
34  --  added wait parameter to allow write command to block until move is complete
35  -- added sequence playing ability to asynchronously move the servo through a series  of positions, must be called in a loop
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
41  Note that analogWrite of PWM on pins associated  with the timer are disabled when the first servo is attached.
42  Timers are seized  as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use  four.
43  The sequence used to sieze timers is defined in timers.h
44
45  The  methods are:
46
47   VarSpeedServo - Class for manipulating servo motors connected  to Arduino pins.
48
49   attach(pin )  - Attaches a servo motor to an i/o pin.
50   attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
51   default min is 544, max is 2400  
52 
53   write(value)     - Sets the servo  angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated  as microseconds)
54   write(value, speed) - speed varies the speed of the move  to new position 0=full speed, 1-255 slower to faster
55   write(value, speed, wait)  - wait is a boolean that, if true, causes the function call to block until move  is complete
56
57   writeMicroseconds() - Sets the servo pulse width in microseconds  
58   read()      - Gets the last written servo pulse width as an angle between  0 and 180. 
59   readMicroseconds()  - Gets the last written servo pulse width  in microseconds. (was read_us() in first release)
60   attached()  - Returns true  if there is a servo attached. 
61   detach()    - Stops an attached servos from  pulsing its i/o pin. 
62
63   slowmove(value, speed) - The same as write(value,  speed), retained for compatibility with Korman's version
64
65   stop() - stops  the servo at the current position
66
67   sequencePlay(sequence, sequencePositions);  // play a looping sequence starting at position 0
68   sequencePlay(sequence, sequencePositions,  loop, startPosition); // play sequence with number of positions, loop if true, start  at position
69   sequenceStop(); // stop sequence at current position
70
71 */
72
73#ifndef  VarSpeedServo_h
74#define VarSpeedServo_h
75
76#include <inttypes.h>
77
78/*  
79 * Defines for 16 bit timers used with  Servo library 
80 *
81 * If _useTimerX  is defined then TimerX is a 16 bit timer on the curent board
82 * timer16_Sequence_t  enumerates the sequence that the timers should be allocated
83 * _Nbr_16timers  indicates how many 16 bit timers are available.
84 *
85 */
86
87// Say which  16 bit timers can be used and in what order
88#if defined(__AVR_ATmega1280__)  ||  defined(__AVR_ATmega2560__)
89#define _useTimer5
90#define _useTimer1 
91#define  _useTimer3
92#define _useTimer4 
93typedef enum { _timer5, _timer1, _timer3, _timer4,  _Nbr_16timers } timer16_Sequence_t ;
94
95#elif defined(__AVR_ATmega32U4__)  
96#define  _useTimer3
97#define _useTimer1 
98typedef enum { _timer3, _timer1, _Nbr_16timers  } timer16_Sequence_t ;
99
100#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
101#define  _useTimer3
102#define _useTimer1
103typedef enum { _timer3, _timer1, _Nbr_16timers  } timer16_Sequence_t ;
104
105#elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
106#define  _useTimer3
107#define _useTimer1
108typedef enum { _timer3, _timer1, _Nbr_16timers  } timer16_Sequence_t ;
109
110#else  // everything else
111#define _useTimer1
112typedef  enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;                  
113#endif
114
115#define  VarSpeedServo_VERSION           2      // software version of this library
116
117#define  MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo  
118#define  MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo 
119#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
120#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds 
121
122#define  SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one  timer 
123#define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
124
125#define  INVALID_SERVO         255     // flag indicating an invalid servo index
126
127#define  CURRENT_SEQUENCE_STOP   255    // used to indicate the current sequence is not used  and sequence should stop
128
129
130typedef struct  {
131  uint8_t nbr        :6  ;             // a pin number from 0 to 63
132  uint8_t isActive   :1 ;             //  true if this channel is enabled, pin not pulsed if false 
133} ServoPin_t   ;  
134
135typedef  struct {
136  ServoPin_t Pin;
137  unsigned int ticks;
138	unsigned int target;			//  Extension for slowmove
139	uint8_t speed;					// Extension for slowmove
140}  servo_t;
141
142typedef struct {
143  uint8_t position;
144  uint8_t speed;
145}  servoSequencePoint;
146
147class VarSpeedServo
148{
149public:
150  VarSpeedServo();
151  uint8_t attach(int pin);           // attach the given pin to the next free channel,  sets pinMode, returns channel number or 0 if failure
152  uint8_t attach(int pin,  int min, int max); // as above but also sets min and max values for writes. 
153  void detach();
154  void write(int value);             // if value is < 200 its  treated as an angle, otherwise as pulse width in microseconds
155  void write(int  value, uint8_t speed); // Move to given position at reduced speed.
156          //  speed=0 is identical to write, speed=1 slowest and speed=255 fastest.
157          //  On the RC-Servos tested, speeds differences above 127 can't be noticed,
158          //  because of the mechanical limits of the servo.
159  void write(int value, uint8_t  speed, bool wait); // wait parameter causes call to block until move completes
160  void writeMicroseconds(int value); // Write pulse width in microseconds 
161  void  slowmove(int value, uint8_t speed);
162  void stop(); // stop the servo where it  is
163  
164  int read();                        // returns current pulse width  as an angle between 0 and 180 degrees
165  int readMicroseconds();            //  returns current pulse width in microseconds for this servo (was read_us() in first  release)
166  bool attached();                   // return true if this servo is  attached, otherwise false 
167
168  uint8_t sequencePlay(servoSequencePoint sequenceIn[],  uint8_t numPositions, bool loop, uint8_t startPos);
169  uint8_t sequencePlay(servoSequencePoint  sequenceIn[], uint8_t numPositions); // play a looping sequence starting at position  0
170  void sequenceStop(); // stop movement
171private:
172   uint8_t servoIndex;               // index into the channel data for this servo
173   int8_t min;                       //  minimum is this value times 4 added to MIN_PULSE_WIDTH    
174   int8_t max;                       //  maximum is this value times 4 added to MAX_PULSE_WIDTH
175   servoSequencePoint  * curSequence; // for sequences
176   uint8_t curSeqPosition; // for sequences
177
178};
179
180#endif
181

1#include "VarSpeedServo.h"  //include the VarSpeedServo library
2#include  <NewPing.h>        //include the NewPing library
3
4
5VarSpeedServo RU;  //Right  Upper
6VarSpeedServo RL;  //Right Lower
7VarSpeedServo LU;  //Left Upper
8VarSpeedServo  LL;  //Left Lower
9
10NewPing sonar(4,3,100); 
11                                                     //vel(min),  delay_Forward(max) = (5, 2000) 
12const int vel = 40, vel_Back = 15;                   //vel(mid),  delay_Forward(mid) = (20, 750) 
13const int delay_Forward = 300, delay_Back = 750;     //vel(max), delay_Forward(min)= (256, 50)
14                                                     //wonderful  ---> (10, 700) (50, 500) (100, 100) (100, 300) (100, 500)
15
16int vel_Dance1  = 30,    vel_Dance2 = 25,    vel_Dance3 = 40;     
17int delay_Dance1 = 300, delay_Dance2  = 750, delay_Dance3 = 200;  
18
19int vel_Dance4 = 40,    vel_Dance5 = 40,    vel_Dance6  = 30;
20int delay_Dance4 = 400, delay_Dance5 = 400, delay_Dance6 = 500;
21
22const  int array_cal[4] = {90,90,90,90};    
23int RU_Degree = 0, LU_Degree = array_cal[2]  + 5;
24
25const int num_dance4 = 20;      //in place somersault inwards and accelerate  angles
26const int array_dance4[num_dance4][4] =
27{     
28    {0,-20,0,20},   
29    {0,0,0,0},
30    {0,-20,0,20},
31    {0,0,0,0},
32     {0,0,0,0},
33    {0,-20,0,20},
34    {0,0,0,0},
35    
36    {0,-50,0,50},
37    {0,0,0,0},    
38   {0,-50,0,50},
39    {0,0,0,0},    
40    {0,-50,0,50},
41    {0,0,0,0},    
42    {0,-50,0,50},
43    {0,0,0,0},
44    
45    {0,-40,0,40},
46    {0,-50,0,50},
47    {0,-60,0,60},
48   {0,0,0,0,},
49};
50 void Dancing4()
51{  
52    for(int  z=0; z<num_dance4; z++) {
53        if ( z > 17) {
54            vel_Dance4 =  10;
55            delay_Dance4 = 1500;
56            }
57        else {
58            vel_Dance4  = 40;
59            delay_Dance4 = 400;
60            }
61                 
62        RU.slowmove (array_cal[0] + array_dance4[z][0] , vel_Dance4);   
63        RL.slowmove  (array_cal[1] + array_dance4[z][1] , vel_Dance4);
64        LU.slowmove (array_cal[2]  + array_dance4[z][2] , vel_Dance4);
65        LL.slowmove (array_cal[3] + array_dance4[z][3]  , vel_Dance4);
66        delay(delay_Dance4); 
67    } 
68}
69
70
71#define  INSTALL
72#define CALIBRATION
73#define RUN
74
75void Servo_Init()
76{
77    RU.attach(9);   // Connect the signal wire of the upper-right servo to pin 9  
78    RL.attach(10);   // Connect the signal wire of the lower-right servo to  pin 10 
79    LU.attach(11);   // Connect the signal wire of the upper-left  servo  to pin 11 
80    LL.attach(12);   // Connect the signal wire of the lower-left  servo to pin 12 
81}
82
83void Adjust()                            // Avoid  the servo's fast spinning in initialization 
84{                                        //  RU,LU goes from array_cal[0] - 5 ,array_cal[2] + 5 degrees to array_cal[0],array_cal[2]  degrees
85    for(RU_Degree = array_cal[0] - 5; RU_Degree <= array_cal[0]; RU_Degree  += 1) {
86        RU.write(RU_Degree);             // in steps of 1 degree
87        LU.write(LU_Degree--);           // tell servo to go to RU_Degreeition,  LU_Degreeition in variable 'RU_Degree', 'LU_Degree'         
88        delay(15);                       // waits 15ms for the servo to reach the RU_Degreeition
89    }
90}
91/// *******************************************************************************ping********************************************************************************************
92bool  TooClose()
93{
94    int tooclose = 0;
95    for(int a=0; a<30; a++) {  ////////  a< Was 5
96        delay(50);
97        int din = sonar.ping_in();
98        if  (din < 7 && din > 0) tooclose++;
99    }
100//{
101     if (tooclose < 5) return  0;    /// chnage 1 to 0
102    return 1;  
103    delay (15);////// changed 0 to  1    ///***********************************************************************************************************
104     
105    
106}
107
108  
109void setup()  
110{
111#ifdef INSTALL
112    Servo_Init();
113  
114    RU.slowmove (90 , vel);
115    RL.slowmove (90 , vel);
116    LU.slowmove  (90 , vel);
117    LL.slowmove (90 , vel);
118    while(1);
119#endif
120
121#ifdef  CALIBRATION 
122    Servo_Init();  
123    Adjust();
124    
125    RL.slowmove  (array_cal[1] , vel);
126    LL.slowmove (array_cal[3] , vel);
127    delay(2000);
128    while(1);
129#endif
130
131#ifdef RUN 
132    Servo_Init();
133    Adjust();  
134       
135    RL.slowmove (array_cal[1] , vel);
136    LL.slowmove (array_cal[3]  , vel);
137    delay(2000);
138#endif
139}
140
141void loop() 
142{   
143   
144    
145    while(TooClose())
146    Dancing4(); 
147    delay(100);
148     
149     
150}
151

1/*
2  VarSpeedServo.h - Interrupt driven Servo library for Arduino using
3  16 bit timers- Version 2
4  Copyright (c) 2009 Michael Margolis.  All right reserved.
5
6
7  This library is free software; you can redistribute it and/or
8  modify it under
9  the terms of the GNU Lesser General Public
10  License as published by the Free
11  Software Foundation; either
12  version 2.1 of the License, or (at your option)
13  any later version.
14
15  This library is distributed in the hope that it will
16  be useful,
17  but WITHOUT ANY WARRANTY; without even the implied warranty of
18
19  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
20  Lesser
21  General Public License for more details.
22
23  You should have received a copy
24  of the GNU Lesser General Public
25  License along with this library; if not, write
26  to the Free Software
27  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
28  MA  02110-1301  USA
29*/
30
31
32/*
33  Function slowmove and supporting code
34  added 2010 by Korman. Above limitations apply
35  to all added code, except for
36  the official maintainer of the Servo library. If he,
37  and only he deems the
38  enhancment a good idea to add to the official Servo library,
39  he may add it
40  without the requirement to name the author of the parts original to
41  this version
42  of the library.
43*/
44
45/*
46  Updated 2013 by Philip van Allen (pva), 
47
48  -- updated for Arduino 1.0 +
49  -- consolidated slowmove into the write command
50  (while keeping slowmove() for compatibility
51     with Korman's version)
52  --
53  added wait parameter to allow write command to block until move is complete
54
55  -- added sequence playing ability to asynchronously move the servo through a series
56  of positions, must be called in a loop
57
58  
59  A servo is activated by creating
60  an instance of the Servo class passing the desired pin to the attach() method.
61
62  The servos are pulsed in the background using the value most recently written
63  using the write() method
64
65  Note that analogWrite of PWM on pins associated
66  with the timer are disabled when the first servo is attached.
67  Timers are seized
68  as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use
69  four.
70  The sequence used to sieze timers is defined in timers.h
71
72  The
73  methods are:
74
75   VarSpeedServo - Class for manipulating servo motors connected
76  to Arduino pins.
77
78   attach(pin )  - Attaches a servo motor to an i/o pin.
79
80   attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
81
82   default min is 544, max is 2400  
83 
84   write(value)     - Sets the servo
85  angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated
86  as microseconds)
87   write(value, speed) - speed varies the speed of the move
88  to new position 0=full speed, 1-255 slower to faster
89   write(value, speed, wait)
90  - wait is a boolean that, if true, causes the function call to block until move
91  is complete
92
93   writeMicroseconds() - Sets the servo pulse width in microseconds
94  
95   read()      - Gets the last written servo pulse width as an angle between
96  0 and 180. 
97   readMicroseconds()  - Gets the last written servo pulse width
98  in microseconds. (was read_us() in first release)
99   attached()  - Returns true
100  if there is a servo attached. 
101   detach()    - Stops an attached servos from
102  pulsing its i/o pin. 
103
104   slowmove(value, speed) - The same as write(value,
105  speed), retained for compatibility with Korman's version
106
107   stop() - stops
108  the servo at the current position
109
110   sequencePlay(sequence, sequencePositions);
111  // play a looping sequence starting at position 0
112   sequencePlay(sequence, sequencePositions,
113  loop, startPosition); // play sequence with number of positions, loop if true, start
114  at position
115   sequenceStop(); // stop sequence at current position
116
117 */
118
119#ifndef
120  VarSpeedServo_h
121#define VarSpeedServo_h
122
123#include <inttypes.h>
124
125/*
126  
127 * Defines for 16 bit timers used with  Servo library 
128 *
129 * If _useTimerX
130  is defined then TimerX is a 16 bit timer on the curent board
131 * timer16_Sequence_t
132  enumerates the sequence that the timers should be allocated
133 * _Nbr_16timers
134  indicates how many 16 bit timers are available.
135 *
136 */
137
138// Say which
139  16 bit timers can be used and in what order
140#if defined(__AVR_ATmega1280__)  ||
141  defined(__AVR_ATmega2560__)
142#define _useTimer5
143#define _useTimer1 
144#define
145  _useTimer3
146#define _useTimer4 
147typedef enum { _timer5, _timer1, _timer3, _timer4,
148  _Nbr_16timers } timer16_Sequence_t ;
149
150#elif defined(__AVR_ATmega32U4__)  
151#define
152  _useTimer3
153#define _useTimer1 
154typedef enum { _timer3, _timer1, _Nbr_16timers
155  } timer16_Sequence_t ;
156
157#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
158#define
159  _useTimer3
160#define _useTimer1
161typedef enum { _timer3, _timer1, _Nbr_16timers
162  } timer16_Sequence_t ;
163
164#elif defined(__AVR_ATmega128__) ||defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
165#define
166  _useTimer3
167#define _useTimer1
168typedef enum { _timer3, _timer1, _Nbr_16timers
169  } timer16_Sequence_t ;
170
171#else  // everything else
172#define _useTimer1
173typedef
174  enum { _timer1, _Nbr_16timers } timer16_Sequence_t ;                  
175#endif
176
177#define
178  VarSpeedServo_VERSION           2      // software version of this library
179
180#define
181  MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo  
182#define
183  MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo 
184#define DEFAULT_PULSE_WIDTH
185  1500     // default pulse width when servo is attached
186#define REFRESH_INTERVAL
187    20000     // minumim time to refresh servos in microseconds 
188
189#define
190  SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one
191  timer 
192#define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
193
194#define
195  INVALID_SERVO         255     // flag indicating an invalid servo index
196
197#define
198  CURRENT_SEQUENCE_STOP   255    // used to indicate the current sequence is not used
199  and sequence should stop
200
201
202typedef struct  {
203  uint8_t nbr        :6
204  ;             // a pin number from 0 to 63
205  uint8_t isActive   :1 ;             //
206  true if this channel is enabled, pin not pulsed if false 
207} ServoPin_t   ;  
208
209typedef
210  struct {
211  ServoPin_t Pin;
212  unsigned int ticks;
213	unsigned int target;			//
214  Extension for slowmove
215	uint8_t speed;					// Extension for slowmove
216}
217  servo_t;
218
219typedef struct {
220  uint8_t position;
221  uint8_t speed;
222}
223  servoSequencePoint;
224
225class VarSpeedServo
226{
227public:
228  VarSpeedServo();
229
230  uint8_t attach(int pin);           // attach the given pin to the next free channel,
231  sets pinMode, returns channel number or 0 if failure
232  uint8_t attach(int pin,
233  int min, int max); // as above but also sets min and max values for writes. 
234
235  void detach();
236  void write(int value);             // if value is < 200 its
237  treated as an angle, otherwise as pulse width in microseconds
238  void write(int
239  value, uint8_t speed); // Move to given position at reduced speed.
240          //
241  speed=0 is identical to write, speed=1 slowest and speed=255 fastest.
242          //
243  On the RC-Servos tested, speeds differences above 127 can't be noticed,
244          //
245  because of the mechanical limits of the servo.
246  void write(int value, uint8_t
247  speed, bool wait); // wait parameter causes call to block until move completes
248
249  void writeMicroseconds(int value); // Write pulse width in microseconds 
250  void
251  slowmove(int value, uint8_t speed);
252  void stop(); // stop the servo where it
253  is
254  
255  int read();                        // returns current pulse width
256  as an angle between 0 and 180 degrees
257  int readMicroseconds();            //
258  returns current pulse width in microseconds for this servo (was read_us() in first
259  release)
260  bool attached();                   // return true if this servo is
261  attached, otherwise false 
262
263  uint8_t sequencePlay(servoSequencePoint sequenceIn[],
264  uint8_t numPositions, bool loop, uint8_t startPos);
265  uint8_t sequencePlay(servoSequencePoint
266  sequenceIn[], uint8_t numPositions); // play a looping sequence starting at position
267  0
268  void sequenceStop(); // stop movement
269private:
270   uint8_t servoIndex;
271               // index into the channel data for this servo
272   int8_t min;                       //
273  minimum is this value times 4 added to MIN_PULSE_WIDTH    
274   int8_t max;                       //
275  maximum is this value times 4 added to MAX_PULSE_WIDTH
276   servoSequencePoint
277  * curSequence; // for sequences
278   uint8_t curSeqPosition; // for sequences
279
280};
281
282#endif
283

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  
5 This library is free software; you can redistribute it and/or
6 modify it  under the terms of the GNU Lesser General Public
7 License as published by the  Free Software Foundation; either
8 version 2.1 of the License, or (at your option)  any later version.
9 
10 This library is distributed in the hope that it will  be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13 Lesser General  Public License for more details.
14 
15 You should have received a copy of the  GNU Lesser General Public
16 License along with this library; if not, write to  the Free Software
17 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
18 */
19
20/*
21  Function slowmove and supporting code added  2010 by Korman. Above limitations apply
22  to all added code, except for the official  maintainer of the Servo library. If he,
23  and only he deems the enhancment a  good idea to add to the official Servo library,
24  he may add it without the requirement  to name the author of the parts original to
25  this version of the library.
26*/
27
28/*
29  Updated 2013 by Philip van Allen (pva), 
30  -- updated for Arduino 1.0 +
31  -- consolidated slowmove into the write command (while keeping slowmove() for  compatibility
32     with Korman's version)
33  -- added wait parameter to allow  write command to block until move is complete
34  -- added sequence playing ability  to asynchronously move the servo through a series of positions, must be called in  a loop
35
36  A servo is activated by creating an instance of the Servo class  passing the desired pin to the attach() method.
37  The servos are pulsed in the  background using the value most recently written using the write() method
38
39  Note that analogWrite of PWM on pins associated with the timer are disabled when  the first servo is attached.
40  Timers are seized as needed in groups of 12 servos  - 24 servos use two timers, 48 servos will use four.
41  The sequence used to sieze  timers is defined in timers.h
42
43  The methods are:
44
45   VarSpeedServo  - Class for manipulating servo motors connected to Arduino pins.
46
47   attach(pin  )  - Attaches a servo motor to an i/o pin.
48   attach(pin, min, max  ) - Attaches  to a pin setting min and max values in microseconds
49   default min is 544, max  is 2400  
50 
51   write(value)     - Sets the servo angle in degrees.  (invalid  angle that is valid as pulse in microseconds is treated as microseconds)
52   write(value,  speed) - speed varies the speed of the move to new position 0=full speed, 1-255  slower to faster
53   write(value, speed, wait) - wait is a boolean that, if true,  causes the function call to block until move is complete
54
55   writeMicroseconds()  - Sets the servo pulse width in microseconds 
56   read()      - Gets the last  written servo pulse width as an angle between 0 and 180. 
57   readMicroseconds()  - Gets the last written servo pulse width in microseconds. (was read_us() in first  release)
58   attached()  - Returns true if there is a servo attached. 
59   detach()    - Stops an attached servos from pulsing its i/o pin. 
60
61   slowmove(value,  speed) - The same as write(value, speed), retained for compatibility with Korman's  version
62
63   stop() - stops the servo at the current position
64
65   sequencePlay(sequence,  sequencePositions); // play a looping sequence starting at position 0
66   sequencePlay(sequence,  sequencePositions, loop, startPosition); // play sequence with number of positions,  loop if true, start at position
67   sequenceStop(); // stop sequence at current  position
68
69 */
70
71#include <avr/interrupt.h>
72#include <Arduino.h> //  updated from WProgram.h to Arduino.h for Arduino 1.0+, pva
73
74#include "VarSpeedServo.h"
75
76#define  usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds  to tick (assumes prescale of 8)  // 12 Aug 2009
77#define ticksToUs(_ticks) ((  (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back  to microseconds
78
79
80#define TRIM_DURATION       2                               //  compensation ticks to trim adjust for digitalWrite delays // 12 August 2009
81
82//#define  NBR_TIMERS        (MAX_SERVOS / SERVOS_PER_TIMER)
83
84static servo_t servos[MAX_SERVOS];                          // static array of servo structures
85static volatile  int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed  for each timer (or -1 if refresh interval)
86
87uint8_t ServoCount = 0;                                     //  the total number of attached servos
88
89// sequence vars
90
91servoSequencePoint  initSeq[] = {{0,100},{45,100}};
92
93//sequence_t sequences[MAX_SEQUENCE];
94
95//  convenience macros
96#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr  / SERVOS_PER_TIMER)) // returns the timer controlling this servo
97#define SERVO_INDEX_TO_CHANNEL(_servo_nbr)  (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this  timer
98#define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
99#define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class  by timer and channel
100
101#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
102#define SERVO_MAX() (MAX_PULSE_WIDTH  - this->max * 4)  // maximum value in uS for this servo 
103
104/************ static  functions common to all instances ***********************/
105
106static inline  void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile  uint16_t* OCRnA)
107{
108  if( Channel[timer] < 0 )
109    *TCNTn = 0; // channel  set to -1 indicated that refresh interval completed so reset the timer 
110  else{
111    if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive  == true )  
112      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); //  pulse this channel low if activated   
113  }
114
115  Channel[timer]++;    // increment  to the next channel
116  if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer]  < SERVOS_PER_TIMER) {
117
118	// Extension for slowmove
119	if (SERVO(timer,Channel[timer]).speed)  {
120		// Increment ticks by speed until we reach the target.
121		// When the  target is reached, speed is set to 0 to disable that code.
122		if (SERVO(timer,Channel[timer]).target  > SERVO(timer,Channel[timer]).ticks) {
123			SERVO(timer,Channel[timer]).ticks  += SERVO(timer,Channel[timer]).speed;
124			if (SERVO(timer,Channel[timer]).target  <= SERVO(timer,Channel[timer]).ticks) {
125				SERVO(timer,Channel[timer]).ticks  = SERVO(timer,Channel[timer]).target;
126				SERVO(timer,Channel[timer]).speed  = 0;
127			}
128		}
129		else {
130			SERVO(timer,Channel[timer]).ticks  -= SERVO(timer,Channel[timer]).speed;
131			if (SERVO(timer,Channel[timer]).target  >= SERVO(timer,Channel[timer]).ticks) {
132				SERVO(timer,Channel[timer]).ticks  = SERVO(timer,Channel[timer]).target;
133				SERVO(timer,Channel[timer]).speed  = 0;
134			}
135		}
136	}
137	// End of Extension for slowmove
138
139	//  Todo
140	
141    *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
142    if(SERVO(timer,Channel[timer]).Pin.isActive  == true)     // check if activated
143      digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH);  // its an active channel so pulse it high   
144  }  
145  else { 
146    // finished  all channels so wait for the refresh period to expire before starting over 
147    if( (unsigned)*TCNTn <  (usToTicks(REFRESH_INTERVAL) + 4) )  // allow a few  ticks to ensure the next OCR1A not missed
148      *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);  
149    else 
150      *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has  elapsed
151    Channel[timer] = -1; // this will get incremented at the end of the  refresh period to start again at the first channel
152  }
153}
154
155#ifndef WIRING  // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring  platform
156// Interrupt handlers for Arduino 
157#if defined(_useTimer1)
158SIGNAL  (TIMER1_COMPA_vect) 
159{ 
160  handle_interrupts(_timer1, &TCNT1, &OCR1A); 
161}
162#endif
163
164#if  defined(_useTimer3)
165SIGNAL (TIMER3_COMPA_vect) 
166{ 
167  handle_interrupts(_timer3,  &TCNT3, &OCR3A); 
168}
169#endif
170
171#if defined(_useTimer4)
172SIGNAL (TIMER4_COMPA_vect)  
173{
174  handle_interrupts(_timer4, &TCNT4, &OCR4A); 
175}
176#endif
177
178#if  defined(_useTimer5)
179SIGNAL (TIMER5_COMPA_vect) 
180{
181  handle_interrupts(_timer5,  &TCNT5, &OCR5A); 
182}
183#endif
184
185#elif defined WIRING
186// Interrupt handlers  for Wiring 
187#if defined(_useTimer1)
188void Timer1Service() 
189{ 
190  handle_interrupts(_timer1,  &TCNT1, &OCR1A); 
191}
192#endif
193#if defined(_useTimer3)
194void Timer3Service()  
195{ 
196  handle_interrupts(_timer3, &TCNT3, &OCR3A); 
197}
198#endif
199#endif
200
201
202static  void initISR(timer16_Sequence_t timer)
203{  
204#if defined (_useTimer1)
205  if(timer  == _timer1) {
206    TCCR1A = 0;             // normal counting mode 
207    TCCR1B  = _BV(CS11);     // set prescaler of 8 
208    TCNT1 = 0;              // clear  the timer count 
209#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
210    TIFR |= _BV(OCF1A);      // clear any pending interrupts; 
211    TIMSK |=  _BV(OCIE1A)  ;  // enable the output compare interrupt  
212#else
213    // here if not ATmega8  or ATmega128
214    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts; 
215    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt 
216#endif    
217#if  defined(WIRING)       
218    timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);  
219#endif	
220  } 
221#endif  
222
223#if defined (_useTimer3)
224  if(timer ==  _timer3) {
225    TCCR3A = 0;             // normal counting mode 
226    TCCR3B  = _BV(CS31);     // set prescaler of 8  
227    TCNT3 = 0;              // clear  the timer count 
228#if defined(__AVR_ATmega128__)
229    TIFR |= _BV(OCF3A);     //  clear any pending interrupts;   
230	ETIMSK |= _BV(OCIE3A);  // enable the output  compare interrupt     
231#else  
232    TIFR3 = _BV(OCF3A);     // clear any pending  interrupts; 
233    TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt      
234#endif
235#if defined(WIRING)    
236    timerAttach(TIMER3OUTCOMPAREA_INT,  Timer3Service);  // for Wiring platform only	
237#endif  
238  }
239#endif
240
241#if  defined (_useTimer4)
242  if(timer == _timer4) {
243    TCCR4A = 0;             //  normal counting mode 
244    TCCR4B = _BV(CS41);     // set prescaler of 8  
245    TCNT4 = 0;              // clear the timer count 
246    TIFR4 = _BV(OCF4A);     // clear any pending interrupts; 
247    TIMSK4 =  _BV(OCIE4A) ; // enable  the output compare interrupt
248  }    
249#endif
250
251#if defined (_useTimer5)
252  if(timer == _timer5) {
253    TCCR5A = 0;             // normal counting mode  
254    TCCR5B = _BV(CS51);     // set prescaler of 8  
255    TCNT5 = 0;              //  clear the timer count 
256    TIFR5 = _BV(OCF5A);     // clear any pending interrupts;  
257    TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt      
258  }
259#endif
260} 
261
262static void finISR(timer16_Sequence_t timer)
263{
264    //disable use of the given timer
265#if defined WIRING   // Wiring
266  if(timer  == _timer1) {
267    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
268    TIMSK1 &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
269    #else  
270    TIMSK &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt   
271    #endif
272    timerDetach(TIMER1OUTCOMPAREA_INT); 
273  }
274  else if(timer  == _timer3) {     
275    #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
276    TIMSK3 &= ~_BV(OCIE3A);    // disable the timer3 output compare A interrupt
277    #else
278    ETIMSK &= ~_BV(OCIE3A);    // disable the timer3 output compare  A interrupt
279    #endif
280    timerDetach(TIMER3OUTCOMPAREA_INT);
281  }
282#else
283    //For arduino - in future: call here to a currently undefined function to reset  the timer
284#endif
285}
286
287static boolean isTimerActive(timer16_Sequence_t  timer)
288{
289  // returns true if any servo is active on this timer
290  for(uint8_t  channel=0; channel < SERVOS_PER_TIMER; channel++) {
291    if(SERVO(timer,channel).Pin.isActive  == true)
292      return true;
293  }
294  return false;
295}
296
297
298/******************  end of static functions ******************************/
299
300VarSpeedServo::VarSpeedServo()
301{
302  if( ServoCount < MAX_SERVOS) {
303    this->servoIndex = ServoCount++;                    //  assign a servo index to this instance
304	  servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values  - 12 Aug 2009
305    this->curSeqPosition = 0;
306    this->curSequence  = initSeq;
307  }
308  else
309    this->servoIndex = INVALID_SERVO ;  // too many  servos 
310}
311
312uint8_t VarSpeedServo::attach(int pin)
313{
314  return this->attach(pin,  MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
315}
316
317uint8_t VarSpeedServo::attach(int  pin, int min, int max)
318{
319  if(this->servoIndex < MAX_SERVOS ) {
320    pinMode(  pin, OUTPUT) ;                                   // set servo pin to output
321    servos[this->servoIndex].Pin.nbr = pin;  
322    // todo min/max check: abs(min  - MIN_PULSE_WIDTH) /4 < 128 
323    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution  of min/max is 4 uS
324    this->max  = (MAX_PULSE_WIDTH - max)/4; 
325    // initialize  the timer if it has not already been initialized 
326    timer16_Sequence_t timer  = SERVO_INDEX_TO_TIMER(servoIndex);
327    if(isTimerActive(timer) == false)
328      initISR(timer);    
329    servos[this->servoIndex].Pin.isActive = true;  //  this must be set after the check for isTimerActive
330  } 
331  return this->servoIndex  ;
332}
333
334void VarSpeedServo::detach()  
335{
336  servos[this->servoIndex].Pin.isActive  = false;  
337  timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
338  if(isTimerActive(timer) == false) {
339    finISR(timer);
340  }
341}
342
343void  VarSpeedServo::write(int value)
344{  
345  if(value < MIN_PULSE_WIDTH)
346  {  //  treat values less than 544 as angles in degrees (valid values in microseconds are  handled as microseconds)
347    // updated to use constrain() instead of if(), pva
348    value = constrain(value, 0, 180);
349    value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());      
350  }
351  this->writeMicroseconds(value);
352}
353
354void  VarSpeedServo::writeMicroseconds(int value)
355{
356  // calculate and store the  values for the given channel
357  byte channel = this->servoIndex;
358  if( (channel  >= 0) && (channel < MAX_SERVOS) )   // ensure channel is valid
359  {  
360    if(  value < SERVO_MIN() )          // ensure pulse width is valid
361      value = SERVO_MIN();
362    else if( value > SERVO_MAX() )
363      value = SERVO_MAX();   
364    
365  	value -= TRIM_DURATION;
366    value = usToTicks(value);  // convert to ticks  after compensating for interrupt overhead - 12 Aug 2009
367
368    uint8_t oldSREG  = SREG;
369    cli();
370    servos[channel].ticks = value;  
371    SREG = oldSREG;   
372
373	// Extension for slowmove
374	// Disable slowmove logic.
375	servos[channel].speed  = 0;  
376	// End of Extension for slowmove
377  } 
378}
379
380// Extension for  slowmove
381/*
382  write(value, speed) - Just like write but at reduced speed.
383
384  value - Target position for the servo. Identical use as value of the function  write.
385  speed - Speed at which to move the servo.
386          speed=0 - Full  speed, identical to write
387          speed=1 - Minimum speed
388          speed=255  - Maximum speed
389*/
390void VarSpeedServo::write(int value, uint8_t speed) {
391	//  This fuction is a copy of write and writeMicroseconds but value will be saved
392	//  in target instead of in ticks in the servo structure and speed will be save
393	//  there too.
394
395  int degrees = value;
396
397	if (speed) {
398		if (value  < MIN_PULSE_WIDTH) {
399			// treat values less than 544 as angles in degrees  (valid values in microseconds are handled as microseconds)
400      		// updated  to use constrain instead of if, pva
401      		value = constrain(value, 0, 180);
402      		value = map(value, 0, 180, SERVO_MIN(),  SERVO_MAX());    
403		}
404		//  calculate and store the values for the given channel
405		byte channel = this->servoIndex;
406		if(  (channel >= 0) && (channel < MAX_SERVOS) ) {   // ensure channel is valid
407      		//  updated to use constrain instead of if, pva
408      		value = constrain(value,  SERVO_MIN(), SERVO_MAX());
409
410			value = value - TRIM_DURATION;
411			value  = usToTicks(value);  // convert to ticks after compensating for interrupt overhead  - 12 Aug 2009
412
413			// Set speed and direction
414			uint8_t oldSREG =  SREG;
415			cli();
416			servos[channel].target = value;  
417			servos[channel].speed  = speed;  
418			SREG = oldSREG;   
419		}
420	} 
421	else {
422		write  (value);
423	}
424}
425
426void VarSpeedServo::write(int value, uint8_t speed,  bool wait) {
427  write(value, speed);
428  if (wait) { // block until the servo  is at its new position
429    if (value < MIN_PULSE_WIDTH) {
430      while (read()  != value) {
431        delay(5);
432      }
433    } else {
434      while (readMicroseconds()  != value) {
435        delay(5);
436      }
437    }
438  }
439}
440
441void VarSpeedServo::stop()  {
442  write(read());
443}
444
445void VarSpeedServo::slowmove(int value, uint8_t  speed) {
446  // legacy function to support original version of VarSpeedServo
447  write(value, speed);
448}
449
450// End of Extension for slowmove
451
452
453int  VarSpeedServo::read() // return the value as degrees
454{
455  return  map( this->readMicroseconds()+1,  SERVO_MIN(), SERVO_MAX(), 0, 180);     
456}
457
458int VarSpeedServo::readMicroseconds()
459{
460  unsigned int pulsewidth;
461  if( this->servoIndex != INVALID_SERVO )
462    pulsewidth  = ticksToUs(servos[this->servoIndex].ticks)  + TRIM_DURATION ;   // 12 aug 2009
463  else 
464    pulsewidth  = 0;
465
466  return pulsewidth;   
467}
468
469bool  VarSpeedServo::attached()
470{
471  return servos[this->servoIndex].Pin.isActive  ;
472}
473
474uint8_t VarSpeedServo::sequencePlay(servoSequencePoint sequenceIn[],  uint8_t numPositions, bool loop, uint8_t startPos) {
475  uint8_t oldSeqPosition  = this->curSeqPosition;
476
477  if( this->curSequence != sequenceIn) {
478    //Serial.println("newSeq");
479    this->curSequence = sequenceIn;
480    this->curSeqPosition = startPos;
481    oldSeqPosition = 255;
482  }
483  
484  if (read() == sequenceIn[this->curSeqPosition].position  && this->curSeqPosition != CURRENT_SEQUENCE_STOP) {
485    this->curSeqPosition++;
486    
487    if (this->curSeqPosition >= numPositions) { // at the end of the loop
488      if (loop) { // reset to the beginning of the loop
489        this->curSeqPosition  = 0;
490      } else { // stop the loop
491        this->curSeqPosition = CURRENT_SEQUENCE_STOP;
492      }
493    }
494  }
495
496  if (this->curSeqPosition != oldSeqPosition && this->curSeqPosition  != CURRENT_SEQUENCE_STOP) { 
497    // CURRENT_SEQUENCE_STOP position means the  animation has ended, and should no longer be played
498    // otherwise move to  the next position
499    write(sequenceIn[this->curSeqPosition].position, sequenceIn[this->curSeqPosition].speed);
500    //Serial.println(this->seqCurPosition);
501  }
502  
503  return this->curSeqPosition;
504}
505
506uint8_t  VarSpeedServo::sequencePlay(servoSequencePoint sequenceIn[], uint8_t numPositions)  {
507  return sequencePlay(sequenceIn, numPositions, true, 0);
508}
509
510void  VarSpeedServo::sequenceStop() {
511  write(read());
512  this->curSeqPosition =  CURRENT_SEQUENCE_STOP;
513}
514
515/*
516	To do
517int VarSpeedServo::targetPosition()  {
518	byte channel = this->servoIndex;
519	return map( servos[channel].target+1,  SERVO_MIN(), SERVO_MAX(), 0, 180);
520}
521
522int VarSpeedServo::targetPositionMicroseconds()  {
523	byte channel = this->servoIndex;
524	return servos[channel].target;
525}
526
527bool  VarSpeedServo::isMoving() {
528	byte channel = this->servoIndex;
529	int servos[channel].target;
530}
531*/
532