Atmega32: Aktueller Ausrichtung des Roboters falsch... Nur wieso ?
Hallo Zusammen,
in meiner Motorsteuerung berechne ich den aktuellen Winkel des Roboters zur Positionsbestimmung.
Nur ändert sich der Winkel nach einer kurzen Fahrstrecke vollkommen, obwohl er geradeausfahren soll (also keine Winkeländerung). Der Roboter fährt einen Meter fast 100% geradeaus, hat aber eine massive Winkeländerung in seiner Berechnung.
Z.B. ist die Ausrichtung 3.14... beim Starten und nach einem Meter etwas über 4 rad.
Ich verwende folgende Formel und sehe den Fehler nicht:
nStepsLeft und nStepsRight sind die letzte Schrittänderungen seit des Letzen Aufruf des PID Controllers
Code:
void CalculateOrientation(unsigned int nStepsLeft, unsigned int nStepsRight)
{
double deltaOrientation;
if (m_DirectionFlagLeft == m_DirectionFlagRight ) //Are we driving in front
{
if( nStepsLeft > nStepsRight)
{
deltaOrientation = -1.0 *(double) (nStepsLeft-nStepsRight) * RADPROIMPULS; // Calculation the change in the angle during the drive
}
else
{
deltaOrientation = (double) (nStepsRight-nStepsLeft) * RADPROIMPULS; // Calculation the change in the angle during the drive
}
}
else
{
deltaOrientation = ((( nStepsLeft + nStepsRight ) / 2.0) * RADPROIMPULS);// Add average calculation for the angle difference
if(m_DirectionFlagLeft == 1 && m_DirectionFlagRight == 0) // Are we turning to the right direction ?
{
deltaOrientation *= -1.0; // negative angle
}
}
m_ActualOrientation = AjustToTwoPI(m_ActualOrientation + deltaOrientation); // Adjust the value to the full circle
}
Die Konstanten ergeben sich hierbei mit :
Code:
#define IMPULSPERFULLROTATION ((double)250.0) // Anzahl der Impulse vom Sensor für eine Radumdrehung
#define RADRADIUSINMM ((double)30.0) // Radius des Antriebrads
#define WHEELBASE_INMM ((double)155.0) // Abstand der Kettenräder
#define TWO_PI (M_PI * 2.0)
#define WAYPROIMPULSINMM ((TWO_PI * RADRADIUSINMM) / IMPULSPERFULLROTATION) // Weg in MM für einen Impuls des Sensors
#define WAYFULLROTATION_INMM (TWO_PI * WHEELBASE_INMM) // Weg für eine volle Drehung in MM
#define IMPULSEPERFULLROTATION (WAYFULLROTATION_INMM / WAYPROIMPULSINMM) //Anzahl der Impulse für eine volle Drehung
#define RADPROIMPULS (TWO_PI / IMPULSEPERFULLROTATION) // Winkeländerung in rad pro Impulse
Generell ist hier mein Ansatz, das wenn ich auf einer Kette einen Step mehr habe, als auf der anderen Kette, ich eine entsprechende Winkeländerung des Roboters bekomme, welche sich bezogen auf die Gesamtanzahl der Impulse für eine volle Umdrehung errechnet.
Der Weg für eine volle Umdehung wäre hierbei:
Code:
#define WAYFULLROTATION_INMM (TWO_PI * WHEELBASE_INMM)
Sind die Anzahl der Schritte auf der rechten Seiten geringer, wird eine negative Winkeländerung erkannt:
daher das "-1.0".
Code:
deltaOrientation = -1.0 *(double) (nStepsLeft-nStepsRight) * RADPROIMPULS; // Calculation the change in the angle during the drive
Entweder mache ich einen generellen Fehler oder ich sehe den Fehler nicht der in dem Programm steckt.
Gruss R.
