Sensorik am RP6, Befehle und Allgemeines

[fabqu]

Ah ja, math.h!
Muss ich die noch extra includieren? Wenn ja, wo?

Und das mit der Berechnung mit Sin und Cos... sollte denke ich stimmen, hatte ich mir auch so aufgeschrieben.
Immer schöne mit Ankathete und Gegenkathete

Fabian

[RP6conrad]

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.

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

[fabqu]

Hi Conrad,
wenn ich das richtig sehe, dann berechnet dein Programm ununterbrochen während der Fahrt die Koordinaten?
Genau so bruche ich das.
Aber was meinst du mit "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." ?
Hast du nun Probleme mit deinem RP6, oder wie?

Und was ist "delta_distance"? Woher kommt das?
Und woher kommt "program" ? Wo wird das hochgezählt?
Was mir in deinem Programm ein bisschen fehlt: Der Robby weiß glaube ich nie, in welchem Winkel er steht, oder?

Danke Dir!
Fabian

[RP6conrad]

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 !!)

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 !! 
}

[fabqu]

Hallo!
Erst mal: Leider wurde das Projekt auf die nächste Woche verswchoben...
Aber noch eine Frage dazu:
Die Lib-Befehle mleftDist & mrightDist (oder so ähnlich), wann genau werden die mit Werten gefüllt? Nach jedem "isMovementComplete"? oder ununterbrochen?
Und muss ich die selbst auf '0' zurücksetzen?
Denn ich will ja nach jedem einzelnen Manöver folgendes in ein Array schreiben:
Distanz links | Distanz rechts | gefahrene Strecke geradeaus | gedrehter Winkel | x-Position | y-Position
Und da brauche ich natürlich nur die momentanen Werte...

Danke Euch!
Fabian

[Fabian E.]

Soweit ich mich erinnere werden die fortlaufend befüllt.
Und ja, ich glaube du musst die auch per Hand nullen. Da würde ich mir aber noch mal genauer ansehen, ob das so ohne weiteres möglich ist, oder ob das die Lib eher verwirrt.
Zur Not einfach eine zweite Variable als delta benutzen.

[fabqu]

Ja. Ist einfacher...
Hat jemand eine Idee, wie ich vom Distanzunterschied zwischen linkem und rechtem Encoder auf den gedrehten Winkel komme? Ich verstehe nicht ganz, wie die das in der Lib.c machen.
Hier mal der Auszug, von dem ich glaube, dass er Winkel(unint16_t angle) in Distanzen umrechnet:

void rotate(uint8_t desired_speed, uint8_t dir, uint16_t angle, uint8_t blocking)
{
    motion_status.move_L = true;
    motion_status.move_R = true;
    uint16_t distance = (uint16_t) (((uint32_t)(ROTATION_FACTOR) * (uint16_t)angle)/100);
    preDecelerate_L = distance - 100;
    preDecelerate_R = distance - 100;
    preStop_L = distance;
    preStop_R = distance;
    if(distance < 40) {
        distance = 40; 
        preStop_L = 20;
        preStop_R = 20;
        preDecelerate_L = 10;
        preDecelerate_R = 10;
    }
    moveAtSpeed(desired_speed,desired_speed);
    changeDirection(dir);
    
    mleft_dist = 0; 
    mright_dist = 0;
    distanceToMove_L = distance;
    distanceToMove_R = distance;
    
    motion_status_tmp = motion_status.byte;
    MOTIONCONTROL_stateChangedHandler();
    if(blocking)
        while(!isMovementComplete())
            task_RP6System();
}

Ich hätts gern andersrum... Winkel in Grad!

mfg