Nahtlose Steuerung von Arduino Roboterarm auf Mecanum-Rad-Plattform

In diesem Tutorial zeige ich, wie ich meine Mecanum-Rad-Roboterplattform aus dem vorherigen Video mit meinem 3D-gedruckten Roboterarm – ebenfalls ein Arduino-Projekt aus einem früheren Video – zusammenarbeiten lasse und automatisch betreibe.

Sie können das folgende Video ansehen oder die schriftliche Anleitung unten lesen.

Überblick

Die Mecanum-Rad-Plattform lässt sich wie im vorherigen Video erklärt per selbstgebauter Android-App steuern. Neu: Die App hat nun auch Buttons zur Steuerung des Roboterarms.

Nahtlose Steuerung von Arduino Roboterarm auf Mecanum-Rad-Plattform

Die ursprüngliche Roboterarm-App nutzte Schieberegler für die Gelenke, was Stabilitätsprobleme verursachte. Mit Buttons funktioniert der Arm deutlich besser. Daher stelle ich diese aktualisierte Version der Roboterarm-App und den Arduino-Code auch für das ursprüngliche Roboterarm-Projekt bereit.

Das coolste Feature dieses Roboters ist die Fähigkeit, Bewegungen zu speichern und dann automatisch zu wiederholen.

Per „Speichern“-Button speichern wir einfach die Motorpositionen für jeden Schritt. Dann „Ausführen“ drücken – der Roboter wiederholt die gespeicherten Bewegungen endlos.

Den Arduino-Roboter bauen

Hier habe ich die Mecanum-Rad-Plattform bereits zusammengebaut – alle Details dazu im vorherigen Video.

Auch die 3D-gedruckten Teile des Roboterarms und die Servomotoren liegen bereit – jetzt geht es an den Zusammenbau. Hier das 3D-Modell des Projekts.

Das 3D-Modell finden Sie auf Thangs zum Download und zur Ansicht im Browser.

3D-Modell auf Thangs herunterladen.

STL-Dateien für 3D-Druck:

Der erste Servo des Roboterarms wird direkt auf der oberen Abdeckplatte der Mecanum-Plattform montiert.

Ich markierte die Position und bohrte mit 10mm Bohrer mehrere Löcher.

Mit einer Raspel schnitt ich die Löcher aus und feilte die Öffnung für den Servo passend. Den Servo mit vier M3-Schrauben und Muttern an der oberen Platte befestigt.

Auf die Abtriebswelle dieses Servos setzen wir den mitgelieferten Rundhorn, um das nächste Teil – den Rumpf des Roboterarms – zu befestigen. Dabei bleibt das Teil ca. 8mm über der Platte. Daher klebte ich zwei 8mm MDF-Plattenstücke darauf, damit der Rumpf darauf gleiten kann und das Gelenk stabiler wird.

Der Rundhorn wird mit den mitgelieferten Selbstschneideschrauben am Rumpfteil befestigt, dann der Rundhorn per passender Schraube an der Servowelle.

Als Nächstes der Schulter-Servo: Einfach einsetzen und mit Selbstschneideschrauben am 3D-Druckteil fixieren.

Der Rundhorn kommt auf das nächste Teil, dann werden beide Teile per Schraube an der Servowelle miteinander verbunden.

Wichtig: Vor dem Festschrauben prüfen, ob das Teil den vollen Bewegungsbereich hat. Ich fügte auch ein Gummiband am Schultergelenk hinzu, um den Servo etwas zu entlasten, da er das Gewicht des restlichen Arms und der Nutzlast trägt.

Auf ähnliche Weise baute ich den Rest des Roboterarms zusammen.

Nun der Greifer-Mechanismus. Er wird von einem SG90-Mikroservo gesteuert, an den wir zuerst ein eigens entworfenes Zahnstangen-Glied anbringen. Dieses paaren wir mit einem zweiten Zahnstangen-Glied auf der anderen Seite, befestigt per M4-Schraube und Mutter. Alle anderen Gelenke verbinden sich ebenfalls per M4-Schrauben und Muttern.

Das 3D-Modell des Greifers hat ursprünglich 3mm Löcher, aber ich hatte nicht genug M3-Schrauben, daher bohrte ich auf 4mm auf und nutzte M4-Schrauben.

Nach Zusammenbau des Greifers befestigte ich ihn am letzten Servo – der Roboterarm war fertig.

Dann Kabelmanagement: Servokabel durch die dafür vorgesehenen Öffnungen im Roboterarm führen. Mit 10mm Bohrer ein Loch in die obere Platte bohren, damit die Kabel hindurchpassen.

Alle Kabel per Kabelbinder zusammenfassen – nun fehlt nur noch der Anschluss an das Arduino-Board.

Schaltplan des Arduino-Roboters

Hier der Schaltplan des Projekts und wie alles verbunden werden muss.

Benötigte Komponenten:

Schrittmotor – NEMA 17
DRV8825 Schrittmotortreiber
MG996R Servomotor
SG90 Mikroservo
HC-05 Bluetooth Modul
Li-Po Akku
Arduino Mega Board

Im vorherigen Tutorial erklärte ich die Mecanum-Rad-Plattform und zeigte, wie ich ein eigenes PCB dafür entwarf.

Auf diesem PCB integrierte ich einen 5V-Spannungsregler (LM350, bis 3A), damit wir das Projekt erweitern oder die Servomotoren anschließen können (diese arbeiten mit 5V). Alle sechs Servos des Roboterarms ziehen zusammen ca. 2-3A – der Regler schafft das, wird aber sehr heiß.

Daher montierte ich einen Kühlkörper sowie einen kleinen 12V-Lüfter, da der Kühlkörper allein nicht ausreichte.

Die Signalleitungen der Servos verband ich mit den Arduino-Digitalpins 5 bis 10, die Stromversorgung über den 5V-Pin-Header auf dem PCB. Schließlich alle Kabel in die Plattform schieben und die obere Platte mit den zwei Muttern sichern.

Damit ist der Bau abgeschlossen.

Arduino-Code

Was bleibt, ist ein Blick auf Arduino-Code und Android-App. Da der Code länger ist, poste ich ihn zur besseren Verständlichkeit in Abschnitten mit Erläuterungen. Am Ende des Artikels folgt der komplette Quellcode.

Zuerst definieren wir die 6 Servos, 4 Schrittmotoren und die Bluetooth-Kommunikation sowie nötige Variablen. Im setup() setzen wir die Maximalgeschwindigkeit der Schrittmotoren, definieren die Servo-Pins, starten die Bluetooth-Kommunikation und fahren den Roboterarm in die Startposition.

#include <SoftwareSerial.h>
#include <AccelStepper.h>
#include <Servo.h>

Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;

SoftwareSerial Bluetooth(A8, 38); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)

// Schrittmotoren und Pins definieren
AccelStepper LeftBackWheel(1, 42, 43);  // (Type:driver, STEP, DIR) - Stepper1
AccelStepper LeftFrontWheel(1, 40, 41); // Stepper2
AccelStepper RightBackWheel(1, 44, 45); // Stepper3
AccelStepper RightFrontWheel(1, 46, 47); // Stepper4

#define led 14
int wheelSpeed = 1500;
int lbw[50], lfw[50], rbw[50], rfw[50]; // Arrays für Positionen/Schritte
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // aktuelle Position
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // vorherige Position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // zum Speichern
int speedDelay = 20;
int index = 0;
int dataIn;
int m = 0;

void setup() {
  // Maximale Schrittmotor-Geschwindigkeit setzen
  LeftFrontWheel.setMaxSpeed(3000);
  LeftBackWheel.setMaxSpeed(3000);
  RightFrontWheel.setMaxSpeed(3000);
  RightBackWheel.setMaxSpeed(3000);

  pinMode(led, OUTPUT);
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);

  Bluetooth.begin(38400); // Standard-Baudrate des Bluetooth-Moduls
  Bluetooth.setTimeout(5);
  delay(20);
  Serial.begin(38400);

  // Roboterarm in Startposition bewegen
  servo1PPos = 90;  servo01.write(servo1PPos);
  servo2PPos = 100; servo02.write(servo2PPos);
  servo3PPos = 120; servo03.write(servo3PPos);
  servo4PPos = 95;  servo04.write(servo4PPos);
  servo5PPos = 60;  servo05.write(servo5PPos);
  servo6PPos = 110; servo06.write(servo6PPos);
}Code language: Arduino (arduino)

Im loop() prüfen wir zuerst auf eingehende Daten.

// Auf eingehende Daten prüfen
if (Bluetooth.available() > 0) {
  dataIn = Bluetooth.read(); // Daten lesen
Code language: Arduino (arduino)

Diese Daten kommen vom Smartphone/Android-App. Sehen wir uns an, was die App sendet. Die App wurde mit MIT App Inventor erstellt. Sie besteht aus einfachen Buttons mit passenden Bildern als Hintergrund.

In den Blöcken der App sieht man: Sie sendet lediglich Ein-Byte-Zahlen beim Button-Klick.

Je nach geklicktem Button teilen wir dem Arduino mit, was zu tun ist. Beispiel: Empfangen wir die Zahl ‚2‘, fährt die Mecanum-Plattform vorwärts per moveForward()-Funktion.

if (dataIn == 2) { m = 2; }
// if (m == 2) { moveForward(); }Code language: Arduino (arduino)

Diese Funktion setzt alle vier Schrittmotoren auf Vorwärtsdrehung.

void moveForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}Code language: Arduino (arduino)

Für andere Richtungen drehen wir die Räder entsprechend entgegengesetzt.

Für den Roboterarm dasselbe Prinzip: Buttons in der App, beim gedrückt Halten bewegt sich das jeweilige Gelenk in die entsprechende Richtung.

Wie erwähnt nutzte die ursprüngliche Roboterarm-App Schieberegler für Servopositionen – das verursachte Probleme, weil Text statt 1-Byte-Zahlen gesendet werden musste. Der Arduino verpasste manchmal den Text, was zu Fehlern oder Zittern des Arms führte.

Jetzt senden wir einfach eine 1-Byte-Zahl beim „Touch Down“ eines Buttons.

Der Arduino-Code tritt in die while-Schleife dieser Zahl ein und bleibt dort, bis wir den Button loslassen – dann senden wir die Zahl 0 (nichts tun).

// Servo 1 in positive Richtung bewegen
while (m == 16) {
  if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
  servo01.write(servo1PPos);
  servo1PPos++;
  delay(speedDelay);
}

// Servo 1 in negative Richtung bewegen
while (m == 17) {
  if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
  servo01.write(servo1PPos);
  servo1PPos--;
  delay(speedDelay);
}Code language: Arduino (arduino)

Je nach gedrücktem Button bewegen sich die Servos in positive oder negative Richtung. Gleiches Prinzip für alle Servos. Für Geschwindigkeitsänderung nutzen wir den Schieberegler-Wert (100-250).

// Wenn Arm-Geschwindigkeits-Schieber geändert wird
if (dataIn > 101 && dataIn < 250) {
  speedDelay = dataIn / 10; // Servogeschwindigkeit ändern (Delay in µs)
}Code language: Arduino (arduino)

Durch Division durch 10 erhalten wir Werte 10-25, die als Delay in den while-Schleifen für die Servos dienen.

Zum Speichern der Roboterbewegungen sichern wir einfach die aktuellen Positionen von Servos und Schrittmotoren in Arrays, jedes Mal beim Klick auf „Speichern“.

// Wenn Button "SAVE" gedrückt
if (m == 12) {
  // Bei erstem Speichern Schrittmotoren-Position auf 0 setzen
  if (index == 0) {
    LeftBackWheel.setCurrentPosition(0);
    LeftFrontWheel.setCurrentPosition(0);
    RightBackWheel.setCurrentPosition(0);
    RightFrontWheel.setCurrentPosition(0);
  }
  lbw[index] = LeftBackWheel.currentPosition();  // Position im Array speichern
  lfw[index] = LeftFrontWheel.currentPosition();
  rbw[index] = RightBackWheel.currentPosition();
  rfw[index] = RightFrontWheel.currentPosition();
  servo01SP[index] = servo1PPos;
  servo02SP[index] = servo2PPos;
  servo03SP[index] = servo3PPos;
  servo04SP[index] = servo4PPos;
  servo05SP[index] = servo5PPos;
  servo06SP[index] = servo6PPos;
  index++; // Array-Index erhöhen
  m = 0;
}Code language: Arduino (arduino)

Beim Drücken von „Ausführen“ rufen wir die runSteps()-Funktion auf. Diese läuft per for- und while-Schleifen durch alle gespeicherten Schritte.

if (m == 14) {
  runSteps();
  // Wenn Button "RESET" gedrückt
  if (dataIn != 14) {
    stopMoving();
    memset(lbw, 0, sizeof(lbw));   // Arrays auf 0 löschen
    memset(lfw, 0, sizeof(lfw));
    memset(rbw, 0, sizeof(rbw));
    memset(rfw, 0, sizeof(rfw));
    memset(servo01SP, 0, sizeof(servo01SP));
    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; // Index auf 0
  }
}Code language: Arduino (arduino)

Wichtig: Es läuft von der ersten zur letzten Position und wiederholt das endlos. Daher muss beim Speichern die erste Position der letzten entsprechen. Während der Ausführung kann man Geschwindigkeit von Plattform und Arm ändern, pausieren und alle Schritte zurücksetzen.

Hier können Sie die App sowie die bearbeitbare Projektdatei herunterladen:

Hier der komplette Arduino-Code für dieses Arduino-Roboter-Projekt:

/* Arduino Robot Arm and Mecanum Wheels Robot Smartphone Control via Bluetooth by Dejan, www.HowToMechatronics.com */
#include <SoftwareSerial.h>
#include <AccelStepper.h>
#include <Servo.h>

Servo servo01;
Servo servo02;
Servo servo03;
Servo servo04;
Servo servo05;
Servo servo06;

SoftwareSerial Bluetooth(A8, 38); // Arduino(RX, TX) - HC-05 Bluetooth (TX, RX)

// Schrittmotoren und Pins definieren
AccelStepper LeftBackWheel(1, 42, 43);  // (Type:driver, STEP, DIR) - Stepper1
AccelStepper LeftFrontWheel(1, 40, 41); // Stepper2
AccelStepper RightBackWheel(1, 44, 45); // Stepper3
AccelStepper RightFrontWheel(1, 46, 47); // Stepper4

#define led 14
int wheelSpeed = 1500;
int lbw[50], lfw[50], rbw[50], rfw[50]; // Arrays für Positionen/Schritte
int servo1Pos, servo2Pos, servo3Pos, servo4Pos, servo5Pos, servo6Pos; // aktuelle Position
int servo1PPos, servo2PPos, servo3PPos, servo4PPos, servo5PPos, servo6PPos; // vorherige Position
int servo01SP[50], servo02SP[50], servo03SP[50], servo04SP[50], servo05SP[50], servo06SP[50]; // zum Speichern
int speedDelay = 20;
int index = 0;
int dataIn;
int m = 0;

void setup() {
  LeftFrontWheel.setMaxSpeed(3000);
  LeftBackWheel.setMaxSpeed(3000);
  RightFrontWheel.setMaxSpeed(3000);
  RightBackWheel.setMaxSpeed(3000);
  pinMode(led, OUTPUT);
  servo01.attach(5);
  servo02.attach(6);
  servo03.attach(7);
  servo04.attach(8);
  servo05.attach(9);
  servo06.attach(10);
  Bluetooth.begin(38400);
  Bluetooth.setTimeout(5);
  delay(20);
  Serial.begin(38400);

  servo1PPos = 90;  servo01.write(servo1PPos);
  servo2PPos = 100; servo02.write(servo2PPos);
  servo3PPos = 120; servo03.write(servo3PPos);
  servo4PPos = 95;  servo04.write(servo4PPos);
  servo5PPos = 60;  servo05.write(servo5PPos);
  servo6PPos = 110; servo06.write(servo6PPos);
}

void loop() {
  if (Bluetooth.available() > 0) {
    dataIn = Bluetooth.read();
    if (dataIn == 0) { m = 0; }
    if (dataIn == 1) { m = 1; }
    if (dataIn == 2) { m = 2; }
    if (dataIn == 3) { m = 3; }
    if (dataIn == 4) { m = 4; }
    if (dataIn == 5) { m = 5; }
    if (dataIn == 6) { m = 6; }
    if (dataIn == 7) { m = 7; }
    if (dataIn == 8) { m = 8; }
    if (dataIn == 9) { m = 9; }
    if (dataIn == 10) { m = 10; }
    if (dataIn == 11) { m = 11; }
    if (dataIn == 12) { m = 12; }
    if (dataIn == 14) { m = 14; }
    if (dataIn == 16) { m = 16; }
    if (dataIn == 17) { m = 17; }
    if (dataIn == 18) { m = 18; }
    if (dataIn == 19) { m = 19; }
    if (dataIn == 20) { m = 20; }
    if (dataIn == 21) { m = 21; }
    if (dataIn == 22) { m = 22; }
    if (dataIn == 23) { m = 23; }
    if (dataIn == 24) { m = 24; }
    if (dataIn == 25) { m = 25; }
    if (dataIn == 26) { m = 26; }
    if (dataIn == 27) { m = 27; }

  // Mecanum-Plattform bewegen
  if (m == 4) { moveSidewaysLeft(); }
  if (m == 5) { moveSidewaysRight(); }
  if (m == 2) { moveForward(); }
  if (m == 7) { moveBackward(); }
  if (m == 3) { moveRightForward(); }
  if (m == 1) { moveLeftForward(); }
  if (m == 8) { moveRightBackward(); }
  if (m == 6) { moveLeftBackward(); }
  if (m == 9) { rotateLeft(); }
  if (m == 10) { rotateRight(); }
  if (m == 0) { stopMoving(); }

  // Mecanum-Geschwindigkeit
  if (dataIn > 30 && dataIn < 100) { wheelSpeed = dataIn * 20; }

  // Roboterarm bewegen
  // Servo 1 positiv
  while (m == 16) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo01.write(servo1PPos);
    servo1PPos++;
    delay(speedDelay);
  }
  // Servo 1 negativ
  while (m == 17) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo01.write(servo1PPos);
    servo1PPos--;
    delay(speedDelay);
  }
  // Servo 2
  while (m == 19) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo02.write(servo2PPos);
    servo2PPos++;
    delay(speedDelay);
  }
  while (m == 18) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo02.write(servo2PPos);
    servo2PPos--;
    delay(speedDelay);
  }
  // Servo 3
  while (m == 20) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo03.write(servo3PPos);
    servo3PPos++;
    delay(speedDelay);
  }
  while (m == 21) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo03.write(servo3PPos);
    servo3PPos--;
    delay(speedDelay);
  }
  // Servo 4
  while (m == 23) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo04.write(servo4PPos);
    servo4PPos++;
    delay(speedDelay);
  }
  while (m == 22) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo04.write(servo4PPos);
    servo4PPos--;
    delay(speedDelay);
  }
  // Servo 5
  while (m == 25) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo05.write(servo5PPos);
    servo5PPos++;
    delay(speedDelay);
  }
  while (m == 24) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo05.write(servo5PPos);
    servo5PPos--;
    delay(speedDelay);
  }
  // Servo 6
  while (m == 26) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo06.write(servo6PPos);
    servo6PPos++;
    delay(speedDelay);
  }
  while (m == 27) {
    if (Bluetooth.available() > 0) { m = Bluetooth.read(); }
    servo06.write(servo6PPos);
    servo6PPos--;
    delay(speedDelay);
  }

  // Arm-Geschwindigkeitsschieber
  if (dataIn > 101 && dataIn < 250) {
    speedDelay = dataIn / 10;
  }

  // Button "SAVE"
  if (m == 12) {
    if (index == 0) {
      LeftBackWheel.setCurrentPosition(0);
      LeftFrontWheel.setCurrentPosition(0);
      RightBackWheel.setCurrentPosition(0);
      RightFrontWheel.setCurrentPosition(0);
    }
    lbw[index] = LeftBackWheel.currentPosition();
    lfw[index] = LeftFrontWheel.currentPosition();
    rbw[index] = RightBackWheel.currentPosition();
    rfw[index] = RightFrontWheel.currentPosition();
    servo01SP[index] = servo1PPos;
    servo02SP[index] = servo2PPos;
    servo03SP[index] = servo3PPos;
    servo04SP[index] = servo4PPos;
    servo05SP[index] = servo5PPos;
    servo06SP[index] = servo6PPos;
    index++;
    m = 0;
  }

  // Button "RUN"
  if (m == 14) {
    runSteps();
    if (dataIn != 14) {
      stopMoving();
      memset(lbw, 0, sizeof(lbw));
      memset(lfw, 0, sizeof(lfw));
      memset(rbw, 0, sizeof(rbw));
      memset(rfw, 0, sizeof(rfw));
      memset(servo01SP, 0, sizeof(servo01SP));
      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;
    }
  }
  }

  LeftFrontWheel.runSpeed();
  LeftBackWheel.runSpeed();
  RightFrontWheel.runSpeed();
  RightBackWheel.runSpeed();

  // Batteriespannung überwachen
  int sensorValue = analogRead(A0);
  float voltage = sensorValue * (5.0 / 1023.00) * 3; // 5V auf 12V umrechnen
  // Serial.println(voltage);
  if (voltage < 11) { digitalWrite(led, HIGH); }
  else { digitalWrite(led, LOW); }
}

void moveForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveBackward() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void moveSidewaysRight() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveSidewaysLeft() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void rotateLeft() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(wheelSpeed);
}
void rotateRight() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void moveRightForward() {
  LeftFrontWheel.setSpeed(wheelSpeed);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(wheelSpeed);
}
void moveRightBackward() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(-wheelSpeed);
  RightFrontWheel.setSpeed(-wheelSpeed);
  RightBackWheel.setSpeed(0);
}
void moveLeftForward() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(wheelSpeed);
  RightFrontWheel.setSpeed(wheelSpeed);
  RightBackWheel.setSpeed(0);
}
void moveLeftBackward() {
  LeftFrontWheel.setSpeed(-wheelSpeed);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(-wheelSpeed);
}
void stopMoving() {
  LeftFrontWheel.setSpeed(0);
  LeftBackWheel.setSpeed(0);
  RightFrontWheel.setSpeed(0);
  RightBackWheel.setSpeed(0);
}

// Automatischer Modus - gespeicherte Schritte ausführen
void runSteps() {
  while (dataIn != 13) { // Wiederholen bis "RESET"
    for (int i = 0; i <= index - 2; i++) {
      if (Bluetooth.available() > 0) {
        dataIn = Bluetooth.read();
        if (dataIn == 15) { // "PAUSE"
          while (dataIn != 14) { // Warten auf "RUN"
            if (Bluetooth.available() > 0) {
              dataIn = Bluetooth.read();
              if (dataIn == 13) { break; }
            }
          }
        }
        // Geschwindigkeits-Schieber
        if (dataIn > 100 && dataIn < 150) {
          speedDelay = dataIn / 10;
        }
        // Mecanum-Geschwindigkeit
        if (dataIn >= 30 && dataIn < 100) {
          wheelSpeed = dataIn * 10;
          dataIn = 14;
        }
      }
      LeftFrontWheel.moveTo(lfw[i]);  LeftFrontWheel.setSpeed(wheelSpeed);
      LeftBackWheel.moveTo(lbw[i]);   LeftBackWheel.setSpeed(wheelSpeed);
      RightFrontWheel.moveTo(rfw[i]); RightFrontWheel.setSpeed(wheelSpeed);
      RightBackWheel.moveTo(rbw[i]);  RightBackWheel.setSpeed(wheelSpeed);

      while (LeftBackWheel.currentPosition() != lbw[i] &&
             LeftFrontWheel.currentPosition() != lfw[i] &&
             RightFrontWheel.currentPosition() != rfw[i] &&
             RightBackWheel.currentPosition() != rbw[i]) {
        LeftFrontWheel.runSpeedToPosition();
        LeftBackWheel.runSpeedToPosition();
        RightFrontWheel.runSpeedToPosition();
        RightBackWheel.runSpeedToPosition();
      }

      // Servo 1
      if (servo01SP[i] == servo01SP[i + 1]) { }
      if (servo01SP[i] > servo01SP[i + 1]) {
        for (int j = servo01SP[i]; j >= servo01SP[i + 1]; j--) {
          servo01.write(j); delay(speedDelay);
        }
      }
      if (servo01SP[i] < servo01SP[i + 1]) {
        for (int j = servo01SP[i]; j <= servo01SP[i + 1]; j++) {
          servo01.write(j); delay(speedDelay);
        }
      }
      // Servo 2
      if (servo02SP[i] == servo02SP[i + 1]) { }
      if (servo02SP[i] > servo02SP[i + 1]) {
        for (int j = servo02SP[i]; j >= servo02SP[i + 1]; j--) {
          servo02.write(j); delay(speedDelay);
        }
      }
      if (servo02SP[i] < servo02SP[i + 1]) {
        for (int j = servo02SP[i]; j <= servo02SP[i + 1]; j++) {
          servo02.write(j); delay(speedDelay);
        }
      }
      // Servo 3
      if (servo03SP[i] == servo03SP[i + 1]) { }
      if (servo03SP[i] > servo03SP[i + 1]) {
        for (int j = servo03SP[i]; j >= servo03SP[i + 1]; j--) {
          servo03.write(j); delay(speedDelay);
        }
      }
      if (servo03SP[i] < servo03SP[i + 1]) {
        for (int j = servo03SP[i]; j <= servo03SP[i + 1]; j++) {
          servo03.write(j); delay(speedDelay);
        }
      }
      // Servo 4
      if (servo04SP[i] == servo04SP[i + 1]) { }
      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);
        }
      }
      // Servo 5
      if (servo05SP[i] == servo05SP[i + 1]) { }
      if (servo05SP[i] > servo05SP[i + 1]) {
        for (int j = servo05SP[i]; j >= servo05SP[i + 1]; j--) {
          servo05.write(j); delay(speedDelay);
        }
      }
      if (servo05SP[i] < servo05SP[i + 1]) {
        for (int j = servo05SP[i]; j <= servo05SP[i + 1]; j++) {
          servo05.write(j); delay(speedDelay);
        }
      }
      // Servo 6
      if (servo06SP[i] == servo06SP[i + 1]) { }
      if (servo06SP[i] > servo06SP[i + 1]) {
        for (int j = servo06SP[i]; j >= servo06SP[i + 1]; j--) {
          servo06.write(j); delay(speedDelay);
        }
      }
      if (servo06SP[i] < servo06SP[i + 1]) {
        for (int j = servo06SP[i]; j <= servo06SP[i + 1]; j++) {
          servo06.write(j); delay(speedDelay);
        }
      }
    }
  }
}Code language: Arduino (arduino)

Das war im Wesentlichen alles für dieses Tutorial. Das Projekt funktioniert gut, aber bitte beachten: Es ist weit von perfekt. Die automatischen Bewegungen sind nicht sehr präzise wegen Schlupf der Mecanum-Räder und der schwachen Leistung der Servomotoren. Diese billigen Servos zittern oft sogar im Stillstand, weil sie das Gewicht der 3D-Druckteile nicht halten können.

Ich hoffe, Ihnen hat dieses Tutorial gefallen und Sie haben etwas Neues gelernt. Fragen gerne in den Kommentaren und schauen Sie in meine Arduino-Projektsammlung.