diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..f9386221c5d1ec7e3a4da63c9732f74be1ea5d38
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+.DS_Store
+.vscode
+build
diff --git a/README.md b/README.md
index bcbc5ef3a79c71db26abd534d16e4b64c7c336d7..ee935fb272c30d2e6e799b5f147b21243cf410f8 100644
--- a/README.md
+++ b/README.md
@@ -1,92 +1,106 @@
-# thk_servo_driver
+# **ServoStep**
+Diese Klasse hat zusätzliche Funktionen für die Servosteuerung und soll die Funktionen der Servo.h-Library etwas erweitern:
+ * Ein ServoStep kann eine Zielstellung mit einer definierten Geschwindigkeit erreichen.
+ * Der Servo wird dann mit dieser Geschwindigkeit zu seiner Zielposition gefahren.
+ * Kommt ein neuer Zielwinkel während einer noch andauernden Fahrt, so wird das neue Ziel direkt gültig und das alte Ziel verworfen.
+ * Dazu wird der Zielwinkel als Referenz in ServoStep verarbeitet. Das bedeutet alle Änderungen des Zielwinkels im aufrufenden Programm werden beachtet.
+ * go() muss dazu im loop kontinuierlich gerufen werden.
+
-## Getting started
+## **Installation:**
-To make it easy for you to get started with GitLab, here's a list of recommended next steps.
+Um diese Klassen verwenden zu können, muss dieses Repository geklont und in das Library-Verzeichnis der Arduino-IDE kopiert werden.<br />
+<br />
-Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)!
+## **Anwendung:**
-## Add your files
+Zur Verwendung siehe die Beispiele:
+* Simple_Servo_Step.ino
+* Simple_Servo_Step_SerialOut.ino
+* Simple_Servo_Step_VelocityChange.ino
+* UnitTest_servo_driver.ino
+<br />
+<br />
-- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files
-- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command:
+**Erläuterung zur Klasse ServoStep**
+Einbinden der Library:
+```Arduino
+#include <thk_servo_step.h>
```
-cd existing_repo
-git remote add origin https://git-ce.rwth-aachen.de/thk_libs/microcontrollers/thk_servo_driver.git
-git branch -M main
-git push -uf origin main
-```
-
-## Integrate with your tools
+<br />
-- [ ] [Set up project integrations](https://git-ce.rwth-aachen.de/thk_libs/microcontrollers/thk_servo_driver/-/settings/integrations)
+**Instanziieren eines ServoStep-Objektes:**
-## Collaborate with your team
-
-- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/)
-- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html)
-- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically)
-- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/)
-- [ ] [Set auto-merge](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html)
+```Arduino
+ServoStep servo(Servo &_servo, int16_t *_p_ang);
+```
+Bei der Instanziierung eines Servo-Objektes wird eine Referenz auf ein Servoobjekt, ein Vorgabewinkel [Centigrad] übergeben.
-## Test and Deploy
+```Arduino
+ServoStep servo(Servo &_servo, int16_t *_p_ang, const uint16_t _sampleTime=20, int16_t _K=10);
+```
+Erläuterung der Parameter:
-Use the built-in continuous integration in GitLab.
+* `Servo`: Referenz auf ein Servoobjekt.<br />
+* `*_p_ang`: ein Vorgabewinkel [Centigrad].<br />
+* `_sampleTime`: optional: Samplezeit [ms].<br />
+* `_K`: optional: Faktor für Annäherung an das Ziel [1-10].<br />
+<br />
-- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html)
-- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing(SAST)](https://docs.gitlab.com/ee/user/application_security/sast/)
-- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html)
-- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/)
-- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html)
+**Erläuterung zu den Funktionen**
-***
+`go(a, v)`: Diese Funktion führt die Servobewegung mit der Geschwindigkeit v [Grad/s] auf den Zielwinkel a [Grad] in Schritten durch und muss deshalb im loop() kontinuierlich ausgeführt werden. Jeder Schritt hat die gleiche Dauer.
-# Editing this README
+`set_port(&Serial)`:
+Mit dieser Funktion kann eine Referenz auf ein serielles Objekt übergeben werden. Damit können Ausgaben z.B. auf dem seriellen Plotter erzeugt werden.
-When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thank you to [makeareadme.com](https://www.makeareadme.com/) for this template.
+<br />
-## Suggestions for a good README
-Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information.
+# **ServoTravel**
-## Name
-Choose a self-explaining name for your project.
+Diese Klasse hat zusätzliche Funktionen für die Servosteuerung und soll die Funktionen der Servo.h-Library etwas erweitern:
+ * Ein ServoTravel kann eine Zielstellung in 3 Phasen erreichen: RAMPUP, DRIVE, RAMPDOWN
+ * Beim RAMPUP und RAMPDOWN wird die Vorgabegeschwindigkeit halbier, um die Bewegung flüssiger zu machen.
+ * Kommt ein neuer Zielwinkel während einer noch andauernden Fahrt, so wird das neue Ziel direkt gültig und das alte Ziel verworfen.
+ * go(Winkel) bzw. go(Winkel, Geschwindigkeit) muss dazu im loop kontinuierlich gerufen werden.
-## Description
-Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors.
+## **Anwendung:**
-## Badges
-On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge.
+Zur Verwendung siehe die Beispiele:
+* servo_travel.ino
+<br />
+<br />
+
-## Visuals
-Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method.
+ **Erläuterung zur Klasse**
-## Installation
-Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection.
+Einbinden der Library:
+```Arduino
+#include <thk_servo_travel.h>
+```
+<br />
-## Usage
-Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README.
+**Instanziieren eines ServoTravel-Objektes:**
-## Support
-Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc.
+```Arduino
+ServoTravel servo(Servo &_servo, int16_t _p_ang_vel);
+```
+Bei der Instanziierung eines Servo-Objektes wird eine Referenz auf ein Servoobjekt, eine Winkelgeschwindigkeit [Grad/s] übergeben.
-## Roadmap
-If you have ideas for releases in the future, it is a good idea to list them in the README.
-## Contributing
-State if you are open to contributions and what your requirements are for accepting them.
+**Erläuterung zu den Funktionen**
-For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self.
+`go(a)`: Diese Funktion führt die Servobewegung auf den Zielwinkel a [Grad]. Sie muss im loop gerufen werden.
-You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser.
+`go(a, v)`: Diese Funktion führt die Servobewegung mit der Geschwindigkeit v [Grad/s] auf den Zielwinkel a [Grad] durch. Sie muss im loop gerufen werden.
-## Authors and acknowledgment
-Show your appreciation to those who have contributed to the project.
+`set_port(&Serial)`:
+Mit dieser Funktion kann eine Referenz auf ein serielles Objekt übergeben werden. Damit können Ausgaben z.B. auf dem seriellen Plotter erzeugt werden.
-## License
-For open source projects, say how it is licensed.
+`set_angle(Winkel)`:
+Mit dieser Funktion wird der Winkelwert der Instanz auf einen gewünschten Wert gesetzt. Das kann beispielsweise beim Start nützlich sein.
-## Project status
-If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers.
+<br />
\ No newline at end of file
diff --git a/examples/Servo_Driver_SerialOutput/Servo_Driver_SerialOutput.ino b/examples/Servo_Driver_SerialOutput/Servo_Driver_SerialOutput.ino
new file mode 100644
index 0000000000000000000000000000000000000000..e107a71e7076b62e5d351f82ca453f0b9f6f5264
--- /dev/null
+++ b/examples/Servo_Driver_SerialOutput/Servo_Driver_SerialOutput.ino
@@ -0,0 +1,44 @@
+// Dieses Beispiel veranschaulicht die Verwendung der ServoStep-Klasse.
+// Dafür werden Vorgabewinkel für einen Servo erzeugt.
+// In einem bestimmten Zeitabstand wechselt der Winkel zwischen zwei Werten.
+// Somit fährt der Servo zwischen zwei Winkelstellungen hin und her.
+// -> Der FACTOR kann zwischen 1 und 10 verändert werden
+// -> im seriellen Plotter nachvollziehen!
+
+#define SERIALOUT_FOR_PLOTTER
+
+#include "thk_servo_driver.h"
+
+#define SERVOPIN 9
+#define ANGLEVELOCITY 200
+#define FACTOR 10 // <---- hier Änderungen die Auswirkungen im ser. Plotter anschauen
+
+
+Servo servo; // Servo-Instanz
+int16_t angle; // Speicherung des Servos-Winkels
+ServoDriver sstep(servo, &angle, FACTOR); // Die Instanz zur Steuerung der Servobewegung
+ // (Servo, Samplezeit in ms, Zeiger auf Winkelwert, Entschleunigungsfaktor)
+
+void setup()
+{
+ Serial.begin(115200);
+ delay(1000);
+ servo.attach(SERVOPIN);
+ sstep.set_port(&Serial);
+ Serial.println("Maxwinkel Zielwinkel Winkelstellung Geschwindigkeit Minwinkel");
+}
+
+void loop()
+{
+ static int16_t i, a;
+ const static int16_t A[2] = {10, 170};
+ static uint32_t t;
+
+ if (millis() - t > 2000)
+ {
+ t = millis();
+ a = A[i % 2];
+ i++;
+ }
+ sstep.go(a, ANGLEVELOCITY);
+}
\ No newline at end of file
diff --git a/examples/Servo_Ramp/Servo_Ramp.ino b/examples/Servo_Ramp/Servo_Ramp.ino
new file mode 100644
index 0000000000000000000000000000000000000000..933abe38400e93e644c0db8093fa47ac0e782d53
--- /dev/null
+++ b/examples/Servo_Ramp/Servo_Ramp.ino
@@ -0,0 +1,38 @@
+// Dieses Beispiel veranschaulicht die Verwendung der ServoRampDriver-Klasse
+// Dazu wird eine Servo-Instanz und eine ServoRampDriver-Instanz gebildet.
+// ServoRampDriver steuert den Servo und ServoRampDriver bekommt
+// Geschwindigkeitsvorgaben und Winkelvorgaben im loop().
+
+#define SERIALOUT_FOR_PLOTTER
+
+#include "thk_servo_ramp_driver.h"
+
+Servo servo;
+ServoRampDriver servot(servo, 100); // Parameter: Servoinstanz, Winkelgeschwindigkeit
+
+void setup()
+{
+ Serial.begin(115200);
+ delay(1000);
+ servo.attach(9);
+ servot.set_port(&Serial);
+ servot.set_angle(170);
+ Serial.println("ServoTravelTest: Über seriellen Plotter anzeigen!");
+}
+
+void loop()
+{
+ static int16_t i, a=10, v=150;
+ const static int16_t ANGLE[2] = {10, 170};
+ const static int16_t VELOCITY[2] = {150, 50};
+ static uint32_t t;
+
+ if (millis() - t > 5000)
+ {
+ t = millis();
+ a = ANGLE[i % 2];
+ v = VELOCITY[i % 2];
+ i++;
+ }
+ servot.go(a, v);
+}
\ No newline at end of file
diff --git a/examples/Servo_VelocityChange/Servo_VelocityChange.ino b/examples/Servo_VelocityChange/Servo_VelocityChange.ino
new file mode 100644
index 0000000000000000000000000000000000000000..bdd75c861a3116bdd8f1f5190f8cbf39e657ab9c
--- /dev/null
+++ b/examples/Servo_VelocityChange/Servo_VelocityChange.ino
@@ -0,0 +1,48 @@
+
+// Dieses Beispiel veranschaulicht die Verwendung der ServoStep-Klasse.
+// Dieses Beispiel erzeugt Vorgabewinkel für einen Servo.
+// In einem bestimmten Zeitabstand wechselt der Winkel zwischen zwei Werten.
+// Somit fährt der Servo zwischen zwei Winkelstellungen hin und her.
+// Zudem wird die Winkelgeschwindigkeit verändert.
+// -> Im seriellen Plotter kann die Steuerung nachvollzogen werden.
+// -> Der FACTOR kann zwischen 1 und 10 verändert werden -> im seriellen Plotter nachvollziehen!
+
+#define SERIALOUT_FOR_PLOTTER
+
+#include "thk_servo_driver.h"
+
+#define SAMPLETIME 25
+#define SERVOPIN 9
+#define FACTOR 1 // <---- hier Änderungen die Auswirkungen im ser. Plotter anschauen
+
+
+Servo servo; // Servo-Instanz
+int16_t angle; // Speicherung des Servos-Winkels
+ServoDriver sstep(servo, SAMPLETIME, &angle, FACTOR); // Die Instanz zur Steuerung der Servobewegung
+ // (Servo, Samplezeit in ms, Zeiger auf Winkelwert, Entschleunigungsfaktor)
+
+void setup()
+{
+ Serial.begin(115200);
+ delay(1000);
+ servo.attach(SERVOPIN);
+ sstep.set_port(&Serial);
+ Serial.println("Maxwinkel Zielwinkel Winkelstellung Geschwindigkeit Minwinkel");
+}
+
+void loop()
+{
+ static int16_t i, a, v;
+ static int16_t ANGLE[2] = {10, 170};
+ static int16_t VELOCITY[2] = {150, 50};
+ static uint32_t t;
+
+ if (millis() - t > 2000)
+ {
+ t = millis();
+ a = ANGLE[i % 2];
+ v = VELOCITY[i % 2];
+ i++;
+ }
+ sstep.go(a, v);
+}
\ No newline at end of file
diff --git a/examples/Simple_Servo_Driver/Simple_Servo_Driver.ino b/examples/Simple_Servo_Driver/Simple_Servo_Driver.ino
new file mode 100644
index 0000000000000000000000000000000000000000..7c8003a6d0dac78ce784eb8162c5f24860009836
--- /dev/null
+++ b/examples/Simple_Servo_Driver/Simple_Servo_Driver.ino
@@ -0,0 +1,42 @@
+// Dieses Beispiel veranschaulicht die Verwendung der ServoStep-Klasse.
+// Dafür werden Vorgabewinkel für einen Servo erzeugt.
+// In einem bestimmten Zeitabstand wechselt der Winkel zwischen zwei Werten.
+// Somit fährt der Servo zwischen zwei Winkelstellungen hin und her.
+// -> Im seriellen Plotter kann die Steuerung nachvollzogen werden.
+// -> Bitte Änderungen von ANGLEVELOCITY experimentell nachvollziehen
+
+#define SERIALOUT_FOR_PLOTTER
+
+#include "thk_servo_driver.h"
+
+#define SERVOPIN 9
+#define ANGLEVELOCITY 80
+
+
+Servo servo; // Servo-Instanz
+int16_t angle; // Speicherung des Servos-Winkels
+ServoDriver sstep(servo, &angle); // Die Instanz zur Steuerung der Servobewegung
+ // (Servo, Zeiger auf Winkelwert)
+
+void setup()
+{
+ Serial.begin(115200);
+ delay(1000);
+ servo.attach(SERVOPIN);
+ sstep.set_port(&Serial);
+}
+
+void loop()
+{
+ static int16_t i, a;
+ const static int16_t A[2] = {10, 170};
+ static uint32_t t;
+
+ if (millis() - t > 3000)
+ {
+ t = millis();
+ a = A[i % 2];
+ i++;
+ }
+ sstep.go(a, ANGLEVELOCITY);
+}
\ No newline at end of file
diff --git a/examples/UnitTest_servo_driver/UnitTest_servo_driver.ino b/examples/UnitTest_servo_driver/UnitTest_servo_driver.ino
new file mode 100644
index 0000000000000000000000000000000000000000..edde9f8008c5579403646b70858eb7607a144e7c
--- /dev/null
+++ b/examples/UnitTest_servo_driver/UnitTest_servo_driver.ino
@@ -0,0 +1,89 @@
+#line 2 "UnitTest_servo_driver.ino"
+
+#include <AUnit.h>
+#include "thk_servo_driver.h"
+
+Servo servo;
+int16_t angle;
+ServoDriver sstep(servo, &angle);
+
+
+test(01_calc_angle_velocity) // 10Grad in 100 [ms] = 1 [Centigrad]/s
+{
+ //Achtung Angabe in: [Centigrad], [Centigrad], [ms]
+ int16_t w = sstep.calc_angle_velocity(0, 100, 100);
+ assertEqual(w, 1);
+}
+
+test(02_calc_angle_velocity) // -10Grad in 100 [ms] = -1 [Centigrad]/s
+{
+ //Achtung Angabe in: [Centigrad], [Centigrad], [ms]
+ int16_t w = sstep.calc_angle_velocity(100, 0, 100);
+ assertEqual(w, -1);
+}
+
+test(03_calc_angle_velocity) // 1Grad in 100 [ms] = 0.1 [Centigrad]/s = 0 (Integer)
+{
+ //Achtung Angabe in: [Centigrad], [Centigrad], [ms]
+ int16_t w = sstep.calc_angle_velocity(0, 10, 100);
+ assertEqual(w, 0);
+}
+
+test(04_calc_angle_velocity) // 1Grad in 11 [ms] = 0.91 [Centigrad]/s = 0 (Integer)
+{
+ //Achtung Angabe in: [Centigrad], [Centigrad], [ms]
+ int16_t w = sstep.calc_angle_velocity(0, 10, 11);
+ assertEqual(w, 0);
+}
+
+test(05_set_angle_velocity)
+{
+ // Achtung Angabe in: [Centigrad], [Centigrad], [Grad/s], [Grad/s]
+ int16_t w = sstep.set_angle_velocity(0, 10, 10, 1); // 1 Grad fahren mit 10/s oder 1/s
+ assertEqual(w, 1);
+}
+
+test(06_set_angle_velocity)
+{
+ // Achtung Angabe in: [Centigrad], [Centigrad], [Grad/s], [Grad/s]
+ int16_t w = sstep.set_angle_velocity(0, 10, 1, 10); // 1 Grad fahren mit 1/s oder 10/s?
+ assertEqual(w, 1);
+}
+
+test(07_set_angle_velocity)
+{
+ // Achtung Angabe in: [Centigrad], [Centigrad], [Grad/s], [Grad/s]
+ int16_t w = sstep.set_angle_velocity(10, 0, 1, -10); // -1 Grad fahren mit -10/s oder -1/s (Eingabe der Absolutgeschwindigkeit!)
+ assertEqual(w, -1);
+}
+
+test(08_go)
+{
+ // Test von 0 [Centigrad] nach 100 [Grad] mit Geschwindigkeit 100/[s] und einer Samplezeit von 10 [ms]
+ // sollte 100 [Centigrad] ergeben
+ //
+ // Achtung Angabe in: von [Centigrad], nach [Grad], [Grad/s], [ms]
+ int16_t angle = 0;
+ sstep.go(&angle, 100, 100, 10);
+ assertEqual(angle, 100);
+}
+
+test(09_go)
+{
+ // Test von 10000 [Centigrad] nach 0 [Grad] mit Geschwindigkeit 100/[s] und einer Samplezeit von 10 [ms]
+ // sollte 10000-100 = 9900 [Centigrad] ergeben
+ //
+ // Achtung Angabe in: von [Centigrad], nach [Grad], [Grad/s], [ms]
+ int16_t angle = 10000; //[Centigrad]
+ sstep.go(&angle, 0, 100, 10);
+ assertEqual(angle, 10000-100);
+}
+void setup()
+{
+ Serial.begin(115200);
+}
+
+void loop()
+{
+ aunit::TestRunner::run();
+}
\ No newline at end of file
diff --git a/thk_servo_driver.h b/thk_servo_driver.h
new file mode 100644
index 0000000000000000000000000000000000000000..30cd6a97597711881f453703e12519f93c480a66
--- /dev/null
+++ b/thk_servo_driver.h
@@ -0,0 +1,137 @@
+#ifndef SERVO_STEP_H
+#define SERVO_STEP_H
+
+#include <Arduino.h>
+#include <Servo.h>
+
+#define TOLERANCE_ANGLE 100 // erlaubte Differenz SOLL-IST-Winkel [Centigrad]
+
+class ServoDriver
+{
+private:
+ Servo &servo; // Servo wird in übergeordner Instanz gespeichert
+ const uint16_t sampleTime; // [ms] Zeit in ms für Taktung der Servofahrten
+ int16_t *p_ang; // [CentiGrad] Aktuelle Winkelstellung; Wird in übergeordneter Instanz gespeichert
+ uint32_t t; // [ms] lokal verwendete Zeit für Taktungssteuerung
+ int16_t K; // [] Faktor für Geschwindigkeitsanpassung vor dem Ziel
+ // 1=Servo wird langsamer falls zur Erreichen der Zielstellung noch Zeit vorhanden ist
+ // 10=Servo fährt mit angegebener Geschwindigkeit auf die Zielstellung
+ Stream *p_port; // Pointer auf Serialobjekt für Ausgaben
+
+public:
+ ServoDriver(Servo &_servo, int16_t *_p_ang, const uint16_t _sampleTime=10, int16_t _K=10)
+ : servo(_servo), sampleTime(_sampleTime), K(_K){p_ang=_p_ang;};
+
+
+
+ // Seriellen Port für Ausgaben übergeben
+ void set_port(Stream *_p_port)
+ {
+ p_port = _p_port;
+ }
+
+
+
+
+ // Aufgabe: 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(const int16_t &ang_end, const int16_t &ang_vel) // [Grad], [Grad/s]
+ {
+ if ((millis() - t) > (uint32_t)sampleTime)
+ {
+ t = millis();
+ go(p_ang, ang_end, ang_vel, sampleTime);
+ servo.write(*p_ang / 100);
+ #ifdef SERIALOUT_FOR_PLOTTER
+ // Ausgabe für seriellen Plotter:
+ print_plotter(*p_ang / 100, ang_end, ang_vel);
+ #else
+ // Ausgabe des Zielwinkels für den Step:
+ p_port->println(*p_ang / 100);
+ #endif
+ }
+
+ return abs(abs(ang_end * 100) - abs(*p_ang)) < TOLERANCE_ANGLE; // <100 [Centigrad]
+ }
+
+
+
+ // Berechnung der nötigen Winkelgeschwindigkeit, um von einem zum nächsten Punkt zu fahren
+ //
+ // Parameter: Winkel in [CentiGrad], Zeit in [ms]
+ // Rückgabe: Winkelgeschwindigkeit in [CentiGrad/ms]=10*[Grad/s]
+ int16_t calc_angle_velocity(const int16_t &a_beg, const int16_t &a_end, const uint16_t &sampletime)
+ {
+ return (a_end - a_beg) / (int16_t)sampletime;
+ }
+
+
+
+ // Entscheidung ob die Vorgabegeschwindigkeit gefahren wird, oder die berechnete Geschwindigkeit,
+ // um zu vermeiden, dass der Servo in der Samplezeit zu weit fährt.
+ //
+ // Achtung: "a_vel" ist immer positiv, da Vorgabegeschwindigkeit.
+ // Das Vorzeichen für "a_vel" wird über die Richtung bestimmt. "a_vel_calc" hat bereits ein Vorzeichen.
+ //
+ // Parameter: Winkel in [CentiGrad], Winkelgeschwindigkeit in [Grad/s]
+ // Rückgabe: Winkelgeschwindigkeit in [CentiGrad/ms]=10*[Grad/s]
+ int16_t set_angle_velocity(const int16_t &a_beg, const int16_t &a_end, const int16_t &a_vel, const int16_t &a_vel_calc)
+ {
+ return (a_end > a_beg) ? min(a_vel, a_vel_calc) : max(a_vel_calc, -a_vel);
+ };
+
+
+
+ // Berechne und Fahre den nächsten Step
+ //
+ // Parameter: Winkel in [CentiGrad], [Grad], Winkelgeschwindigkeit in [Grad/s], Zeit in [ms]
+ // Rückgabe: Servostellwert in [Grad]
+ int16_t go(int16_t *_p_ang, int16_t ang_end, int16_t ang_vel, const uint16_t &sampletime)
+ {
+ ang_end *= 100; // [Grad] -> [CentiGrad]
+
+ int16_t ang_vel_calc = calc_angle_velocity(*_p_ang, ang_end, sampletime); // [CentiGrad/ms]=10*[Grad/s]
+
+ ang_vel_calc *= K; // k*[10*Grad/s]
+
+ ang_vel = set_angle_velocity(*_p_ang, ang_end, ang_vel, ang_vel_calc); // [CentiGrad/ms]=10*[Grad/s]
+
+ *_p_ang += (ang_vel * (int16_t)sampletime) / 10; // Wert am Ursprungsort speichern in [CentiGrad]
+
+ return *_p_ang / 100;
+ };
+
+
+
+ // Serielle Ausgabe der zu fahrenden Winkel und der Winkelgeschwindigkeit
+ //
+ // Umschalteoption der Ausgabe per Präprozessor-Direktive:
+ // SERIALOUT_FOR_PLOTTER : zeichnet einen Verlauf von: Zielwinkel, aktueller Winkel, Winkel-Geschwindigkeit
+ // #else : gibt nur den aktuellen Winkel aus
+ void print_plotter(const int16_t &ang, const int16_t &ang_end, const int16_t &ang_vel)
+ {
+ if (p_port != NULL)
+ {
+ // Ausgabe des Zielwinkels, aktuellen Winkels, der Winkelgeschwindigkeit für den Step:
+ p_port->print(180);
+ p_port->print(F(" "));
+ // p_port->print(F(" Ziel="));
+ p_port->print(ang_end);
+ p_port->print(F(" "));
+ // p_port->print(F("Aktuell="));
+ p_port->print(ang);
+ p_port->print(F(" "));
+ // p_port->print(F("Geschw="));
+ p_port->print(ang_vel);
+ p_port->print(F(" "));
+ p_port->print(0);
+ p_port->print(F(" "));
+ p_port->println();
+ }
+ }
+};
+
+#endif
diff --git a/thk_servo_ramp_driver.h b/thk_servo_ramp_driver.h
new file mode 100644
index 0000000000000000000000000000000000000000..51684137751ba386aa9c9842f3c5bd97b748d752
--- /dev/null
+++ b/thk_servo_ramp_driver.h
@@ -0,0 +1,100 @@
+#ifndef SERVO_STEPCHAIN_H
+#define SERVO_STEPCHAIN_H
+
+#include <Arduino.h>
+#include <Servo.h>
+#include "thk_servo_driver.h"
+
+#define RAMP_VELOCITY_REDUCTION 50 // [%] Geschwindigkeit wird um x% reduziert
+#define RAMP_WAY_PERCENTAGE 15 // [%] Anteil des Weges, der mit RAMP-Geschwindigkeit gefahren werden soll
+
+enum StepState {RAMPUP, DRIVE, RAMPDOWN, STAY};
+
+class ServoRampDriver
+{
+ private:
+ Servo &servo; // Servo wird in übergeordner Instanz gespeichert
+ const uint8_t sampleTimeServo = 50; // [ms] Zeit in ms für Taktung der Servofahrten
+ int16_t ang = 9000; // [CentiGrad] Aktueller Winkel: Wird in dieser Instanz gespeichert und an die "steps" durchgereicht
+ int16_t ang_beg; // [Grad] Startwinkel
+ int16_t ang_vel_ramp; // [Grad/s]
+ int16_t ang_vel_drive; // [Grad/s]
+ uint32_t t; // [ms] Zeit für Taktungssteuerung
+ StepState sstate; // Servozustand
+ ServoDriver* sstep;
+ Stream* p_port; // Pointer auf Serialobjekt für Ausgaben
+
+ public:
+ ServoRampDriver(Servo &_servo, int16_t _ang_vel_drive): servo(_servo), ang_vel_drive(_ang_vel_drive)
+ {
+ sstate = STAY;
+ ang_vel_ramp = ang_vel_drive * RAMP_VELOCITY_REDUCTION / 100;
+
+ if (sstep == NULL)
+ sstep = new ServoDriver(servo, &ang, sampleTimeServo);
+ };
+
+ void set_port(Stream* _p_port)
+ {
+ p_port = _p_port;
+ sstep->set_port(_p_port);
+ }
+
+ void set_angle(int16_t _ang)
+ {
+ ang = _ang * 100;
+ }
+
+ bool go(const int16_t& _ang_end, int16_t _ang_vel_drive)
+ {
+ ang_vel_drive = _ang_vel_drive;
+ ang_vel_ramp = ang_vel_drive * RAMP_VELOCITY_REDUCTION / 100;
+ return go(_ang_end);
+ }
+
+ bool go(const int16_t& _ang_end)
+ {
+ bool result = false;
+ static int16_t old_ang;
+ static int16_t way;
+
+ if (_ang_end != old_ang)
+ {
+ ang_beg = ang / 100;
+ old_ang = _ang_end;
+ way = _ang_end - ang_beg;
+ sstate = RAMPUP;
+ }
+
+ switch (sstate)
+ {
+ case STAY:
+ //sstep->go(ang, 0);
+ break;
+ case RAMPUP:
+ if (sstep->go(ang_beg + way * RAMP_WAY_PERCENTAGE/100, ang_vel_ramp))
+ {
+ sstate = DRIVE;
+ }
+ break;
+ case DRIVE:
+ if (sstep->go(_ang_end - way * RAMP_WAY_PERCENTAGE/100, ang_vel_drive))
+ {
+ sstate = RAMPDOWN;
+ }
+ break;
+ case RAMPDOWN:
+ if (sstep->go(_ang_end, ang_vel_ramp))
+ {
+ sstate = STAY;
+ result = true;
+ }; break;
+
+ };
+ return result;
+ }
+
+
+};
+
+#endif