Diese "odometrie()" function nutzte ich nich in meinen RP6, aber in eine "differential drive" robby. Da ich die RP6 Software wirklich sehr gut finden, nutze ich die als basis für al meine Roboter.
Die odometrie function wird in Hauptschleife angerufen. Sobald eine minimale Abstand, oder eine minimale Winkelaenderung seitens letze Mal das diese Function durchgelaufen war, wird er wieder durchgelaufen.
delta_distance ist das Anzahl tics von L und R motor zusammen seitens letze Durchlauf. Sobald diesen Wert grosse ist dan 200 tics, wird die neue X_pos und Y-pos berechnet
Identisch met delta_compass : alte winkel - actuelle winkel.
Den actuelle winkel wird auch jedes mal in ISR von Encoder neu berechnet (differenz von L und R motor tics). Da den RP6 keine kwadratur encoder hat, muss du da naturlich noch Rechnung halten oder der Motor forwarts oder ruckwarts angesteuert wird.
Die odometrie fucntioniert damit auch in Curven !!
Die variabele Program ist eine globlae variabele, womit ich das Program von Robby wahlen. Nur bei progam nr 10 wird eine Ausdruck gemacht von diese Positionen und Winkel bei jeden Durchlauf.
Hier meine Encoder ISR (bei RP6 fehlt die kwadratur enc !!)
Code:
ISR (INT0_vect)
{ 
 if(PINC&KMI_L){
  mleft_dist--;
  mleft_counter--; //writeString_P("ISR0");//controle interrupt  
     e_compass--;
  delta_distance--;} //richting robby !!
 
 else {
  mleft_dist++;
  mleft_counter++;
  e_compass++;
  delta_distance++;} //delta_distance voor odometrie !! 
 
}
/**
 * External Interrupt 1 ISR
 * (ENCR)
 *
 */
ISR (INT1_vect)
{ 
 if(PINC&KMI_R){
  mright_dist++;
  mright_counter++;  //writeString_P("ISR0");//controle interrupt
     e_compass--;
  delta_distance++;} //richting robby !!
 else {
  mright_dist--;
  mright_counter--;
  e_compass++;
  delta_distance--;} //delta_distance voor odometrie !! 
   //delta_distance voor odometrie !! 
}