Benutzer-Werkzeuge
Seitenleiste
mechatronik:wise21:kugel_balancieren
Wir, de Equalizer-Gruppe, wollen hier eine Zusammenfassung unserer Präsentation im Wissenschaftsfenster hochladen. Die Präsentationsfolien: ml_theequalizor_2.pdf
Der Bauplan:
Der Schaltplan:
PID-Regler
Zum Balancieren der Kugel benutzen wir einen PID-Regler. Dieser versucht, einen Eingabewert einem gesetzten Punkt anzugleichen, indem er einen Ausgabewert verändert.
PID-Regler benötigen außerdem drei festgelegte Werte, die sein Verhalten bestimmen. Das tun sie, indem sie unterschiedliche Gewichtung auf die momentane Position, Geschwindigkeit und die Änderung der Geschwindigkeit der Kugel zur Findung des Ausgabewertes legen.
Zum Finden der korrekten PID-Werte haben wir ein zweites Programm geschrieben, das Variationen dieser Werte für jeweils 20 Sekunden ausprobiert und dann speichert, wie gut der jeweilige Versuch lief. Die besten Ergebnisse wurden danach noch einmal per Hand ausprobiert und verglichen.
Jedes der drei Parameter konnte einen Wert von 0,01 bis 0,1 in Schritten von 0,005 annehmen. Das sind 18 Werte Pro Parameter, also insgesamt 18^3 = 5832 Tests über einen Zeitraum von 1 Tag, 8 Stunden und 24 Minuten.
Zeitraffer des Wertfindungs-Programms
Auswertung der besten Werte
Der Arduino-Sketch:
//=============================================================================================================================Includes und Definitionen
#include "Adafruit_VL53L0X.h" //Bibliothek zur Verwendung des TOF-Sensors (Entfernungsmessung)
#include <Servo.h> //Bibliothek zur Steuerung des Servomotors
#include <PID_v2.h> //Bibliothek zur Verwendung eines PID-Controllers
#include <FastLED.h> //Bibliothek zur Steuerung der LED-Kette
#define POTI_PIN A0 //Pin zum Auslesen des Slider-Potentiometers
#define SERVO_OUT 8 //Pin für die Motorsteuerung
#define LED_PIN 7 //Ansteuerungspin für die LED-Kette
#define NUM_LEDS 12 //Anzahl der LEDs in der Kette
#define POS_MAX 195 //höchste Entfernung, die die Kugel annehmen kann
#define POS_MIN 20 //niedrigste Entfernung, die die Kugel annehmen kann
//---------------------------------------------------------------------------------Deklarierung aller benötigten Objekte||
CRGB leds[NUM_LEDS]; //das Objekt, das den LED-Streifen steuert //
Adafruit_VL53L0X lox = Adafruit_VL53L0X(); //das Objekt, das Messwerte vom TOF-Sensor ausliest //
Servo myservo; //das Objekt, das den Servomotor steuert //
//
//Es werden zwei PID-Werte benutzt, zwischen denen je nach Entfernung gewechselt wird //
double aggKp = 0.075, aggKi = 0.07, aggKd = 0.055; //agressive Werte //
double consKp = 0.09, consKi = 0.025, consKd = 0.055; //feine Werte //
PID_v2 myPID(consKp, consKi, consKd, PID::Direct); //Der PID-Controller, kriegt vorerst die feinen Parameter übergeben //
//----------------------------------------------------------------------------------------------------------------------||
//---------------------------------------------------------------------------------------------------------Variablen||
int e1, e2, entfernung = 125; //Die beiden vorherigen und die durchschnittliche gemessene Entfernung //
int s1, s2, s3, s4, sollwert = 130; //Die vier vorherigen und der durchschnittliche gemessene Sollwert //
int motorwert = 90; //Der Winkel, zu dem sich der Motor drehen soll //
int gap; //Entfernung, die zum gesetzten Punkt besteht //
int zeit = 0; //Variable zur Messung der Zeit, die der Ball schon nahe am Sollwert liegt //
//Misst aber nicht genau die Zeit, ist eher zur ungefähren Abschätzung der vergangenen Zeit //
//------------------------------------------------------------------------------------------------------------------||
//=================================================================================================================================================Setup
void setup()
{
FastLED.addLeds<WS2812, LED_PIN, GRB>(leds, NUM_LEDS); //Setup des LED-Controllers
lox.begin();
VL53L0X_RangingMeasurementData_t measure; //Erste Messung für die Initialisierung des PID-Controllers
myPID.Start( //Initialisierung des PID-Controllers Dabei werden drei Variablen übergeben:
measure.RangeMilliMeter, //Die gemessene entfernung
105, //der momentane Motorwinkel (wird erst einmal auf die Mitte des nutzbaren Bereiches gesetzt)
130); //der Sollwert
myservo.attach(SERVO_OUT); //Der Motorsteuerung wird der Ansteuerungspin für den Servo übrgeben
myPID.SetOutputLimits(70, 140); //Minimal- und Maximalwert für den PID-Ausgabewert werden beschränkt, da der Servo bei Winkeln unter 70° oder über 140° die Konstruktion beschädigt.
}
//======================================================================================================================================Programmschleife
void loop()
{
//----------------------------------------------------------------wiederholendes Setup||
VL53L0X_RangingMeasurementData_t measure; //Die Entfernung des Balls wird gemessen //
lox.rangingTest(&measure, false); //konstantes Debuggen des TOF-Sensors //
//------------------------------------------------------------------------------------||
//------------Berechnung des Mittelwertes der letzten TOF-Messwerte||
e1 = e2; //Aktualisierung der Messwerte //
e2 = measure.RangeMilliMeter; //
entfernung = int((e1 + e2)/2); //Berechnung des Durchschnitts //
//-----------------------------------------------------------------||
//---------------------------------------------------------------------------------------Setzen der neuen gewünschten Entfernung||
if(map(analogRead(POTI_PIN), 0, 1023, 50, 200) - sollwert > 5 || map(analogRead(POTI_PIN), 0, 1023, 50, 200) - sollwert < -5) //
{ //
//Geschieht zur Vermeidung von Messpitzen nur, wenn sie sich um mehr als 5 bewegt //
s1 = 2; //Zusätliche Glättung der Messwerte, Verfährt wie in Zeilen 59-61 //
s2 = s3; //
s3 = s4; //
s4 = map(analogRead(POTI_PIN), 0, 1023, 70, 180); //
sollwert = int((s1 + s2 + s3 + s4)/4); //
myPID.Setpoint(sollwert); //Neuer Sollwert wird an den PID-Controller übergeben //
} //
//------------------------------------------------------------------------------------------------------------------------------||
//----------------------------------------------------------------------------------------------------------Setzen der PID-Parameter||
gap = abs(myPID.GetSetpoint() - measure.RangeMilliMeter); //Berechnet die Entfernung vom gewünschten Punkt //
if (gap < 40) //
{ //
myPID.SetTunings(consKp, consKi, consKd); //Benutzt schwache PID-Werte, da der Ball nahe am gewünschten Punkt ist //
} else { //
myPID.SetTunings(aggKp, aggKi, aggKd); //Benutzt aggressive PID-Werte, da der Ball weit vom gewünschten Punkt entfernt ist //
} //
//----------------------------------------------------------------------------------------------------------------------------------||
//------------------------------------------------------------------------------------------------------------Setzen der Punkte auf dem LED-Streifen||
for(int i = 0; i<NUM_LEDS; i++) //der LED-Streifen wird komplett dunkel geschaltet Relevante Punkte werden später neu berechnet und hinzugefügt //
{ //
leds[i] = CRGB(0, 0, 0); //
} //
//
if(map(sollwert, POS_MIN, POS_MAX, 0, NUM_LEDS - 1) == map(entfernung, POS_MIN, POS_MAX, 0, NUM_LEDS - 1)) //
{ //
//
leds[map(sollwert, POS_MIN, POS_MAX, 0, NUM_LEDS - 1)] = CRGB(255, 100, 0); //
//Befindet sich der Punkt für den Ball auf dem Punkt für den Sollwert, wird dieser orange //
if(zeit < 40) //
{ //
zeit++; //Der Punkt bleibt für Zeiten unter 40 Einheiten orange //
} //
else if(zeit < 100) //
{ //
leds[map(sollwert, POS_MIN, POS_MAX, 0, NUM_LEDS - 1)] = CRGB(0, 255, 0); //
zeit++; //
//Bleibt der Ball länger als 40 Zeiteinheiten auf dem Punkt, wird er grün //
} //
else //
{ //
leds[map(sollwert, POS_MIN, POS_MAX, 0, NUM_LEDS - 1)] = CRGB(0, 200, 200); //
//Bleibt der Ball länger als 40 Zeiteinheiten auf dem Punkt, wird er cyan //
} //
} //
else //
{ //
//Sind der Ball und der Sollwert unterschiedliche Punkte, werden sie einzeln dargestellt //
zeit = 0; //
leds[map(entfernung, POS_MIN, POS_MAX, 0, NUM_LEDS - 1)] = CRGB(255, 255, 255); //Der Punkt zur Darstellung der Ballposition wird gesetzt //
leds[map(sollwert, POS_MIN, POS_MAX, 0, NUM_LEDS - 1)] = CRGB(255, 0, 0); //Der Punkt zur Darstellung des Sollwertes wird gesetzt //
} //
FastLED.show(); //Übertragung der gesetzten Farben an den LED-Streifen //
//--------------------------------------------------------------------------------------------------------------------------------------------------||
motorwert = myPID.Run(entfernung); //Berechnung des neuen Motorwertes anhand der durchschnittlichen gemessenen Entfernung
myservo.write(motorwert); //Übertragung des Motorwertes an den Servo
}
Das fertige Projekt in Aktion:
mechatronik/wise21/kugel_balancieren.txt · Zuletzt geändert: 2022/02/18 18:16 von EinPegler
Seiten-Werkzeuge
