servo-loskaai

Gebruik van een servo sturing voor het aansturen van een vliegtuigje dat van links naar rechts beweegt tussen 2 vaste punten.

Er wordt gebruikt gemaakt van een atmel2313 chip voor de sturing.

Programmatie met AVR studio4.

Print gebouwd op gaatjesprint.

servo6.c

/*

pb4 -> pin 16

pb3 -> pin 15

pb2 ->

pb1 ->

*/

#include <avr/interrupt.h>

#include <avr/io.h>

#include <util/atomic.h>

#include <inttypes.h>

#define ever (;;)

volatile uint16_t cycles = 0;

volatile uint8_t servo = 0x01;

volatile uint8_t idx = 0;

static const uint16_t slot_len = 2400;

/*

static const uint16_t range_min = 1000;

static const uint16_t range_max = 2000;

*/

static const uint16_t delay = 1*8;

volatile int16_t delta[8] =

{

1, /* not used */

1, 1, /* not used for the moment*/

1, 1,

1, 1, 1 /* not used */

};

volatile uint16_t servo_pos[8] =

{

1500, /* not used */

1500, 1500, /* not used for the moment */

1500, 1500,

1500, 1500, 1500 /* not used */

};

volatile uint16_t range_min[8] =

{

1000, /* not used */

1000, 1000, /* not used for the moment */

1000, 1200,

1000, 1000, 1000 /* not used */

};

volatile uint16_t range_max[8] =

{

2000, /* not used */

2000, 2000, /* not used for the moment */

2000, 1700,

2000, 2000, 2000 /* not used */

};

ISR(TIMER1_COMPA_vect) // Timer/Counter1 Compare Match A

{

PORTB &= ~(servo); // Disable current servo,

servo = (servo << 1) | (servo >> 7); // rotate bitmask

idx = (idx + 1) & 7; // and increment index.

}

ISR(TIMER1_COMPB_vect) // Timer/Counter1 Compare Match B

{

PORTB |= servo; // Activate current servo,

OCR1A = servo_pos[idx]; // load next pwm duty cycle

TCNT1 = 0x0000; // and reset counter;

cycles++;

}

inline static void init_portb

( ) {

PORTB = 0x01; // Port_B[0] = high, Port_B[1..7] = low

DDRB = 0xFF; // Port_B[0..7] = output

}

inline static void init_timer1

( ) {

// Enable both Timer1 Compare Match Interrupts.

TIMSK = (1 << OCIE1A) | (1 << OCIE1B);

// Start with first servo;

OCR1A = servo_pos[idx];

// All servo slots have the same length;

OCR1B = slot_len;

// Timer0/1 Prescaler = F_CPU / 1

TCCR1B = (1 << CS10);

}

int main (void)

{

init_portb();

init_timer1();

sei(); // Enable Interrupts.

for ever

{

uint16_t cycles_snapshot;

ATOMIC_BLOCK(ATOMIC_FORCEON)

{

cycles_snapshot = cycles;

}

if ( cycles_snapshot > delay )

{

uint8_t i;

ATOMIC_BLOCK(ATOMIC_FORCEON)

{

cycles = 0x0000;

}

for ( i = 0; i < 8; i++ )

{

uint16_t pos = servo_pos[i];

uint16_t rng_min = range_min[i];

uint16_t rng_max = range_max[i];

if ( ( ( pos + delta[i] ) > rng_max ) |

( ( pos + delta[i] ) < rng_min ) ) ATOMIC_BLOCK(ATOMIC_FORCEON)

{

delta[i] = -delta[i];

}

else ATOMIC_BLOCK(ATOMIC_FORCEON)

{

servo_pos[i] = pos + delta[i];

}

}

}

}

}

//20ms / 8 = 2.5ms