Ok, das hab ich soweit verstanden. Hab auch gleichmal ein bisschen Code geschrieben:
Code:
#define F_CPU	8000000

#include <avr/delay.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <math.h>
#include <stdlib.h>

#include "uart.h"


// METHODEN DEKLARATIONEN
void keep_moving(void);
void move(int servo, int pos);
void calc_steps(void);
int signum(int val);


//VARIABLEN DEKLARATIONEN
volatile uint8_t servo_flag = 0;
volatile uint8_t recalc_flag = 1;

volatile uint8_t shoulder_pos = 127;
volatile uint8_t shoulder_dest = 255;
volatile float shoulder_step = 0;

volatile uint8_t elbow_pos = 127;
volatile uint8_t elbow_dest = 255;
volatile float elbow_step = 0;





int main (void) {

	//UART AN
	uart_init(1, 0);

	//INTERUPT UND TIMER AN
	sei();
	TCCR0 |= (1<<CS02) | (1<<CS00);
	TIMSK |= (1<<TOIE0);

	while(1)
	{
		//WENN DIE EINMALE ANGEGEBENE POSITION ERREICHT WURDE
		//DÜRFEN DIE STEPS NEU BERECHNET WERDEN
		if(recalc_flag == 1) {
			calc_steps();
		}
		//NUR AUFRUFEN, WENN EIN TIMER INTERUPT DA WAR
		if(servo_flag == 1) {
			keep_moving();
		}
	}
	return 0;
}

void keep_moving() {
	cli();

	//NEUE POSITIONEN AUSRECHNEN UND RUNDEN
	shoulder_pos = abs(shoulder_pos + shoulder_step);
	elbow_pos = abs(elbow_pos + elbow_step);

	//POSITIONEN SCHICKEN
	move(1, shoulder_pos);
	move(2, shoulder_pos);
	move(3, elbow_pos);

	//WENN DIE POSITIONEN GERUNDET ÜBEREINSTIMMEN
	//NEUBERECHNUNG DER STEPS ERLAUBEN UND NEUES ZIEL FÜR ELBOW SETZEN
	if(abs(shoulder_pos) == shoulder_dest && abs(elbow_pos) == elbow_dest) {
		elbow_dest = 1;
		recalc_flag = 1;
	} else {
		recalc_flag = 0;
	}

	servo_flag = 0;
	sei();
}

void calc_steps(void) {
	//BERECHNEN WIE VIEL SICH DIE Servos BEWEGEN MÜSSEN
	uint16_t shoulder_to_go = shoulder_dest - shoulder_pos;
	uint16_t elbow_to_go = elbow_dest - elbow_pos;

	//HIER ALS ABSOLUTE WERTE
	int16_t shoulder_abs = abs(shoulder_to_go);
	int16_t elbow_abs = abs(elbow_to_go);

	//STEP ERSTMAL AUF +/- 1 SETZEN, JE NACH RICHTUNG
	elbow_step = signum(elbow_to_go);
	shoulder_step = signum(shoulder_to_go);

	
	if(elbow_abs < shoulder_abs) {
		elbow_step = 1 * signum(elbow_to_go);
		shoulder_step = (shoulder_abs / elbow_abs) * signum(shoulder_to_go);
	}
	if(elbow_abs > shoulder_abs) {
		shoulder_step = 1 * signum(shoulder_to_go);
		elbow_step = elbow_abs / shoulder_abs * signum(shoulder_to_go);
	}

}

void move(int servo, int pos){
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = '#';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = 's';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = servo;
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = pos;
}

int signum(int val) {
	if(val != 0) {
		return val/abs(val);
	} else {
		return 0;
	}
}

SIGNAL (SIG_OVERFLOW0){
	servo_flag = 1;
}
Der Code bewegt zur Vereinfachung erstmal nur die Schulter und den Ellbogen. Die Bewegung klappt schon ganz gut. Allerdings passt das Neusetzen des Ziels für Ellbogens nicht ("elbow_dest = 1;" in keep_moving() )
Wer Interesse und Zeit hat kann sich das ja mal anschauen warum das nicht klappt. Ich weiß es ist viel Code zum "kurz" anschauen aber ich habs ein wenig auskommentiert, jetzt isses recht gut zu verstehen.

mfg
jagdfalke