Meine odometrie ablauf fur eine differential drive ist relatif einfach :
Sobald das e_compass (unterschied linker und rechter tics von antrieb) eine bestimme Schwelle ueberschreitet oder die gefahrene abstand (delta_distance) eine Schwell ueberschreitet wird die neu X und Y position berechnet. Obwohl einfach, functioniert das sehrt gut : Meine robby kan nach 20 curven und Abstanden von 10 meter noch immer zuruck nach das Startpunkt fahren. Aber ich habe keine Schlupf-probleme !! Mit den RP6 konnen sie das vergessen, wegend Schlupf. Abstande gerade aus geht noch, aber curven sind fiel zu ungenau.
Code:
void odometrie (void){
int16_t delta_compass; //hoekverdraaing tov vorige doorloop
static int16_t e_compass_old; //waarde e_compass bij vorige doorloop (encodertics)
delta_compass=abs(e_compass - e_compass_old); //hoekverdraaing tov vorige doorloop (encodertics)
if((abs(delta_distance) > 200)||(delta_compass > 100)||(isMovementComplete()&(abs(delta_distance) > 10))){ //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 !!
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(program==10){
writeString("\n");
writeInteger(x_pos,DEC);writeString_P(" ");
writeInteger(y_pos,DEC);writeString_P(" ");
writeInteger(e_compass,DEC);writeString_P(" ");
writeInteger(state,DEC);writeString_P(" ");
}
}
Lesezeichen