Druckbare Version
Guten Abend,
ich brauche einen Quellcode, der mir die Position des Roboters nach zurüchgelegter Strecke in x,y-Richtung und den Winkel von dort aus, wo es gefahren ist, zurückgibt.
dafür habe ich auch ein Bild, das euch bestimmt weiterhelfen würde!
danke im vorraus
gruß
Dafür wäre es erstmal sinnvoll zu erfahren, womit du die Positionen der Achsen misst.
An sich würde ich das Ganze zeitdiskret angehen. Ausgehend von einer Startposition, wo alle Werte praktischerweise 0 sind, müsstest du dann sehen, wie sich die Achspositionen verändern. Je nach Differenz zur vorhergehenden Stellung ließe sich über den Achsabstand dann der differentielle Winkel und die differentielle Position berechnen. Als würde man für jeden Abschnitt ein kurzes Kreissegment anlegen. Wie man den Kurvenradius eines einachsigen Fahrzeugs berechnet, kommst du sicher von alleine drauf. Letztendlich wird durch numerische Diskretisierungsfehler und leichten Schlupf in den Rädern die gemessene Position von der Tatsächlichen immer mehr abweichen. Dagegen helfen nur externe Messsysteme wie GPS oder irgendwelche revolutionären Ansätze.
Wie wärs damit ?Zitat:
Zitat von kaugummi
Code:; :::::::x:::::::::x:::::::::x:::::::::x:::::::::x:::::::::x:::::::::x::
; **********************************************************************
; * *
; * W E G E N . L S P *
; * Gegenwaertiger ( 12. Juli 1989 ) Bestand an Modulen: *
; * defun c:wegen *
; * defun ... *
; **********************************************************************
; :::::::x:::::::::x:::::::::x:::::::::x:::::::::x:::::::::x:::::::::x::
; Winkel von Altgrad ins Bogenmass konvertieren
(defun aib (w)
(* pi (/ w 180.0))
)
; Benoetigte Informationen fuer den Weg
(defun wegbenutzer ()
(setq sp (getpoint "\nStartpunkt des Wegs: "))
(setq ep (getpoint "\nEndpunkt des Wegs: "))
;
(setq wwinkel (angle sp ep))
(setq wlaenge (distance sp ep))
(setq breite (* 2 hbreite))
(setq winp90 (+ wwinkel (aib 90))) ; Weg Winkel + 90 Grad
(setq winm90 (- wwinkel (aib 90))) ; Weg Winkel - 90 Grad
)
;
;
(defun umriss ()
(command "plinie"
(setq p (polar sp winm90 hbreite))
(setq p (polar p wwinkel wlaenge))
(setq p (polar p winp90 breite))
(polar p (+ wwinkel (aib 180)) wlaenge)
"schliessen"
)
)
;
(defun zreihe (wa versetz)
(setq perster (polar sp wwinkel wa))
(setq pcplatte (polar perster winp90 versetz))
(setq p1platte pcplatte)
(while (< (distance perster p1platte) (- hbreite prad))
(command "kreis" p1platte prad)
(setq p1platte (polar p1platte winp90 (+ pabst prad prad)))
)
(setq p1platte (polar pcplatte winm90 (+ pabst prad prad)))
(while (< (distance perster p1platte) (- hbreite prad))
(command "kreis" p1platte prad)
(setq p1platte (polar p1platte winm90 (+ pabst prad prad)))
)
)
(defun platzeich ()
(setq wstrecke (+ prad pabst))
(setq vers 0.0)
(while (<= wstrecke (- wlaenge prad))
(zreihe wstrecke vers)
(setq wstrecke (+ wstrecke (* (+ pabst prad prad) (sin (aib 60)))))
(if (= vers 0.0)
(setq vers (* (+ pabst prad prad) (cos (aib 60))))
(setq vers 0.0)
)
)
)
;
; >>> Befehl ausfuehren, Funktionen aufrufen
;
(defun C:WEG ()
(wegbenutzer)
(setq bildkp (getvar "blipmode"))
(setq bildcmde (getvar "cmdecho"))
(setvar "blipmode" 0)
(setvar "cmdecho" 0)
(umriss)
(platzeich)
(setvar "blipmode" bildkp)
(setvar "cmdecho" bildcmde)
)
; :::::::x:::::::::x:::::::::x:::::::::x:::::::::x:::::::::x:::::::::x::
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
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(" ");
}
}
}
Der Quatraturencoder braucht eine A und eine B Spur. Die B Spur ist um 90° versetzt zur A Spur.
MfG Hannes