Hallo,

also ich habe für diese Programm eigentlich nur die Funktion initRP6 Controll etwas abgeändert:

Code:
void initRP6Control(void)
{
	portInit();		// Setup port directions and initial values.
					// This is the most important step!

	cli();			// Disable global interrupts.

	// UART:
	UBRRH = UBRR_BAUD_LOW >> 8;	// Setup UART: Baud is Low Speed
	UBRRL = (uint8_t) UBRR_BAUD_LOW;
	UCSRA = 0x00;
    UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0);
    UCSRB = (1 << TXEN) | (1 << RXEN) | (1 << RXCIE);
	
	// Initialize ADC:
	ADMUX = 0; //external reference 
	ADCSRA = (0<<ADIE) | (0<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADIF);
	SFIOR = 0;

	// Initialize External interrupts - all disabled:
	MCUCR = (1 << ISC11) | (1 << ISC10) | (1 << ISC01) | (1 << ISC00);
	GICR = (0 << INT2) | (0 << INT1) | (0 << INT0);
	MCUCSR = (0 << ISC2);
	
	
	// 10kHz Timer 0:
	TCCR0 =   (0 << WGM00) 
			| (1 << WGM01) 
			| (0 << COM00) 
			| (0 << COM01) 
			| (0 << CS02)  
			| (1 << CS01) 
			| (0 << CS00);
	OCR0  = 199;
	

	//Timer 1 is free for your application!
	
	//alle 20ms auslösen auf COMPA
	TCCR1A=   (0 << COM1A1)
			| (0 << COM1A0)
			| (0 << COM1B1)
			| (0 << COM1B0)
			| (0 << FOC1A)
			| (0 << FOC1B)
			| (0 << WGM11)
			| (0 << WGM10);
			
	TCCR1B=   (0 << ICNC1)
			| (0 << ICES1)
			| (0 << WGM13)
			| (0 << WGM12)
			| (1 << CS12)	//Prescaller = 256
			| (0 << CS11)
			| (0 << CS10);
	
	OCR1A = 625;
	TCNT1 = 0;			//Reset

	// Timer 2 - used for beeper:
	TCCR2 =  0; 
	OCR2  = 0xFF; 

	// Enable timer interrupts:
	TIMSK =   (1 << OCIE0)
			| (1 << OCIE1A); 

	// SPI Master (SPI Mode 0, SCK Frequency is F_CPU/2, which means it is 8MHz 
	// on the RP6 CONTROL M32...):
	SPCR =    (0<<SPIE) 
			| (1<<SPE) 
			| (1<<MSTR) 
			| (0<<SPR0) 
			| (0<<SPR1) 
			| (0<<CPOL) 
			| (0<<CPHA);  
	SPSR = (1<<SPI2X);
	
	sei(); // Enable Global Interrupts
}
Dieser Code steht in der Library, hier ist lediglich die Einstellung für Timer 1 und das Interrupt hinzgefügt.

Im meinem "richtigen Programm" ^^ habe ich ein Unterprogramm geschrieben:

Code:
ISR(TIMER1_COMPA_vect)
{
uint16_t Achse1;
uint16_t Achse2;

	TCNT1 = 0;		//reset
	
	DDRC |= IO_PC5;
	DDRC |= IO_PC3;

	Achse1=Greifer_Achse1*10+250;
	Achse2=Greifer_Achse2*10+250;

	PORTC |= IO_PC5;
	delay_us(Achse1);
	PORTC &= ~IO_PC5;

	PORTC |= IO_PC3;
	delay_us(Achse2);
	PORTC &= ~IO_PC3;
}
es gibt zusätzlich noch 2 Globale Variablen (Greifer_Achse1, und Greifer Achse2 vom Typ uint16_t) hier steht der jeweilige Winkelwert des servos, also für Mittelposition z.b 90. Mit 4 Tastern, die bereits auf der M32 Controll vorhanden sind, wird nun diese Variable verändert:

Also hier nochmal mein ganzes Hauptprogramm:
Code:
#include "RP6ControlLib.h" 

//Globale Variablen:
uint16_t Greifer_Achse1=90;
uint16_t Greifer_Achse2=150;

//*** Unterprogramme ***

ISR(TIMER1_COMPA_vect)
{
uint16_t Achse1;
uint16_t Achse2;

	TCNT1 = 0;		//reset
	
	DDRC |= IO_PC5;
	DDRC |= IO_PC3;

	Achse1=Greifer_Achse1*10+250;
	Achse2=Greifer_Achse2*10+250;

	PORTC |= IO_PC5;
	delay_us(Achse1);
	PORTC &= ~IO_PC5;

	PORTC |= IO_PC3;
	delay_us(Achse2);
	PORTC &= ~IO_PC3;
}

void displayAktualisieren()
{
	clearPosLCD(3,10,3);
	setCursorPosLCD(3,10);
	writeIntegerLCD(Greifer_Achse1,DEC);
	clearPosLCD(4,10,3);
	setCursorPosLCD(4,10);
	writeIntegerLCD(Greifer_Achse2,DEC);
}


//*** Hauptprogramm ***
int main(void)
{

uint16_t keys;

	initRP6Control();
	initLCD();
	
	clearLCD();
	_showScreenLCD_P(PSTR("! Willkommen !"),PSTR("2-Achsen-Greifer:"),PSTR("1. Achse: xxx Grad"),PSTR("2. Achse: xxx Grad"));
	displayAktualisieren();
	
	while(1)
	{
		uint8_t key = getPressedKeyNumber();
		switch(key)
		{
			case 5:
				if ((Greifer_Achse1 <= 190) && (Greifer_Achse1 > 80))
				{	
					Greifer_Achse1--;
					displayAktualisieren();
				}
				mSleep(15);
				break;
			case 4:
				if ((Greifer_Achse1 >= 80) && (Greifer_Achse1 < 190))
				{
					Greifer_Achse1++;
					displayAktualisieren();
				}
				mSleep(15);
				break;
			case 3:
				if (Greifer_Achse2 < 170)
				{
					Greifer_Achse2++;
					displayAktualisieren();
				}
				mSleep(5);
				break;
			case 2:
				if (Greifer_Achse2 > 20)
				{
					Greifer_Achse2--;
					displayAktualisieren();
				}
				mSleep(5);
				break;
		}
	}
	return 0;
}
Achja, ich verwende ein 4 Zeilendisplay, also musst du eventuell noch etwas ändern.

mfg