1 #ifndef EAGLE_EBOARD_HELPLIB_SERVO 2 #define EAGLE_EBOARD_HELPLIB_SERVO 22 typedef enum { _timer1, _Nbr_16timers } timer16_Sequence_t;
23 #define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo 24 #define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo 25 #define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached 26 #define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds 28 #define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer 29 #define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER) 31 #define INVALID_SERVO 255 // flag indicating an invalid servo index 60 volatile unsigned int ticks;
108 void write(
int value);
143 #define usToTicks(_us) (( clockCyclesPerMicrosecond()* _us) / 8) // converts microseconds to tick (assumes prescale of 8) // 12 Aug 2009 144 #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds 147 #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays // 12 August 2009 151 static servo_t servos[MAX_SERVOS];
152 static volatile int8_t Channel[_Nbr_16timers ];
154 uint8_t ServoCount = 0;
157 #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo 158 #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer 159 #define SERVO_INDEX(_timer,_channel) ((_timer*SERVOS_PER_TIMER) + _channel) // macro to access servo index by timer and channel 160 #define SERVO(_timer,_channel) (servos[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel 162 #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4) // minimum value in uS for this servo 163 #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4) // maximum value in uS for this servo 165 static inline void handle_interrupts(timer16_Sequence_t timer,
volatile uint16_t *TCNTn,
volatile uint16_t* OCRnA)
167 if( Channel[timer] < 0 )
170 if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive ==
true )
171 digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW);
175 if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
176 *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
177 if(SERVO(timer,Channel[timer]).Pin.isActive ==
true)
178 digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH);
182 if( ((
unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) )
183 *OCRnA = (
unsigned int)usToTicks(REFRESH_INTERVAL);
190 #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform 192 #if defined(_useTimer1) 193 SIGNAL (TIMER1_COMPA_vect)
195 handle_interrupts(_timer1, &TCNT1, &OCR1A);
200 #if defined(_useTimer1) 203 handle_interrupts(_timer1, &TCNT1, &OCR1A);
215 if( ServoCount < MAX_SERVOS) {
217 servos[this->
servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);
224 return this->
attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
229 pinMode( pin, OUTPUT) ;
232 this->min = (MIN_PULSE_WIDTH -
min)/4;
233 this->max = (MAX_PULSE_WIDTH -
max)/4;
235 timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(
servoIndex);
236 if(isTimerActive(timer) ==
false)
244 servos[this->
servoIndex].Pin.isActive =
false;
245 timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(
servoIndex);
246 if(isTimerActive(timer) ==
false) {
252 if(value < MIN_PULSE_WIDTH)
254 if(value < 0) value = 0;
255 if(value > 180) value = 180;
256 value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
264 if( (channel < MAX_SERVOS) )
266 if( value < SERVO_MIN() )
268 else if( value > SERVO_MAX() )
271 value = value - TRIM_DURATION;
272 value = usToTicks(value);
274 uint8_t oldSREG = SREG;
276 servos[channel].ticks = value;
283 unsigned int pulsewidth;
285 pulsewidth = ticksToUs(servos[this->
servoIndex].ticks) + TRIM_DURATION ;
293 return servos[this->
servoIndex].Pin.isActive ;
298 static void initISR(timer16_Sequence_t timer) {
299 if(timer == _timer1) {
304 TIMSK1 |= _BV(OCIE1A) ;
306 timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
311 static void finISR(timer16_Sequence_t timer) {
312 #if defined WIRING // Wiring 313 if(timer == _timer1) {
314 TIMSK &= ~_BV(OCIE1A) ;
315 timerDetach(TIMER1OUTCOMPAREA_INT);
317 else if(timer == _timer3) {
318 ETIMSK &= ~_BV(OCIE3A);
319 timerDetach(TIMER3OUTCOMPAREA_INT);
327 static bool isTimerActive(timer16_Sequence_t timer) {
329 for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
330 if(SERVO(timer,channel).Pin.isActive ==
true)
void write(int value)
sets angle or pulse width of the Servo
bool attached()
check attached status
int read()
reads the current pulse width as angle
int8_t max
maximum is this value times 4 added to MAX_PULSE_WIDTH
void detach()
detaches the connected pin
uint8_t attach(int pin)
this will tell the Servo-instance which pin the data cable is connected to
void writeMicroseconds(int value)
sets pulse width of the Servo
uint8_t servoIndex
the channel index this servo is using
int readMicroseconds()
reads the current pulse width
int8_t min
minimum is this value times 4 added to MIN_PULSE_WIDTH
[NANO] The standard Arduino Servo Class