Benutzer-Werkzeuge
Seitenleiste
mechatronik:wise21:kugel_balancieren
Dies ist eine alte Version des Dokuments!
Wir, de Equalizer-Gruppe, wollen hier eine Zusammenfassung unserer Präsentation im Wissenschaftsfenster hochladen.
Die Präsentationsfolien:
Der Bauplan:
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 //
//
//PID-Regler benötigen drei Parameter, die bestimmen, wie stark die momentane Position, //
//Steigung und die Änderung der Steigung des Eingabewertes zur Findung des Ausgabewertes beitragen //
//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 61-63 //
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
}
mechatronik/wise21/kugel_balancieren.1645096548.txt.gz · Zuletzt geändert: 2022/02/17 12:15 von Seraphin
Seiten-Werkzeuge
