Sie machen das fiel zu kompliziert !! .Es ist sehr einfach : die Winkelaenderung ist identisch an der Differenz von Linker und Rechter Anzahl pulsen. Die Absolute Distance ist unwichtig, ebenso die abgelegte Strecke !! Sie mussen dan auch nur diesen Differenz verwenden um ihre actuelle Winkel zu wissen. Dafur brauchen sie die Abstand zwischen beide Rader (oder Ketten) und die Auflosung ihre Encoder (mm/tic) Der Cartesiaanse Position da entgegen braucht eine Regelmassige Update und Berechnung. Eine Kettenantrieb hat eher eine Schlechte Odometrie Genauigkeit wegend den Schlupf.
Google mal nach Odometrie. Meine Odometrie Function (E-Compass ist die Differenz zwischen L/R Encoder,delta_distance ist Summe L/R encoder):
Code:
void odometrie (void){ 
static int16_t e_compass_old; //waarde e_compass bij vorige doorloop (encodertics)

delta_compass=e_compass - e_compass_old; //hoekverdraaing tov vorige doorloop (encodertics)

if((abs(delta_distance) > 200)||(abs(delta_compass) > 100)||(isMovementComplete()&(abs(delta_distance) >100)&(program>40))){ //alleen update indien voldoende verschil !!
int32_t delta_x = 0; //||((delta_distance>5)&isMovementComplete())
int32_t delta_y = 0;
double hoek= ((e_compass_old+e_compass)/2/E_COMPASS_RESOLUTION)*M_PI/180;//hoek omzetten naar radialen

delta_x = delta_distance*cos(hoek); //
delta_y = delta_distance*sin(hoek); // opgelet overflow vanaf delta_distance >125 !!
total_distance=total_distance+delta_distance; //totaal afgelegde baan-afstand
delta_distance=0; //resetten van delta_distance ; 
x_pos=x_pos+delta_x; //aktuele x_pos updaten *ENCODER_RESOLUTION/2
y_pos=y_pos+delta_y; //aktuele y-pos updaten *ENCODER_RESOLUTION/2
e_compass_old=e_compass;
if(print_data==1){
writeString("\n");
writeInteger((x_pos*ENCODER_RESOLUTION/2),DEC);writeString_P(" ");
writeInteger((y_pos*ENCODER_RESOLUTION/2),DEC);writeString_P(" ");
writeInteger((e_compass/E_COMPASS_RESOLUTION),DEC);writeString_P(" ");
writeInteger(state,DEC);writeString_P(" ");
}
}
}