#include <SoftwareSerial.h>//importiert die SoftwareSerial-Bibliothek, um serielle Kommunikation auf anderen Pins als den Standortpins durchzuführen
#include <Servo.h>// importiert die Servo-Bibliothek, die für die Steuerung von Servomotoren genutzt werden
Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;// jede Zeile definiert ein Servo-Objekt, das zum Steuern eines Servomotors verwendet wird
SoftwareSerial Bluetooth(13 , 12); // erstellt eine SoftwareSerial-Verbindung auf den Pins 13(TX) und 12 (RX), um mit den HC-05-Bluetooth-Modul zu kommunizieren
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // speichert die aktuelle Position jedes Servos
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // previous position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // Array für jeden Servo, um bis zu 50 Positionen zu speichern
int speedDelay = 20;// Geschwindigkeit der Servo-Bewegung (Verzögerung zwischen den Bewegungen)
int index = 0;// Variable für den Index der gespeicherten Positionen im Array
String dataIn = "";// Variable zum Speichern von Daten, die über Bluetooth empfangen werden
void setup() {// diese Funktion wird nur einmal ausgeführt
Serial.begin(9600);// initialisiert die serielle Kommunikation mit einer Baudrate von 9600
servo01.attach(5);// verbindet jeden Servo mit einem bestimmten Pin am Elegoo
servo02.attach(6);
servo03.attach(7);
servo04.attach(8);
servo05.attach(9);
servo06.attach(10);
Bluetooth.begin(9600); // initialisiert die Bluetooth-Kommunikation mit einer Baudrate von 9600
Bluetooth.setTimeout(1);// setzt ein Timeout von 1ms für die Bluetooth-Kommunikation
delay(20);// wartet 20ms, um die initiale Verbindung zu stabilisieren
servo1PPos = 90;//setzt die Anfangsposition jedes Servos auf 90°
servo01.write(servo1PPos);
servo2PPos = 90;
servo02.write(servo2PPos);
servo3PPos = 90;
servo03.write(servo3PPos);
servo4PPos = 90;
servo04.write(servo4PPos);
servo5PPos = 90;
servo05.write(servo5PPos);
servo6PPos = 90;
servo06.write(servo6PPos);
Serial.println("init ok");
}
void loop() {// diese Funktion läuft kontinuierlich
// put your main code here, to run repeatedly:
// Check for incoming data
if (Serial.available() > 0){
dataIn = Serial.readStringUntil( '\n') ;
Serial.print(state + "\n")
if (Bluetooth.available() > 0) {// prüft, ob Daten vom Bluetooth-Modul empfangen wurden. Bluetooth.available()gibt die Anzahl der empfangenden Zeichen zurück
dataIn = Bluetooth.readString(); // liest Daten als String und speichert sie in der Variablen data.In
Serial.println("bluetooth ok");// gibt "Bluetooth ok" auf dem seriellen Monitor aus, um anzuzeigen, dass Daten erfolgreich empfangen wurden
Serial.println(dataIn);//gibt die empfangenen Daten (dataIn) aus. printIn sorgt dafür, dass die Ausgabe in einer neuen Zeile endet
// If "Waist" slider has changed value - Move Servo 1 to position
if (dataIn.startsWith("s1")) {// prüft, ob der empfangene Befehl (dataIn) mit "s1" beginnt. Dies weist darauf hin, dass es sich um einen Befehl für Servo1 handelt
String dataInS = dataIn.substring(2, dataIn.length()); // extrahiert die Zahl(die Zielpositions des Servos) aus dem String
servo1Pos = dataInS.toInt(); // konvertiert den extrahierten String in einen Ganzzahlwert und speichert ihn in der Variablen servo01Pos
Serial.print("Servo 1: ");// gibt "Servo1" auf dem seriellen Monitor aus, um anzuzeigen, dass ein Befehl für Servo1 empfangen wurde
Serial.println(servo1Pos); // gibt die Zielposition von Servo1 auf dem seriellen Monitor aus
// If previous position is bigger then current position
if (servo1PPos > servo1Pos) {// prüft, ob die neue Zielposition (servo01Pos) größer ist als die aktuelle Position (servo01PPos).Wenn ja, bedeutet dies, dass sich der Servo nach vorne bewegen muss
for ( int j = servo1PPos; j >= servo1Pos; j--) { // Schleife, die den Servo Schritt für Schritt von der aktuellen Position (servo01PPos) zur Zielposition(servo01Pos) bewegt
servo01.write(j);// sendet Winkel j an den Servo. Der Servo wird auf die entsprechende Position bewegt
delay(20); // wartet 20ms zwischen den Schritten. Dadurch wird die Bewegung des Servos langsamer und kontrollierter
}
}
// If previous position is smaller then current position
if (servo1PPos < servo1Pos) {// wenn die Zielposition(servo01Pos) kleiner ist als die aktuelle Position (servo01PPos), bewegt sich der Servo rückwärts
for ( int j = servo1PPos; j <= servo1Pos; j++) { // Schleife bewegt den Servo schrittweise rückwärts von der aktuellen Position zur Zielposition
servo01.write(j);// sendet Winkel j an den Servo. Der Servo bewegt sich entsprechend
delay(20);
}
}
servo1PPos = servo1Pos; // aktualisiert die aktuelle Position des Servos (servo01Pos) und speichert sie als vorherige Position (servo01PPos). Dies stellt sicher, dass die zukünftige Bewegung von der richtigen Starposition ausgeht
}
// Der gleiche Prozess folgt für alle anderen Servos, allerdings mit anderen Variablen-> der Code funktioniert nach dem gleichen Muster
// Move Servo 2
if (dataIn.startsWith("s2")) {
String dataInS = dataIn.substring(2, dataIn.length());
servo2Pos = dataInS.toInt();
Serial.print("Servo 2: ");
Serial.println(servo2Pos);
if (servo2PPos > servo2Pos) {
for ( int j = servo2PPos; j >= servo2Pos; j--) {
servo02.write(j);
delay(50);
}
}
if (servo2PPos < servo2Pos) {
for ( int j = servo2PPos; j <= servo2Pos; j++) {
servo02.write(j);
delay(50);
}
}
servo2PPos = servo2Pos;
}
// Move Servo 3
if (dataIn.startsWith("s3")) {
String dataInS = dataIn.substring(2, dataIn.length());
servo3Pos = dataInS.toInt();
Serial.print("Servo 3: ");
Serial.println(servo3Pos);
if (servo3PPos > servo3Pos) {
for ( int j = servo3PPos; j >= servo3Pos; j--) {
servo03.write(j);
delay(30);
}
}
if (servo3PPos < servo3Pos) {
for ( int j = servo3PPos; j <= servo3Pos; j++) {
servo03.write(j);
delay(30);
}
}
servo3PPos = servo3Pos;
}
// Move Servo 4
if (dataIn.startsWith("s4")) {
String dataInS = dataIn.substring(2, dataIn.length());
servo4Pos = dataInS.toInt();
Serial.print("Servo 4: ");
Serial.println(servo4Pos);
if (servo4PPos > servo4Pos) {
for ( int j = servo4PPos; j >= servo4Pos; j--) {
servo04.write(j);
delay(30);
}
}
if (servo4PPos < servo4Pos) {
for ( int j = servo4PPos; j <= servo4Pos; j++) {
servo04.write(j);
delay(30);
}
}
servo4PPos = servo4Pos;
}
// Move Servo 5
if (dataIn.startsWith("s5")) {
String dataInS = dataIn.substring(2, dataIn.length());
servo5Pos = dataInS.toInt();
Serial.print("Servo 5: ");
Serial.println(servo5Pos);
if (servo5PPos > servo5Pos) {
for ( int j = servo5PPos; j >= servo5Pos; j--) {
servo05.write(j);
delay(30);
}
}
if (servo5PPos < servo5Pos) {
for ( int j = servo5PPos; j <= servo5Pos; j++) {
servo05.write(j);
delay(30);
}
}
servo5PPos = servo5Pos;
}
// Move Servo 6
if (dataIn.startsWith("s6")) {
String dataInS = dataIn.substring(2, dataIn.length());
servo6Pos = dataInS.toInt();
Serial.print("Servo 6: ");
Serial.println(servo6Pos);
if (servo6PPos > servo6Pos) {
for ( int j = servo6PPos; j >= servo6Pos; j--) {
servo06.write(j);
delay(30);
}
}
if (servo6PPos < servo6Pos) {
for ( int j = servo6PPos; j <= servo6Pos; j++) {
servo06.write(j);
delay(30);
}
}
servo6PPos = servo6Pos;
}
// If button "SAVE" is pressed
if (dataIn.startsWith("SAVE")) {// prüft, ob der empfangene Befehl "SAVE"ist. Dies bedeutet, dass die aktuellen Servo-Positionen gespeichert werden sollen
Serial.println("SAVE");//gibt "SAVE" auf dem seriellen Monitor aus, um zu bestätigen, dass der Befehl empfangen wurde
servo01SP[index] = servo1PPos; // speichert die aktuellen Positionen der Servos (servo01PPos bis servo06PPos) in den Arrays servo01SP,servo02SP, etc.. Der Index gibt an, an welcher Stelle die Positionen im Array gespeichert werden
servo02SP[index] = servo2PPos;
servo03SP[index] = servo3PPos;
servo04SP[index] = servo4PPos;
servo05SP[index] = servo5PPos;
servo06SP[index] = servo6PPos;
index++; // erhöht den Index, sodass die nächste gespeicherte Position in einem neuen Slot des Arrays landet
}
// If button "RUN" is pressed
if (dataIn.startsWith("RUN")) {//prüft, ob der empfangende Befehl "Run" ist. Dieser Befehl weist das Programm an, die gespeicherten Schritte auszuführen
Serial.println("RUN");//gibt "RUN" auf dem seriellen Monitor aus, um zu bestätigen, dass der Befehl empfangen wurde
runservo(); // ruft die Funktion runServo() auf, die die gespeicherten Schritte abarbeitet
}
// If button "RESET" is pressed
if ( dataIn == "RESET") {//prüft, ob der empfangende Befehl "RESET" ist. Dieser Befehl löscht alle gespeicherten Schritte
Serial.println("RESET");// gibt "RESET" auf dem seriellen Monitor aus, um zu bestätigen, dass der Befehl empfangen wurde
memset(servo01SP, 0, sizeof(servo01SP)); // setzt alle Werte in den Arrays servo01SP bis Servo06SP auf 0. Dadurch werden die gespeicherten Schritte gelöscht
memset(servo02SP, 0, sizeof(servo02SP));
memset(servo03SP, 0, sizeof(servo03SP));
memset(servo04SP, 0, sizeof(servo04SP));
memset(servo05SP, 0, sizeof(servo05SP));
memset(servo06SP, 0, sizeof(servo06SP));
index = 0; // setzt den Index auf 0 zurück, sodass neue Schritte wieder ab der ersten Position im Array gespeichert werden
}
}
}
// Automatic mode custom function - run the saved steps
void runservo() {// definiert die Funktion, die die gespeicherten Schritte wiederholt ausführt, bis der "RESET"-Befehl empfangen wird
while (dataIn != "RESET") { // führt Schleife kontinuierlich aus, solange der "RESET"-Befehl niicht empfangen wird
for (int i = 0; i <= index - 2; i++) { // durchläuft alle gespeicherten Schritte
if (Bluetooth.available() > 0) { // prüft, ob neue Daten über Bluetooth empfangen wurden
dataIn = Bluetooth.readString();// liest die empfangenen Daten
if ( dataIn == "PAUSE") { // prüft, ob der "PAUSE"-Befehl empfangen wurde. Wenn ja, pausiert die Ausführung
while (dataIn != "RUN") { // wartet, bis der "RUN"-Befehl empfangen wird, um die Ausführung fortzusetzen
if (Bluetooth.available() > 0) {
dataIn = Bluetooth.readString();
if ( dataIn == "RESET") { // wenn der "RESET"-Befehl während der Pause empfangen wird, bricht die Schleife ab
break;
}
}
}
}
// If speed slider is changed
if (dataIn.startsWith("ss")) {//überprüft, ob der empfangende String (dataIn) mit den Buchstaben "ss" beginnt
String dataInS = dataIn.substring(2, dataIn.length());//schneidet die Zeichenkette so zu, dass nur die Zahl nach dem ss übrig bleibt
speedDelay = dataInS.toInt(); // wandelt String in eine Ganzzahl und speichert diesen Wert in speedDelay -> dieser Wert wird später genutzt, um die Geschwindigkeit für die Bewegung der Servos festzulegen
}
}
// Servo 1
if (servo01SP[i] == servo01SP[i + 1]) {//überprüft, ob die aktuelle Servo-Position (servo01SP[i]) gleich der nächsten Position (servo01SP[i + 1]) ist
Serial.println("Servo 1 Position: Kein Wechsel"); // gibt eine Nachricht auf dem seriellen Monitor aus, um anzuzeigen, dass kein Positionswechsel stattfindet
// Elegoo sendet jede Servoposition zeilenweise an seriellen Monitor-> jede Position ist klar und getrennt sichtbar
}
if (servo01SP[i] > servo01SP[i + 1]) {// überprüft, ob die aktuelle Position größer ist als die nächste Position -> trifft dies zu, muss der Servomotor seine Position schrittweise verringern
for ( int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {//startet Schleife, die den Servo von seiner aktuellen Position bis zur Zielposition in Einzelschritten bewegt, j-- verringert die Position in jedem Schritt um 1
servo01.write(j);// bewegt Servo zu der aktuellen Position j
delay(speedDelay);// wartet eine Zeitspanne, die durch speedDelay definiert ist, bevor die nächste Bewegung ausgeführt wird
Serial.print("Servo 1 Position: ");// gibt die neue Position des Servos auf dem seriellen Monitor aus
Serial.println(j); // jede Position wird in eine neue Zeile geschrieben
}
}
if (servo01SP[i] < servo01SP[i + 1]) {//überprüft, ob die aktuelle Position kleiner ist als die nächste Position-> trifft dies zu, muss der Servo seine Position schrittweise erhöhen
for ( int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {//startet Schleife, die Servo von seiner aktuellen Position bis zur Zielposition in Einzelschritten bewegt, j++ erhöht die Position in jedem Schritt um 1
servo01.write(j);// bewegt Servo zu der aktuellen Position j
delay(speedDelay);// wartet eine Zeitspanne, die durch speedDelay definiert ist, bevor die nächste Bewegung ausgeführt wird
Serial.print("Servo 1 Position: ");// gibt die neue Position des Servos auf dem seriellen Monitor aus
Serial.println(j); // jede Position wird in eine neue Zeile geschrieben
}
}
// Code für die anderen Servos funktioniert genauso wie für Servo1, allerdings variieren die Variablen
// Servo 2
if (servo02SP[i] == servo02SP[i + 1]) {
Serial.println("Servo 2 Position: Kein Wechsel"); // Nachricht über keinen Positionswechsel optional
}
if (servo02SP[i] > servo02SP[i + 1]) {
for ( int j = servo02SP[i]; j >= servo02SP[i + 1]; j--) {
servo02.write(j);
delay(speedDelay);
Serial.print("Servo 2 Position: ");
Serial.println(j); // jede Position wird in eine neue Zeile geschrieben
}
}
if (servo02SP[i] < servo02SP[i + 1]) {
for ( int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {
servo02.write(j);
delay(speedDelay);
Serial.print("Servo 2 Position: ");
Serial.println(j); // jede Position wird in eine neue Zeile geschrieben
}
}
// Servo 3
if (servo03SP[i] == servo03SP[i + 1]) {
Serial.println("Servo 3 Position: Kein Wechsel");// Nachricht über keinen Positionswechsel optional
}
if (servo03SP[i] > servo03SP[i + 1]) {
for ( int j = servo03SP[i]; j >= servo03SP[i + 1]; j--) {
servo03.write(j);
delay(speedDelay);
Serial.print("Servo 3 Position: ");
Serial.println(j); // jede Position wird in eine neue Zeile geschrieben
}
}
if (servo03SP[i] < servo03SP[i + 1]) {
for ( int j = servo03SP[i]; j <= servo03SP[i + 1]; j++) {
servo03.write(j);
delay(speedDelay);
Serial.print("Servo 3 Position: ");
Serial.println(j); // jede Position wird in eine neue Zeile geschrieben
}
}
// Servo 4
if (servo04SP[i] == servo04SP[i + 1]) {
Serial.println("Servo 4 Position: Kein Wechsel"); // Nachricht über keinen Positionswechsel optional
}
if (servo04SP[i] > servo04SP[i + 1]) {
for ( int j = servo04SP[i]; j >= servo04SP[i + 1]; j--) {
servo04.write(j);
delay(speedDelay);
}
}
if (servo04SP[i] < servo04SP[i + 1]) {
for ( int j = servo04SP[i]; j <= servo04SP[i + 1]; j++) {
servo04.write(j);
delay(speedDelay);
Serial.print("Servo 4 Position: ");
Serial.println(j);// automatischer Zeilenumbruch wichtig für richtiges Lesen
}
}
// Servo 5
if (servo05SP[i] == servo05SP[i + 1]) {
Serial.println("Servo 5 Position: Kein Wechsel");
}
if (servo05SP[i] > servo05SP[i + 1]) {
for ( int j = servo05SP[i]; j >= servo05SP[i + 1]; j--) {
servo05.write(j);
delay(speedDelay);
Serial.print("Servo 5 Position: ");
Serial.println(j);
}
}
if (servo05SP[i] < servo05SP[i + 1]) {
for ( int j = servo05SP[i]; j <= servo05SP[i + 1]; j++) {
servo05.write(j);
delay(speedDelay);
Serial.print("Servo 5 Position: ");
Serial.println(j); // jede Position wird in eine neue Zeile geschrieben
}
}
// Servo 6
if (servo06SP[i] == servo06SP[i + 1]) {
Serial.println("Servo 6 Position: Kein Wechsel"); // Nachricht über keinen Positionswechsel optional
}
if (servo06SP[i] > servo06SP[i + 1]) {
for ( int j = servo06SP[i]; j >= servo06SP[i + 1]; j--) {
servo06.write(j);
delay(speedDelay);
Serial.print("Servo 6 Position: ");
Serial.println(j);
}
}
if (servo06SP[i] < servo06SP[i + 1]) {
for ( int j = servo06SP[i]; j <= servo06SP[i + 1]; j++) {
servo06.write(j);
delay(speedDelay);
Serial.print("Servo 6 Position: ");
Serial.println(j);
}
}
}
}
}
Must I correct something in the upper part as well?
