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 168 static void initISR(timer16_Sequence_t timer) {
169 if(timer == _timer1) {
174 TIMSK1 |= _BV(OCIE1A) ;
176 timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
181 static void finISR(timer16_Sequence_t timer) {
182 #if defined WIRING // Wiring 183 if(timer == _timer1) {
184 TIMSK &= ~_BV(OCIE1A) ;
185 timerDetach(TIMER1OUTCOMPAREA_INT);
187 else if(timer == _timer3) {
188 ETIMSK &= ~_BV(OCIE3A);
189 timerDetach(TIMER3OUTCOMPAREA_INT);
197 static bool isTimerActive(timer16_Sequence_t timer) {
199 for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
200 if(SERVO(timer,channel).Pin.isActive ==
true)
206 static inline void handle_interrupts(timer16_Sequence_t timer,
volatile uint16_t *TCNTn,
volatile uint16_t* OCRnA)
208 if( Channel[timer] < 0 )
211 if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive ==
true )
212 digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW);
216 if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
217 *OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
218 if(SERVO(timer,Channel[timer]).Pin.isActive ==
true)
219 digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH);
223 if( ((
unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL) )
224 *OCRnA = (
unsigned int)usToTicks(REFRESH_INTERVAL);
231 #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform 233 #if defined(_useTimer1) 234 SIGNAL (TIMER1_COMPA_vect)
236 handle_interrupts(_timer1, &TCNT1, &OCR1A);
241 #if defined(_useTimer1) 244 handle_interrupts(_timer1, &TCNT1, &OCR1A);
256 if( ServoCount < MAX_SERVOS) {
258 servos[this->
servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);
265 return this->
attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
270 pinMode( pin, OUTPUT) ;
273 this->min = (MIN_PULSE_WIDTH -
min)/4;
274 this->max = (MAX_PULSE_WIDTH -
max)/4;
276 timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(
servoIndex);
277 if(isTimerActive(timer) ==
false)
285 servos[this->
servoIndex].Pin.isActive =
false;
286 timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(
servoIndex);
287 if(isTimerActive(timer) ==
false) {
293 if(value < MIN_PULSE_WIDTH)
295 if(value < 0) value = 0;
296 if(value > 180) value = 180;
297 value = map(value, 0, 180, SERVO_MIN(), SERVO_MAX());
305 if( (channel < MAX_SERVOS) )
307 if( value < SERVO_MIN() )
309 else if( value > SERVO_MAX() )
312 value = value - TRIM_DURATION;
313 value = usToTicks(value);
315 uint8_t oldSREG = SREG;
317 servos[channel].ticks = value;
324 unsigned int pulsewidth;
326 pulsewidth = ticksToUs(servos[this->
servoIndex].ticks) + TRIM_DURATION ;
334 return servos[this->
servoIndex].Pin.isActive ;
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