8-channel servo controller

file: http://darcs.erazor-zone.de/avr/avr-gcc/servo-ctl/firmware.c

#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <math.h>
 
#define FIRMWARE_VERSION "0.2"
 
#define CHANNELS 3
 
#define T0_20ms 77
#define T0_SIZE 256
#define T1_SIZE 65536
 
unsigned char current_channel=0;								//current channel
unsigned int channel_timer_value[CHANNELS];						//channel timer-value
 
unsigned char channel_value[CHANNELS]		={72,125,250};		//channel value
unsigned char channel_value_multiplier[CHANNELS]={5,6,4};
unsigned char channel_offset[CHANNELS]		={55,40,125};	
 
SIGNAL (SIG_OVERFLOW0) {
    TCNT0=T0_SIZE-T0_20ms;
 
    //start timer1, prescaler==1
    TCCR1B=(1<<CS10);
    //SIG_OVERFLOW1 follows for current_channel==0
}
 
SIGNAL (SIG_OVERFLOW1) {
    TCNT1=channel_timer_value[current_channel];
 
    if (current_channel==CHANNELS) {
	TCCR1B=0;
	TCNT1=T1_SIZE-1;
	current_channel=0;
	PORTD=0;
    } else {
	PORTD=1<<current_channel;
	current_channel++;
    }
}
 
void calculate_channel_value(unsigned char channel) {
    //65536-(500+offset*4+value*factor)
 
    channel_timer_value[channel]=T1_SIZE-
		    (500+channel_offset[channel]*4+
		     channel_value[channel]*channel_value_multiplier[channel]);
}
 
void init(void) {
    //1ms<=t<=2ms
    //1000<=t<=2000
    //setup 3 servo channels
    calculate_channel_value(0);
    calculate_channel_value(1);
    calculate_channel_value(2);
 
 
    //setup output port
    DDRD=255;
    DDRC=(1<<PC2);
 
    PORTC=(1<<PC2);
    //PORTD=0;
 
 
    //10^3/(10^6 / 256 /77) == 19.71ms
    //we need 20ms
    TCNT0=T0_SIZE-T0_20ms;
 
    //enable timer0 int & timer1 int
    TIMSK=(1<<TOIE1)|(1<<TOIE0);
 
    //enable all int's
    sei();
 
    //generate a overflow in timer1
    TCNT1=T1_SIZE-1;
 
    //start timer0, prescaler==256
    TCCR0=(1<<CS02);
 
    //setup ADC
    ADCSRA=(1<<ADEN)|(1<<ADFR)|(1<<ADSC);
    ADMUX=(1<<ADLAR);
 
}
 
int main(void) {
    unsigned char w;
    double x,y;
    init();
    /* 000D.XXXX
 
	D- Direction (1==read, 0==write)
 
	XXXX:
	    0x00	nop
	    0x1n	channel_value n
	    0x2n	channel_value_multiplier n
	    0x3n	channel_offset
	    0xfn	ADC
    */
    while(1) {
		for (w=0;w<255;w++) {
			x=w*M_PI/2/255;
			y=255*sin(x);
			channel_value[0]=(unsigned char)y;
			//channel_value[0]=ADCH;
			//channel_value[0]=w/2;
 
			calculate_channel_value(0);
		}
		for (w=0;w<254;w+=2) {
			x=w*M_PI/2/255;
			y=255*sin(x);
			channel_value[1]=(unsigned char)y;	
			calculate_channel_value(1);
		}
 
		for (w=255;w>0;w--) {
			x=w*M_PI/2/255;
			y=255*sin(x);
			channel_value[0]=(unsigned char)y;	
			calculate_channel_value(0);
		}
		for (w=254;w>0;w-=2) {
			x=w*M_PI/2/255;
			y=255*sin(x);
			channel_value[1]=(unsigned char)y;
			calculate_channel_value(1);
		}
    }
}