Projektlabor Robotik MINTgrün

Diese Seite ist nicht editierbar. Sie können den Quelltext sehen, jedoch nicht verändern. Kontaktieren Sie den Administrator, wenn Sie glauben, dass hier ein Fehler vorliegt.
Wir, de Equalizer-Gruppe, wollen hier eine Zusammenfassung unserer Präsentation im Wissenschaftsfenster hochladen.

Die Präsentationsfolien:
{{:mechatronik:wise21:ml_theequalizor_2.pdf|}}

Der Bauplan:
{{:mechatronik:wise21:bauplan.pdf|}}

Der Arduino-Sketch:
<code>
//=============================================================================================================================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
}
</code>
Projektlabor Robotik MINTgrün

Benutzer-Werkzeuge

Webseiten-Werkzeuge

Seiten-Werkzeuge
