Odometrie: Position, winkel und Strecke

[RP6conrad]

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)

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

[021aet04]

Der Quatraturencoder braucht eine A und eine B Spur. Die B Spur ist um 90° versetzt zur A Spur.

MfG Hannes