// Ansteuerung von 6 RC-Servomotoren
// ATmega8, 8MHz
// AVR-Studio, Optimierungsgrad: O2
// erstellt am 09/02/2011
// www.alex-handfest.de/avr/avr.html


#include "servo.h"


unsigned int  s_count 	 = 0;
unsigned char s_array[3] = {'\0'};



// --------- Inizialisierung --------

void servo_init (void) {

	//Timer init
	TCCR0 = (0<<CS02) | (0<<CS01) | (1<<CS00);	//Setzen des Prescaler auf 1
	TIMSK |= (1<<TOIE0);
	// Interrupt Aktion alle ( 8000000/1 ) / 256 = 31,250 kHz -> 0,000032s = 32us
	// Impuls für den Servo alle 20ms
	// Impulslänge 1 - 2 ms

	//Ausgänge
	DDRB  = 0xff;    				// PortB: 6 Ausgänge für 6 Servos

}



// --------- Time Overflow alle 32us --------
// muss in der main.c ausgeführt werden: ISR(TIMER0_OVF_vect){ ... }

void servo_TIMER (void) {

	s_count++;

	if (s_count == 625){					//bei 625*32us = 20ms
		s_count = 0;
		PORTB = 0xff;
	}
	if (s_count == 28 + s_array[0]) PORTB &= ~(1<<PC3);	//nach 28*32us = 1ms
	if (s_count == 28 + s_array[1]) PORTB &= ~(1<<PC4);
	if (s_count == 28 + s_array[2]) PORTC &= ~(1<<PC2);
	if (s_count == 28 + s_array[3]) PORTC &= ~(1<<PC3);
	if (s_count == 28 + s_array[4]) PORTC &= ~(1<<PC4);
	if (s_count == 28 + s_array[5]) PORTC &= ~(1<<PC5);
	if (s_count >= 68) PORTB = 0x00; 			//nach 68*32us = 2ms
	//Werte für 1ms und 2ms experimentell bestimmen!!!
	//Jeder Servo reagiert ein wenig anders auf das Signal.
}



// --------- Einlesen und Auswerten der Zahl --------
// Werte für s_stellung: 0-40 !!

void servo_stellung (unsigned char s_servo, unsigned char s_stellung){
	s_array[s_servo] = s_stellung;
}