ScOe
Advanced Member level 4
servo avr
Hi,
i've found one lib for controlling RC servo motors, but i get some strange issues while using it .. servo motor is rotating fine but sometime it get "stuck" between pulses or something. It behave like it get stalled.
Here is the lib ..
Regards,
Sven
Hi,
i've found one lib for controlling RC servo motors, but i get some strange issues while using it .. servo motor is rotating fine but sometime it get "stuck" between pulses or something. It behave like it get stalled.
Here is the lib ..
Code:
#ifndef GUARD_SERVO_H
#define GUARD_SERVO_H
//-------------------------------------------------------------------
//#include "globaldefs.h"
//-------------------------------------------------------------------
#ifndef SERVO_INIT
#define SERVO_INIT
#define SERVO_PORT PORTB
#define SERVO_DDR DDRB
#define N_SERVOS 2
#define SERVO_FRAME 18000
#define SERVO_TIME_DIV (SERVO_FRAME / N_SERVOS)
#define US2TIMER1(us) ((us) * (uint16_t)(F_CPU / 1E6))
// Servo times (this is Futaba timing).
#define SERVO_MIN 1000 // µs
#define SERVO_MAX 2000 // µs
#define SERVO_MID (SERVO_MIN + SERVO_MAX) / 2
#define TRUE 1
#warning "No global defs"
const static uint8_t servoOutMask[N_SERVOS] = {
0b00000001, // PX0
0b00000010, // PX1
};
#endif
//-------------------------------------------------------------------
volatile uint16_t servoTime[N_SERVOS];
void servoSet(uint8_t servo, uint16_t time /* µs */);
void servoTest (void);
void ServoInit(void);
//-------------------------------------------------------------------
void ServoInit (void)
{
int i;
for(i = 0; i < N_SERVOS; i++) {
servoTime[i] = US2TIMER1(SERVO_MID);
}
SERVO_DDR |= _BV(PIN0);
SERVO_DDR |= _BV(PIN1);
// Setupt a first compare match
OCR1A = TCNT1 + US2TIMER1(100);
// start timer 1 with no prescaler
TCCR1B = (1 << CS10);
// Enable interrupt
TIMSK |= (1 << OCIE1A);
}
//-------------------------------------------------------------------
void servoSet(uint8_t servo, uint16_t time /* µs */)
{
uint16_t ticks = US2TIMER1(time);
cli();
servoTime[servo] = ticks;
sei();
}
//-------------------------------------------------------------------
void servoTest (void)
{
uint16_t time;
uint8_t w;
// Test update of servo 0
for(time = SERVO_MIN; time <= SERVO_MAX; time += 50) {
servoSet(0, time);
servoSet(1, time);
for(w = 0; w < 25; w++) {
_delay_ms(10);
}
}
}
//-------------------------------------------------------------------
ISR(TIMER1_COMPA_vect)
{
static uint16_t nextStart;
static uint8_t servo;
static short outputHigh = TRUE;
uint16_t currentTime = OCR1A;
uint8_t mask = servoOutMask[servo];
if (outputHigh) {
SERVO_PORT |= mask;
// Set the end time for the servo pulse
OCR1A = currentTime + servoTime[servo];
nextStart = currentTime + US2TIMER1(SERVO_TIME_DIV);
} else {
SERVO_PORT &= ~mask;
if (++servo == N_SERVOS) {
servo = 0;
}
OCR1A = nextStart;
}
outputHigh = !outputHigh;
}
//-------------------------------------------------------------------
#endif
Regards,
Sven