Erstes muss du verstehen was dahinten steckt : die reine Mathematic. Ist gar nicht so schwierig :
Abstand L-rad : encoder_tics L, Abstand R-rad : encoder_tics R.
Dan wird die Verplatzung von mittelpunkt Radachse : (encoder_tics L + encoder_tics R). Das ist naturlich nur eine Gerade wen Abstand L-rad = Abstand R-rad. When nicht, ist das eine Boge.
Die Verdrehung um die Hochachse ist auch gans einfach : (encoder_tics L- encoder_tics R) Wiederum, wen beide gleich ist da kein Verdrehung passiert ! So wird die ganse Fahrstrecke in kleine Bogen verteilt und immer wird die Aenderungen aufaddiert mit folgende Mathematic :
Neue X_pos= cosinus(Winkelaenderung/2)*Verplatzung + Alte X_pos
Neue Y_pos= sinus(Winkelaenderung/2)*Verplatzung+Alte Y_pos
Neue Winkel = Winkeaenderung+alte Winkel
Für ihre Winkelberechnungen ist es notwendig das die wirkliche Winkel in radial berechnet werd. Das ist abhangig von Radabstand und Encoder-resolution. Auch wichtig ist naturlich das die Encoder Auf und Abzaehlen konnen (kwadratuurencoder)
Code:
int16_t delta_compass; //hoekverdraaing tov vorige doorloop
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) >10)&(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(" ");
}
}
}
Lesezeichen