Ich habs geschafft: \
/
Jetzt habe ich einen funktionierenden Code zur berechnung der Drehung mit Hilfe von mare_crisiums Dokumentationen und .xls erstellt:
(Falls ihr Verbesserungsvorschläge habt, bitte melden)
Dazu sind 2 Funktionen nötig:
-Eine zum Initialisieren der Variablen, welche beim Start des Roboters aufgerufen wird
-Eine, welche immer nach einer kurzen Drehung die aktuelle Position auswertet.
Also hier der Code (einfach in eine Header File kopieren und im Programm includieren)
ACHTUNG: Die defines müssen an euren Maussensor angepasst werden. Zur näheren Beschreibung und zur Berechnung siehe in der Dokumentation von mare_crisium weiter vorn im Thread.
Code:
/*
###################################################
27.09.08 - TurnAlgorithm.h
---------------------------------------------------
Header File welche diverse Funktionen zum Berechnen
der Drehung beinhaltet
###################################################
*/
#include <stdlib.h>
#include <math.h>
//PI mit 2 multipliziert (= 360°)
#define PI_DOUBLE 6.283185307179586476925286766559
//PI mit 2 dividiert (= 90°)
#define PI_HALF 1.5707963267948966192313216916398
// Benötigte Variablen zur Berechnung
#define TURN_ALGO_ALPHA 0.0336681 //Benötigter Wert zum Umrechnen von Ticks in mm
#define TURN_ALGO_DIST 62.6 //Abstand zwischen Maussensor-Messpunkt und Roboter-Mittelpunkt in mm
#define TURN_ALGO_MOUSE_ANGLE -1.62187951 //Fehlausrichtungswinkel der x-Richtung der Maus zur Längsachse des Roboters im Bogenmaß
#define TURN_ALGO_COS_MOUSE_ANGLE -0.05106097 //cos(TURN_ALGO_MOUSE_ANGLE)
#define TURN_ALGO_SIN_MOUSE_ANGLE -0.99869554 //sin(TURN_ALGO_MOUSE_ANGLE)
/**
* Initialisiert die Variablen, welche benötigt werden, um die Drehung zu berechnen
* @param CurrX Beinhaltet die aktuelle X-Position des Sensors
* @param CurrY Beinhaltet die aktuelle X-Position des Sensors
* @param LastX Pointer auf die Variable mit der letzten X-Koordinate (übergabe an Funktion TurnAlgo_update)
* @param LastY Pointer auf die Variable mit der letzten Y-Koordinate (übergabe an Funktion TurnAlgo_update)
* @param Last_e_p_norm_x Pointer auf die Variable mit dem letzten Wert von e_p (normiert) x (übergabe an Funktion TurnAlgo_update)
* @param Last_e_p_norm_y Pointer auf die Variable mit dem letzten Wert von e_p (normiert) x (übergabe an Funktion TurnAlgo_update)
* @param Last_e_q_norm_x Pointer auf die Variable mit dem letzten Wert von e_q (normiert) x (übergabe an Funktion TurnAlgo_update)
* @param Last_e_q_norm_y Pointer auf die Variable mit dem letzten Wert von e_q (normiert) y (übergabe an Funktion TurnAlgo_update)
* @param turnAngle Pointer auf die Variable welche den Winkel beinhalten wird (übergabe an Funktion TurnAlgo_update)
*
*/
void TurnAlgo_init(int CurrX, int CurrY, int *LastX, int *LastY, double *Last_e_p_norm_x, double *Last_e_p_norm_y, double *Last_e_q_norm_x, double *Last_e_q_norm_y, double *turnAngle)
{
*LastX = CurrX;
*LastY = CurrY;
*Last_e_p_norm_x = 1.0;
*Last_e_p_norm_y = 0.0;
*Last_e_q_norm_x = 0.0;
*Last_e_q_norm_y = 0.0;
*turnAngle = 0.0;
}
/**
* Wird nach jeder kurzen Drehung aufgerufen, um den neuen Winkel zu berechnen
* @param CurrX Beinhaltet die aktuelle X-Position des Sensors
* @param CurrY Beinhaltet die aktuelle X-Position des Sensors
* @param LastX Pointer auf die Variable mit der letzten X-Koordinate (übergabe an Funktion TurnAlgo_update)
* @param LastY Pointer auf die Variable mit der letzten Y-Koordinate (übergabe an Funktion TurnAlgo_update)
* @param Last_e_p_norm_x Pointer auf die Variable mit dem letzten Wert von e_p (normiert) x (übergabe an Funktion TurnAlgo_update)
* @param Last_e_p_norm_y Pointer auf die Variable mit dem letzten Wert von e_p (normiert) x (übergabe an Funktion TurnAlgo_update)
* @param Last_e_q_norm_x Pointer auf die Variable mit dem letzten Wert von e_q (normiert) x (übergabe an Funktion TurnAlgo_update)
* @param Last_e_q_norm_y Pointer auf die Variable mit dem letzten Wert von e_q (normiert) y (übergabe an Funktion TurnAlgo_update)
* @param turnAngle Pointer auf die Variable welche den Winkel beinhalten wird (übergabe an Funktion TurnAlgo_update)
*
*/
void TurnAlgo_update(int CurrX, int CurrY, int *LastX, int *LastY, double *Last_e_p_norm_x, double *Last_e_p_norm_y, double *Last_e_q_norm_x, double *Last_e_q_norm_y, double *turnAngle)
{
double delta_x = (CurrX - (*LastX)) * (double)TURN_ALGO_ALPHA; //Delta X in mm
double delta_y = (CurrY - (*LastY)) * (double)TURN_ALGO_ALPHA; //Delta Y in mm
double delta_y_norm = delta_x * (double)TURN_ALGO_SIN_MOUSE_ANGLE + delta_y * (double)TURN_ALGO_COS_MOUSE_ANGLE; //Delta Y in mm mit berücksichtigung der Fehlausrichtung
double delta_angle = delta_y_norm / (double)TURN_ALGO_DIST;
double e_p_not_norm_x = (*Last_e_p_norm_x) + delta_angle * (*Last_e_q_norm_x); //e_p (nicht normiert) x
double e_p_not_norm_y = (*Last_e_p_norm_y) + delta_angle * (*Last_e_q_norm_y); //e_p (nicht normiert) y
double e_p_not_norm_sum = sqrt(pow(e_p_not_norm_x,2) + pow(e_p_not_norm_y,2)); //Betrag von e_p (nicht normiert)
double curr_e_p_norm_x = e_p_not_norm_x/e_p_not_norm_sum;
double curr_e_p_norm_y = e_p_not_norm_y/e_p_not_norm_sum;
*LastX = CurrX;
*LastY = CurrY;
*Last_e_p_norm_x = curr_e_p_norm_x;
*Last_e_p_norm_y = curr_e_p_norm_y;
*Last_e_q_norm_x = e_p_not_norm_y * (-1);
*Last_e_q_norm_y = e_p_not_norm_x;
*turnAngle = atan2(curr_e_p_norm_y, curr_e_p_norm_x) * (-1); //Winkel im bereich von [-179,179]
if (*turnAngle < 0)
*turnAngle = PI_DOUBLE + *turnAngle;
}
Ich habe das alles so realisiert, dass der Drehfunktion die Variablen der Vorherigen dreh-berechnung als Pointer übergeben werden.
In der TurnRobot funktion kann ich einfach einen Winkel übergeben, zu dem hingedreht werden soll, also nicht, um wieviel gedreht wird. Würde man der Funktion zu umschreiben, dass man übergibt, um wie viel gedreht wird, dann wäre die Fehlerwahrscheinlichkeit sehr viel höher.
Man kann auch mal einfach Geradeaus fahren, und danach Drehen. Die absolute Winkelangabe sollte dann trozdem funktionieren.
Ein Aufruf könnte dann wie folgt aussehen
Code:
int TurnAlgo_LastX;
int TurnAlgo_LastY;
double TurnAlgo_Last_e_p_norm_x;
double TurnAlgo_Last_e_p_norm_y;
double TurnAlgo_Last_e_q_norm_x;
double TurnAlgo_Last_e_q_norm_y;
double TurnAlgo_currAngle;
// -------------- Main-Funktion --------------
int main(void)
{
/*###Initialisierungsphase###*/
// ... eigene Initialisierungen
TurnAlgo_init(0, 0, &TurnAlgo_LastX, &TurnAlgo_LastY, &TurnAlgo_Last_e_p_norm_x, &TurnAlgo_Last_e_p_norm_y, &TurnAlgo_Last_e_q_norm_x, &TurnAlgo_Last_e_q_norm_y, &currAngle);
while(1)
{
//Hauptschleife
}
}
/**
*Dreht den Roboter zum angegebenen Winkel
* @param degree Grad, auf die gedreht werden soll
*/
void turnRobot(int endDegree)
{
int startPos_X, startPos_Y;
pan_getPos(&startPos_X, &startPos_Y);
TurnAlgo_update(startPos_X, startPos_Y, &TurnAlgo_LastX, &TurnAlgo_LastY, &TurnAlgo_Last_e_p_norm_x, &TurnAlgo_Last_e_p_norm_y, &TurnAlgo_Last_e_q_norm_x, &TurnAlgo_Last_e_q_norm_y, &TurnAlgo_currAngle);
double startAngle = TurnAlgo_currAngle;
double endAngle = (M_PI * endDegree) / 180.0;
//Winkel in Bereich von 0 bis 360° (= PI_DOUBLE) umrechnen
while (endAngle >= PI_DOUBLE)
endAngle -= PI_DOUBLE;
while (endAngle <0)
endAngle += PI_DOUBLE;
double diff = endAngle - TurnAlgo_currAngle;
//Gibt an, ob sich der Roboter gegen den Uhrzeiger (mathematischer Drehsinn) = 0 oder mit dem Urhzeiger = 1 dreht
int turnDirection = 0;
//Überprüfung, ob Differenz mehr als 180° (= PI), wenn ja, andere Drehrichtung verwenden, da dort der Weg kürzer ist.
if (diff > M_PI)
{
turnDirection = 1;
diff -=PI_DOUBLE;
}
else if (diff < M_PI*(-1))
{
turnDirection = 0;
diff +=PI_DOUBLE;
}
else if (diff > 0)
turnDirection = 0;
else if (diff < 0)
turnDirection = 1;
else
{
return; //Nothing to do
}
int speedL;
int speedR;
if (turnDirection == 0) //Drehrichtung = 0
{
speedL = -245;
speedR = 255;
}
else if (turnDirection == 1) //Drehrichtung = 1
{
speedL = 255;
speedR = -245;
}
int newPos_X, newPos_Y;
//Schleife wird auch innerhalb unterbrochen, sobald der endAngle erreicht ist
while (startAngle!=endAngle)
{
moveRobot(speedL, speedR, 3);
pan_getPos(&newPos_X, &newPos_Y);
TurnAlgo_update(newPos_X, newPos_Y, &TurnAlgo_LastX, &TurnAlgo_LastY, &TurnAlgo_Last_e_p_norm_x, &TurnAlgo_Last_e_p_norm_y, &TurnAlgo_Last_e_q_norm_x, &TurnAlgo_Last_e_q_norm_y, &TurnAlgo_currAngle);
if (TurnAlgo_currAngle == endAngle)
break;
if (turnDirection == 0)
{
if ((startAngle < endAngle) && (TurnAlgo_currAngle >= endAngle))
break; //Beim startWinkel 170° abziehen, da es vorkommen kann, dass der TurnAlgo_currAngle den Winkel 259° anstatt 0° hat und dann unterbrochen werden würde
else if ((startAngle > endAngle) && (TurnAlgo_currAngle >= endAngle) && (TurnAlgo_currAngle < startAngle - 2.9670597283903602807702743064306)) //Bereich liegt über dem 4. und 1. Quartal
break;
}
else if (turnDirection == 1)
{
if ((startAngle > endAngle) && ((TurnAlgo_currAngle <= endAngle) || (TurnAlgo_currAngle > endAngle + M_PI)))
break; //Beim startWinkel 170° addieren, da es vorkommen kann, dass der TurnAlgo_currAngle den Winkel 1° anstatt 0° hat und dann unterbrochen werden würde
else if ((startAngle < endAngle) && (TurnAlgo_currAngle <= endAngle) && (TurnAlgo_currAngle > startAngle + 2.9670597283903602807702743064306)) //Bereich liegt über dem 4. und 1. Quartal
break;
}
}
}
Danke an alle die mitgeholfen haben.
Edit: Code verbessert
Edit 2 (Dez. 2014): PDF von mare_crisium zur Berechnung erneut hochgeladen: (siehe post #62):
https://www.roboternetz.de/community...l=1#post609113
Zitieren! Beim Durchsehen des Algorithmus (mit beschränkten C-Kenntnissen) ist mir nur aufgefallen, dass Du e_q_norm_y mit 0.0 initialisierst; der Startwert muss aber 1.0 sein.
. Jubel, habt ihr gut gemacht.
Zitat von Sternthaler
Lesezeichen