wie hast du die Positionen geteacht?
teachen=solange an den Servopositionen fummeln bis es halbwegs passt.

In C sieht das bei mir dann so aus:
Code:
// Deltaarm ansteuern mit dem RP6                                  3.2.09  mic

#include "rblib.h"
#include "rblib.c"

volatile uint16_t p=0;
uint8_t  xpos=120, ypos=120, zpos=120, demo;
uint16_t speed;

void servo_init(void);
void pause(uint16_t p_10ms);
void moveto(uint8_t x, uint8_t y, uint8_t z, uint8_t pause_10ms);
void holen1(void);
void holen2(void);
void holen3(void);
void bringen1(void);
void bringen2(void);
void bringen3(void);
int main(void)
{
	rblib_init();
	servo_init();
	pause(100);
/*   for(demo=120; demo<190; demo++) moveto(demo,demo,demo,5);
   for(demo=189; demo>120; demo--) moveto(demo,demo,demo,4);
   for(demo=121; demo<190; demo++) moveto(demo,demo,demo,3);
   for(demo=189; demo>120; demo--) moveto(demo,demo,demo,2);
   for(demo=121; demo<190; demo++) moveto(demo,demo,demo,2);
   for(demo=189; demo>120; demo--) moveto(demo,demo,demo,1);
   moveto(190,190,190,50);
	moveto(120,120,120,50);
*/

	// heben und legen
	holen1(); bringen1();
	holen3(); bringen3();

	while(1)
	{
	// tauschen
		holen1(); bringen2();
		holen3(); bringen1();
		holen2(); moveto(90,160,90,40); bringen3();
	}
	return(0);
}
ISR(TIMER0_COMP_vect)
{
	static uint16_t count=0;
	if(count>xpos) PORTA &= ~16; else PORTA |= 16;	// E_INT1 (Pin8)
	if(count>ypos) PORTC &= ~1;  else PORTC |= 1;	// SCL (Pin10)
	if(count>zpos) PORTC &= ~2;  else PORTC |= 2;	// SDA (Pin12)
	if(count<1000)count++; else { count=0; if(p) p--; }
}
void servo_init(void)
{
	DDRA |= 16;				// E_INT1 als Ausgang
	DDRC |= 3;				// SCL und SDA als Ausgang

	TCCR0 =  (0 << WGM00) | (1 << WGM01);					// CTC-Mode
	TCCR0 |= (0 << COM00) | (0 << COM01);					// ohne OCR-Pin
	TCCR0 |=	(0 << CS02)  | (1 << CS01) | (0 << CS00);	// prescaler /8
	TIMSK =  (1 << OCIE0); 										// Interrupt ein
	OCR0  = 10; // 100kHz?
}
void pause(uint16_t p_10ms)
{
	p=p_10ms;
	while(p);
}
void moveto(uint8_t x, uint8_t y, uint8_t z, uint8_t p_10ms)
{
	xpos=x;
	ypos=y;
	zpos=z;
	p=p_10ms;
	while(p);
}
void holen1(void)
{
		moveto(99,104,91,50);
		moveto(80,90,73,20);
		moveto(99,104,91,30);
}
void holen2(void)
{
		moveto(60,111,83,50);
		moveto(50,95,65,20);
		moveto(60,111,83,30);
}
void holen3(void)
{
		moveto(84,110,52,50);
		moveto(69,95,47,20);
		moveto(84,110,52,30);
}
void bringen1(void)
{
		moveto(99,104,91,50);
		pause(200);
}
void bringen2(void)
{
		moveto(60,111,83,50);
		pause(200);
}
void bringen3(void)
{
		moveto(81,107,50,50);
		pause(200);
}
*GG*