Odometrie: Position, winkel und Strecke

**kaugummi** · 08.11.2011, 15:57

danke für eure schnelle Antwort, aber ich bin sozusagen ein anfänger, der sich seit kurzem richtig mit programmieren beschäftigt und nicht viel ahnung von dem ganzen krempel hat!,
zum dem quellcode muss ich sagen, viel habe ich da nicht verstanden, war mir zu kompliziert und hätte gern bisschen kommenare zu

Gibt es vielleicht ne einfacherere Version für den Quellcode?!
danke

**RP6conrad** · 08.11.2011, 19:53

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(" ");
  }
 }
}