Neue Datei hochladen

examples/servo_driver_algorithm.ino

+#include <Servo.h>
+
+Servo myservo;
+
+void setup()
+{
+  Serial.begin(115200);
+  myservo.attach(9);
+}
+
+void loop()
+{
+  // ------------------------------------------------
+  // ------------------------------------------------
+  // Variablendeklarationen:
+  // - Variable für Sample-Zeitsteuerung [ms]
+  // - Zielwinkelvariable in [Grad]
+  // - Winkelgeschwindigkeit in [Grad/s]
+  // ------------------------------------------------
+  // ------------------------------------------------
+
+  static unsigned long timestamp;
+  int ang_set;
+  int velocity;
+
+  // ------------------------------------------------
+  // ------------------------------------------------
+  // Ein Zeitsteuerung, um den Zielwinkel im
+  // zeitlichen Abstand zu verändern:
+  // ------------------------------------------------
+  // ------------------------------------------------
+
+  if ((millis() - timestamp) > 8000)
+    timestamp = millis();
+  else if ((millis() - timestamp) > 4000)
+  {
+    ang_set = 90;
+    velocity = 25;
+  }
+  else
+  {
+    ang_set = 0;
+    velocity = 50;
+  }
+
+  // ------------------------------------------------
+  // ------------------------------------------------
+  // Aufruf der Funktion, um den Zielwinkel in der
+  // gewählten Geschwindigkeit anzufahren:
+  // ------------------------------------------------
+  // ------------------------------------------------
+
+  go(ang_set, velocity);
+
+  // ------------------------------------------------
+  // ------------------------------------------------
+}
+
+
+/*
+
+  // Aufgabe der Funktion: Fahre den nächsten Step, falls Samplezeit "abgelaufen"
+  // Verwendung: Zyklischer Aufruf im loop()
+  //
+  // Parameter: Winkel in [Grad], Winkelgeschwindigkeit in [Grad/s]
+  // Rückgabe: TRUE=falls <0.1 Grad (TOLERANCE_ANGLE) vom Ziel entfernt
+
+  bool go(int ang_end, int ang_vel) // [Grad], [Grad/s]
+  {
+  // ------------------------------------------------
+  // ------------------------------------------------
+  // Variablendeklarationen:
+  // - Variable t [ms] für Zeitpunkt zur Steuerung der Samplezeit
+  // - SAMPLETIME [ms] ist die Zeit in der der Servo den nächsten Zielwinkel bekommt
+  // - Variable ang_actual [CentiGrad], um den aktuellen Winkel zu speichern
+  // ------------------------------------------------
+  // ------------------------------------------------
+
+  unsigned long t;
+  const int SAMPLETIME = 10;
+  static int ang_actual;
+
+  // ------------------------------------------------
+  // ------------------------------------------------
+
+  if ((millis() - t) > SAMPLETIME)
+  {
+    // Zeit dieser Verarbeitung merken, um den nächsten Samplezeitraum erkennen zu können
+    t = millis();
+
+    // Umwandlung Zielwinkel von [Grad] -> 100[CentiGrad]
+    ang_end *= 100;
+
+    // Umwandlung Winkelgeschwindigkeit von [Grad/s] -> 1/10[CentiGrad/ms]
+    ang_vel /= 10;
+
+    // Berechne die Winkelgeschwindigkeit, die nötig ist, um in der SAMPLEZEIT zum Ziel zu fahren.
+    // Diese nehmen wir dann, wenn wir sehr nah am Zielwinkel sind. Sonst nicht!
+    // [CentiGrad/ms] = ( [CentiGrad] - [CentiGrad] ) / [ms]
+    int ang_vel_calc = (ang_end - ang_actual) / SAMPLETIME;
+
+    // Prüfe jetzt die Drehrichtung und wähle entweder die übergebdene Winkelgeschwindigkeit oder die berechnete
+    // [CentiGrad/ms]
+    ang_vel = (ang_end > ang_actual) ? min(ang_vel, ang_vel_calc) : max(-ang_vel, ang_vel_calc);
+
+    // Berechne den Winkelstellwert für dieses Sample.
+    // Wert speichern in [CentiGrad]
+    ang_actual += (ang_vel * SAMPLETIME);
+
+    // Stelle den Servo auf die gewünschte Gradstellung:
+    //myservo.write(ang_actual / 100);
+    serial_plot(String(ang_end) + " " + String(ang_vel) + " " + String(ang_actual));
+  }
+
+  // Rückgabe: TRUE=falls <0.1 Grad (TOLERANCE_ANGLE) vom Ziel entfernt, sonst FALSE
+  return abs(abs(ang_end * 100) - abs(ang_actual)) < 0.1;
+  }
+*/
+
+bool go(int ang_end, int ang_vel) // Parameter in [Grad], [Grad/s]
+{ 
+  static unsigned long t = 0; // [ms] Zeitmerker für Taktung
+  const int SAMPLETIME = 10;  // [ms] Taktungszeit
+  static int ang_actual = 0;  // [CentiGrad] Variable für Winkelwert
+  ang_end *= 100;             // [CentiGrad] Zielwinkel auf Hundertstelgrad skalieren.
+
+  // Geschwindigkeit als Grad pro SAMPLETIME Millisekunden anpassen, skaliert auf Hundertstelgrad.
+  // Die Berechnung hier bleibt unverändert, beachtet jedoch die Richtung der Anpassung im weiteren Verlauf.
+  int ang_diff = ang_end - ang_actual; // Differenz zwischen aktuellem und Zielwinkel
+  int direction = (ang_diff >= 0) ? 1 : -1; // Richtung der Bewegung: 1 für Zunahme, -1 für Abnahme
+  int ang_step_max = direction * (ang_vel * SAMPLETIME) / 10; // Skalierte maximale Schrittgröße pro SAMPLETIME.
+
+  if ((millis() - t) >= SAMPLETIME) 
+  {
+    t = millis();
+
+    // Berechnung des tatsächlichen Schrittes basierend auf der maximalen Schrittgröße und der verbleibenden Differenz.
+    int ang_step = direction * min(abs(ang_step_max), abs(ang_diff));
+
+    ang_actual += ang_step; // Aktualisieren des aktuellen Winkels
+
+    // Ausgabe zur Überwachung: Ziel, aktuelle Geschwindigkeit und aktueller Winkel
+    Serial.print("Ziel: ");
+    Serial.print(ang_end / 100);
+    Serial.print("°, Aktuelle Geschw.: ");
+    Serial.print(ang_vel * direction / 10);
+    Serial.print("°/s, Aktueller Winkel: ");
+    Serial.println(ang_actual / 100);
+    myservo.write(ang_actual / 100);
+  }
+
+  // Überprüfen, ob der Zielwinkel erreicht ist (Hier wird eine Toleranz von 1° = 100 Hundertstelgrad berücksichtigt)
+  return abs(ang_end - ang_actual) < 100;
+}
+
+void serial_plot(String v)
+{
+  static unsigned long t;
+  if (millis() - t > 100)
+  {
+    t = millis();
+    Serial.println(v);
+  }
+}