atmega 16 servo

#define F_CPU 8000000UL

// Wijzigen naar behoefte
#define SERVO1 PC6
#define SERVO2 PC7
#define SERVOPORT PORTC
#define SERVODDR DDRC
#define SERVO_MAX 2048
#define SERVO_MIN 1024
#define SERVO_PAUSE 10240

#define TOETSPORT PORTC
#define TOETSPIN PINC

#include <avr/interrupt.h>
#include <avr/io.h>
//#include <avr/sleep.h>
#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <util/delay.h>
#include <compat/deprecated.h>

volatile unsigned int servo1 = 1536, servo2 = 1536;
volatile unsigned char fase = 0;

// De timer1 interrupt-routine
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
    switch (fase) {
    case 0:
	cbi(SERVOPORT, SERVO2);		       // Eind puls voor servo2
	OCR1A = SERVO_PAUSE - servo2;	       // Wachttijd tussen servo-pulsen
	break;
    case 1:
	sbi(SERVOPORT, SERVO1);		       // Begin puls voor servo1
	OCR1A = servo1;			       // Wachttijd servo1-puls
	break;
    case 2:
	cbi(SERVOPORT, SERVO1);		       // Eind puls voor servo1
	OCR1A = SERVO_PAUSE - servo1;	       // Wachttijd tussen servo-pulsen
	break;
    case 3:
	sbi(SERVOPORT, SERVO2);		       // Begin puls voor servo2
	OCR1A = servo1;			       // Wachttijd servo2-puls
	break;
    default:
	break;
    }
    fase++;
    fase &= 0x03;
    TCNT1 = 0;				       // Counter terug op 0 zetten
}


void init(void)
{
    sbi(SERVODDR, SERVO1);		       // SERVO1 = uitgang
    cbi(SERVOPORT, SERVO1);		       // die we onmiddelijk laag maken
    sbi(SERVODDR, SERVO2);		       // Idem voor SERVO2
    cbi(SERVOPORT, SERVO2);
    TOETSPORT |= 0x0F;			       // Interne pull-ups inschakelen 
    TCCR1A = 0;				       // Timer1 in CTC-mode
    TCNT1 = 0;				       // Counter op 0 zetten
    OCR1A = SERVO_PAUSE;		       // Wachttijd tussen servo-pulsen
    TIMSK |= _BV(OCIE1A);		       // Enable Timer1-interrupts
    TCCR1B = _BV(CS11) | _BV(WGM12);	       // Gebruik de 256-prescaler
    sei();				       // Globale interrupt-vlag aan
}



int main(void)
{
    unsigned char toetsen;

    init();

    while (1) {
	switch (TOETSPIN & 0x0F) {
	case 0x07:
	    servo1++;
	    if (servo1 > SERVO_MAX) {
		servo1 = SERVO_MAX;
	    }
	    break;
	case 0x0B:
	    servo1--;
	    if (servo1 < SERVO_MIN) {
		servo1 = SERVO_MIN;
	    }
	    break;
	case 0x0D:
	    servo2++;
	    if (servo2 > SERVO_MAX) {
		servo2 = SERVO_MAX;
	    }
	    break;
	case 0x0E:
	    servo2--;
	    if (servo2 < SERVO_MIN) {
		servo2 = SERVO_MIN;
	    }
	    break;
	default:
	    break;
	}
	_delay_ms(10);
    }
}

#define F_CPU 8000000UL

// Wijzigen naar behoefte
#define POOT1 PB1
#define POOT2 PB2
#define POOT3 PB3	 
#define POOT4 PB4
#define SERVOPORT PORTB
#define SERVODDR DDRB
#define SERVO_MAX 2048 
#define SERVO_MIN 1024 
#define SERVO_PAUSE 10240

#define TOETSPORT PORTA
#define TOETSPIN PINA

#include <avr/interrupt.h>
#include <avr/io.h>
//#include <avr/sleep.h>
#include <inttypes.h>
#include <stdio.h>
#include <stdlib.h>
#include <util/delay.h>
#include <compat/deprecated.h>

volatile unsigned int poot1 = 1536, poot2 = 1536, poot3 = 1536, poot4 = 1536;
volatile unsigned char fase = 0;

int x=0;
char keuze, keuze2;

// De timer1 interrupt-routine
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
    switch (fase) {
    case 0:
	cbi(SERVOPORT, POOT2);		       // Eind puls voor servo2
	OCR1A = SERVO_PAUSE - poot2;	   // Wachttijd tussen servo-pulsen
	break;

    case 1:
	sbi(SERVOPORT, POOT1);		       // Begin puls voor servo1
	OCR1A = poot1;			           // Wachttijd servo1-puls
	break;

    case 2:
	cbi(SERVOPORT, POOT1);		       // Eind puls voor servo1
	OCR1A = SERVO_PAUSE - poot1;	   // Wachttijd tussen servo-pulsen
	break;

    case 3:
	sbi(SERVOPORT, POOT2);		        // Begin puls voor servo2
	OCR1A = poot2;			       		// Wachttijd servo2-puls
	break;

	case 4:
	cbi(SERVOPORT, POOT4);		       // Eind puls voor servo2
	OCR1A = SERVO_PAUSE - poot4;	   // Wachttijd tussen servo-pulsen
	break;

	case 5:
	sbi(SERVOPORT, POOT3);		       // Begin puls voor servo1
	OCR1A = poot3;
	break;

	case 6:
	cbi(SERVOPORT, POOT3);		       // Eind puls voor servo1
	OCR1A = SERVO_PAUSE - poot3;	   // Wachttijd tussen servo-pulsen
	break;

	case 7:
	sbi(SERVOPORT, POOT4);		        // Begin puls voor servo2
	OCR1A = poot4;	
	break;

    default:
	break;
    }
    fase++;
    fase &= 0x07;
    TCNT1 = 0;				       // Counter terug op 0 zetten
}


void init(void)
{
    sbi(SERVODDR, POOT1);		       // SERVO1 = uitgang
    cbi(SERVOPORT, POOT1);		       // die we onmiddelijk laag maken
    sbi(SERVODDR, POOT2);		       // Idem voor SERVO2
    cbi(SERVOPORT, POOT2);
	sbi(SERVODDR, POOT3);		       // SERVO3 = uitgang
    cbi(SERVOPORT, POOT3);		       // die we onmiddelijk laag maken
    sbi(SERVODDR, POOT4);		       // Idem voor SERVO4
    cbi(SERVOPORT, POOT4);
    TOETSPORT |= 0x0F;			       // Interne pull-ups inschakelen 
    TCCR1A = 0;				       // Timer1 in CTC-mode
    TCNT1 = 0;				       // Counter op 0 zetten
    OCR1A = SERVO_PAUSE;		       // Wachttijd tussen servo-pulsen
    TIMSK |= _BV(OCIE1A);		       // Enable Timer1-interrupts
    TCCR1B = _BV(CS11) | _BV(WGM12);	       // Gebruik de 256-prescaler
    sei();				       // Globale interrupt-vlag aan
}



int main(void)
{
    //unsigned char toetsen;

    init();
}