Team: Martin, Tom, Lukas
Problem
Um nun den Gyrosensor kennen zu lernen, welchen wir für die Programmierung unseres Endziels, dem Segway, benötigen, beschäftigen wir uns zunächst mit einer “einfacheren” Wippe. Diese Liegt auf einem Zahnkranz, welcher durch einen Motor bewegt werden kann. In der Mitte der Wippe ist unser Sensor angebracht.
Ansatz
Um nun diese Wippe nach leichtem Anstoßen wieder auszubalanchieren, nutzen wir ein PID-Regelungssystem (Proportional-Integral-Differential Regelungssystem). In diesem System wird versucht den Eingangswert durch die einzelnen P-, I- und D-Anteile auf einen Sollwert zu regeln.
PID-Regler:
Der PID-Regler besteht, wie gesagt, aus den drei oben aufgezählten Komponenten. Der Proportionalanteil ist direkt-proportional zum Mess-/Sensorwert. Der Diffenentialanteil besteht aus der 1. Ableitung der Mess-/Sensorwerte und der Integralanteil ist die Integration der der Mess-/Sensorwerte.
In unserem Fall spricht der Proportionalanteil auf den Winkel der Wippe an. Desto größer der Winkel abweicht von der Horizontale, desto stärker wird entgegen geregelt. Im Fall der Wippe regelt der Differentialanteil stärker entgegen, wenn die Windelgeschwindigkeit groß ist. Der Vorteil des Differentials ist, dass auch bei kleinen Winkeln, aber großen Winkelgeschwindkeiten der Differentialanteil “vorausschauend” der Winkeländerung entgegenwirkt. Der Integralanteil hat eine dämpfende Wirkung und glättet das Schwingen. Desweiteren reagiert Er rückblickend auf die vorhergehende mittlere Abweichung der gemessenen Sollwerte.
Gyrosensor
Der Gyrosensor registriert ein Lageänderung auf einer Ebene. Wir mussten auch feststellen, dass die ausgegebenen Werte nicht Winkel, in Bezug zur Ausgangslage waren, sondern nur die Winkelgeschwindigtkeit, mit den Werten kann man also nicht sagen in welcher Lage sich der Sensor befindet, sondern nur mit welcher Geschwindigkeit er sich von einer Lage zur Anderen bewegt.
Für den PID-Regler hatten wir also schon den Diffentialanteil anhand von den Werten des Sensors und mussten lediglich zweimal integrieren um Proportional- und Integralanteil zu erhalten.
task get_angle() mess0=SensorHTGyro(S1,0); gyrodiffvorher=gyrodiff; gyroangle += gyrodiff*(zeitjetzt-zeitvorher)/1000.; task loop() /* //NumOut(0,LCD_LINE3,gyroint); /* Für dieses System taugt der Integralanteil nichts */ OnFwd(OUT_A,Dreh1); ClearScreen(); }
{while (true)
{
zeitvorher=zeitjetzt; // Die Messzeit der letzte Runde wird
// in zeitvorher gespeichert.
zeitjetzt=CurrentTick(); // Das ist die aktuelle Messzeit.
gyrodiff=0.5*(mess0-gyrosensorOffset)+0.5*gyrodiff;
/* Glättung der Winkelgeschwindigkeit: macht das
System träger, aber weniger empfindlich gegen einzelne
falsche Messwerte, bzw. gegen Messrauschen. */
gyroacc=0.95*gyroacc+0.05*(gyrodiff-gyrodiffvorher)/((zeitjetzt-zeitvorher)/1000.);
gyroint =0.99*gyroint+0.01*gyroangle*(zeitjetzt-zeitvorher)/1000.;
}
}
{
while(true){
float g;
if (gyroangle>200)
{g=200.0;}
else
if (gyroangle<-200)
{g=-200.0;}
else {g=gyroangle;} */
/* Dafür war es nötig, die zweite Winkelbeschleunigung,
d.h. die Ableitung der gemessenen Winkelgeschwindigkeit
zu berücksichtigen. */
Dreh1= -gyroangle*0.45-gyrodiff*(0.40)-gyroacc*0.15-gyroint*0.0;
NumOut(0,LCD_LINE1, mess0);
NumOut(0,LCD_LINE2,gyrodiff);
NumOut(0,LCD_LINE3,gyroangle);
NumOut(0, LCD_LINE4 , gyrosensorOffset);
NumOut(0,LCD_LINE5,zeitjetzt-zeitvorher);
NumOut(0,LCD_LINE6,Dreh1);
NumOut(0,LCD_LINE7,gyroint);
if (ButtonPressed(BTN4,false)==1) {
gyroangle=0.0;
gyroint=0.0;}
}
Wie Ihr im Quelltext wohlmöglich schon gelesen habt, war der Integralanteil für unsere Wippe von wenig Nutzen. Deswegen haben wir die 2. Ableitung (Winkelgeschwindigkeit abgeileitet nach der Zeit), sprich die Winkelbeschleunigung, mit in den Regler einbezogen.
Ein weiteres Hindernis auf Das wir gestoßen sind, war die Kalibrierung des Sensors. Damit in Ruhelage keine Lageänderungen angezeigt werden, mussten wir den Offset (Fehler) des Sensors ermitteln. Später wurde auch festgestellt, dass wenn der Motor läuft, die Ausgabe des Sensors gestört wurde. Das wurde dann auch in der Kalibrierung berücksichtigt. SetSensorHTGyro(S1); /*for(i=0; i<10000; i++) { }*/ /* Kalbrierung: Leider h‰ngt der Wert, den der ResetSensor(S1); gyrosensorOffset=addOffset/3200.0;
Hier der dazugehörige Quelltext:
sub Calibrate()
{
float addOffset=0.0;
int i;
int j;
float sens;
sens=SensorHTGyro(S1,0);
Sensor liefert, von der Auslesegeschwindigkeit ab.
Auflerdem h‰ngt er davon ab, ob der Motor gerade
l‰uft: Deshalb eine mehrteilige Kalibrierung:
mit Motor, ohne, etc.
*/
OnFwd(OUT_A,80) ;
Wait(1000);
Off(OUT_A);
//Wait(1000);
//Off(OUT_A);
for (j=0; j<8; j++) {
for(i=0; i<400; i++) {
sens=SensorHTGyro(S1,0);
NumOut(0, LCD_LINE1, sens);
NumOut(0, LCD_LINE2, addOffset);
OnFwd(OUT_A,2) ;
addOffset=addOffset+sens;
Wait(4);
}
if (j<4){
OnFwd(OUT_A,80) ;
Wait(200);
OnFwd(OUT_A,-80);
Wait(200);
Off(OUT_A) ; }
}
NumOut(0, LCD_LINE1, gyrosensorOffset);
Wait(1000);
}
In dem folgendem Video wird die fertiggestellte Wippe mit dem Skript veranschaulicht: