diff --git a/examples/servo_driver_algorithm.ino b/examples/servo_driver_algorithm.ino new file mode 100644 index 0000000000000000000000000000000000000000..86230011c387f14b82c20fe28da6e9a12568e4f8 --- /dev/null +++ b/examples/servo_driver_algorithm.ino @@ -0,0 +1,164 @@ +#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); + } +}