Hauptsteuerung Arduino
Achtung: Momentan finden Sie nur den durchkommentierten Code für die Hauptsteuerung des Roboterarms. Eine ausführliche Dokumentation folgt in Zukunft.
Hier finden Sie den komplett kommentierten Arduino-Code für die Steuerung des Roboterarms (Arduino Uno), welcher die Bewegungen aus den übergebenen Werten von der Controller- & Gestensteuerung berechnet.
// Bibliotheken für das LCD-Display mit HD44780 Controller
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
// Bibliotheken für die Steuerung der Servomotoren mit dem Motorino Shield
#include <Adafruit_BusIO_Register.h>
#include <Adafruit_I2CDevice.h>
#include <Adafruit_I2CRegister.h>
#include <Adafruit_SPIDevice.h>
#include <Adafruit_PWMServoDriver.h>
//Bibliotheken für das NRF Modul (NRF24L01)
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
// Variableninitialisierung
// Variablen für die Servomotoren
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 150
#define SERVOMAX 600
#define USMIN 600
#define USMAX 2400
#define SERVO_FREQ 50
// Variable für LCD-Display
LiquidCrystal_I2C lcd(0x27, 16, 2);
// Variablen für NRF
RF24 radio(6, 7); // CE, CSN
const byte address[6] = "00001";
// Eigene Variablen zur Speicherung von Inputs vom Controller und der Gestensteuerung
int controlmode = 1; // Speichert welche Steuerung gerade ausgewertet werden soll (1 = Controller, 2 = Gestensteuerung)
int receivedActions[6]; // Speichert in einem Array mit 6 Positionen alle erhaltenen Befehle von der Gestensteuerung über die serielle Schnittstelle
int controllerValues[3]; // Speichert ine einem Array mit 3 Positionen alle erhaltenen Werte von dem Controller (Joystick vX und vY sowie den analog ausgelesenen Drehschalter, welcher durch die angelöteten Wiederstände beim analogen Auslesen immer einen anderen Wert hat.)
int axis = 0; // Speichert welcher Servo gerade angesprochen werden soll, damit dass das ganze Programm weiß
int axisPos[6]; // In diesem Array auch 6 Positionen werden
void setup() {
Serial.begin(115200);
// Initialisierung für Servomotoren
pwm.begin();
pwm.setPWMFreq(SERVO_FREQ);
pwm.setOscillatorFrequency(27000000);
// Initialisierung für NRF-Wireless-Controller
pinMode(3,OUTPUT);
radio.begin();
radio.openReadingPipe(1, address);
radio.setPALevel(RF24_PA_MIN);
radio.startListening();
// Initialisierung des LCD-Displays
lcd.begin(16, 2);
lcd.setBacklight(HIGH);
// Setzt Text passend zum aktuell ausgewählten Steuerungsmodus
changeLCD();
//Button für Steuerungswechsel
pinMode(8, INPUT);
}
void loop() {
// Verzweigung zum Wechsel des Steuerungsmodus
if (digitalRead(8) == HIGH) { // Schaut ob der Druckknopf gedrückt wurd
lcd.clear(); // Leert das LCD-Display, sodass es nichts mehr anzeigt.
if (controlmode == 1) { // Geht in die Verzweigung wenn der aktuelle Steuerungsmodus 1 (=Controller) ist
controlmode = 2; // Steuerungsmodus wird auf 2 (=Gestensteuerung) umgestellt
} else { // Falls der Steuerungsmodus nicht auf 1 (=Controller) ist geht in die Else-Verzweigung
controlmode = 1; // hier: stellt diesen zurück auf Controller bei Betätigung des Druckknopfes
}
delay(500); // Programm stoppt für 0.5 Sekunden, damit die Betätigung des Knopfes nicht doppelt oder dreifach wahrgenommen wird.
}
// Auswertung Controller
if (controlmode == 1) { // Schaut ob die Variable Controlmode auf 1 gesetzt ist, wenn ja wird die Steuerung vom Controller in dieser If-Verzweigung ausgewertet.
if (radio.available()) { // Schaut ob ein NRF-Partnermodul verfügbar ist, wenn ja wird diese If-Verzweigung ausgeführt
radio.read(controllerValues, sizeof(controllerValues)); // Liest die übertragenen Daten vom anderen NRF-Partnermodul und speichert sie in das Array controllerValues
}
changeLCD(); // Ruft die Methode changeLCD auf um das LCD-Display zu aktualisieren
findAxis(controllerValues[0]); // Ruft die Methode findAxis auf und übergibt den ausgelesenen Wert des Drehschalters, die Methode findet die Position des Drehschalters anhand des Wertes heraus und ändert dementsprechend den selektierten Servomotor (Variable: axis)
//Serial.println(axis);
if (axis == 0 || axis == 4 || axis == 5) { // Wenn Integer "axis" (gibt an welcher Servomotor angesteuert werden soll) 0, 4 oder 5 ist wird diese If-Verzweigung ausgeführt
if(controllerValues[2] > 700) { // Auswertung ob der Joystick horizontal einen Wert größer 700 ausliest
calcServoPos(true); // Wenn wird calcServoPos aufgerufen mit übergebenen Wert True, was bedeutet der Servo soll nach links gedreht werden
} else if(controllerValues[2] < 300) { // Auswertung ob der Joystick horizontal einen Wert kleiner 300 ausliest
calcServoPos(false); // Wenn wird calcServoPos aufgerufen mit übergebenen Wert False, was bedeutet der Servo soll nach rechts gedreht werden
}
} else if(axis == 1 || axis == 2 || axis == 3) { // Wenn Integer "axis" (gibt an welcher Servomotor angesteuert werden soll) 1, 2 oder 3 ist wird diese If-Verzweigung ausgeführt
if(controllerValues[2] > 700) { // Auswertung ob der Joystick senkrecht einen Wert größer 700 ausliest
calcServoPos(false); // wird calcServoPos aufgerufen mit übergebenen Wert False, was heißt der Servo dreht nicht links herum und entsprechende Motoren bewegen den Arm Richtung Boden
} else if(controllerValues[2] < 300) { // Auswertung ob der Joystick senkrecht einen Wert kleiner 300 ausliest
calcServoPos(true); // wird calcServoPos aufgerufen mit übergebenen Wert True, was heißt der Servo dreht links herum und entsprechende Motoren bewegen den Arm Richtung Decke
}
}
}
// Auswertung Gestensteuerung
if (controlmode == 2) { // Verzweigung wird ausgeführt wenn Variable controlmode 2 entspricht, d.h. die Gestensteuerung soll ausgewertet werden
changeLCD(); // Ruft Methode changeLCD auf um LCD-Display zu aktualisieren
if (Serial.available() >= 6) { // Schaut ob in der seriallen Schnittstelle ein Array größer gleich 6 verfügbar ist (also das vom Pythoncode übergebene Bytearray)
for(int i = 0; i < 6; i++) { // For-Schleife die 6 mal durchläuft um die 6 Werte aus dem erhaltenen Bytearray auszulesen
receivedActions[i] = Serial.read(); // Speichert in das Array receivedActions in den jeweiligen Index i das aktuell ausgelesene aus der seriallen Schnittstelle
}
// Alle Servomotoren und ihre übergebenen Zustandänderungen werden durchgegangen, j stellt dabei die Zählervariable der For-Schleife da, welcher Servo nun geändert werden soll entsprechend den vom Pythoncode übergebenen Werten
for(int j = 0; j < 6; j++) { // For-Schleife die 6 mal durchläuft
axis = j; // Der Variable axis (legt für das gesamte Programm den nun anzusteuernen Servomotor fest) wird mit j, was sich bei jedem Durchlauf der For-Schleife inkrementiert, initialisiert
if (receivedActions[j] == 2) { // Array receivedActions wird im Index j ausgelesen und es wird überprüft ob im jeweiligen Index die Zahl 2 steht, was bedeutet (aufgrund des Pythoncodes) das sich der jeweilige Servo nach links drehen soll
calcServoPos(true); // Falls die If-Bedingung zutrifft: die Methode calcServoPos wird mit übergebenen Wert True aufgerufen um die Bewegung nach links zu berechnen und auszuführen für den jeweiligen Servomotor
} else if(receivedActions[j] == 1) { // Array receivedActions wird im Index j ausgelesen und es wird überprüft ob im jeweiligen Index die Zahl 1 steht, was bedeutet (aufgrund des Pythoncodes) das sich der jeweilige Servo nach rechts drehen soll
calcServoPos(false); // Falls die If-Bedingung zutrifft: die Methode calcServoPos wird mit übergebenen Wert False aufgerufen um die Bewegung nach rechts zu berechnen und auszuführen für den jeweiligen Servomotor
}
}
}
}
}
// Methode zur Berechnung der neuen Servoposition, welche dann als Default gesetzt wird, da der Servo sonst wieder in die Ursprungsposition zurückfährt.
void calcServoPos(boolean left) {
Serial.println("Sending to Servo");
Serial.print(axis);
if (left == true) { // Wenn der an die Methode der Wert True für die Boolean "left" übergeben wurde, wird diese Methode ausgeführt, die den Servo nach links drehen lässt um 40 Schritte pro durchlauf
if (axisPos[axis] > 600) { // Schaut ob axisPos (Position des jeweiligen Servomotors, festgelegt in der Variable axis die global immer vor jedem Zugriff aktualisiert wird.) größer als 600 ist, damit die Variable innerhalb des Radius vom Servo bleibt.
axisPos[axis] -= 40; // Dekrementiert axisPos (des aktuell ausgewählten Servos, festgelegt in der Variable axis die global immer vor jedem Zugriff aktualisiert wird.) um 40
}
} else { // Wenn der an die Methode der Wert False für die Boolean "left" übergeben wurde, wird diese Else-Verzweigung ausgeführt welche die Bewegung des Motors rechts herum berechnet
if (axisPos[axis] < 2400) { // Schaut ob axisPos (Position des jeweiligen Servomotors, festgelegt in der Variable axis die global immer vor jedem Zugriff aktualisiert wird.) kleiner als 2400 ist, damit die Variable innerhalb des Radius vom Servo bleibt.
axisPos[axis] += 40; // Inkrementiert axisPos (des aktuell ausgewählten Servos, festgelegt in der Variable axis die global immer vor jedem Zugriff aktualisiert wird.) um 40
}
}
pwm.writeMicroseconds(axis, axisPos[axis]); // Sorgt dafür das der jeweilige Servo (axis) an die für in neu berechnete Position in axisPos (im Index axis) fährt.
receivedActions[axis] = 0; // Setzt die von der Gestensteuerung für den jeweiligen Servo erhaltenen Befehle zurück auf den Defaultwert 0, damit diese nicht kontinuierlich ausgeführt werden
}
void changeLCD() {
lcd.setCursor(0, 0); // LCD beginnt in der ersten Zeile an der ersten Stelle zu schreiben (0,0)
if (controlmode == 1) {
lcd.print("Controller"); // LCD schreibt "Controller"
lcd.setCursor(0, 1); // LCD beginnt nun in der zweiten Zeile an der ersten Stelle zu schreiben (0, 1)
lcd.println("Servomotor: "); // LCD schreibt "Servomotor: "
} else if(controlmode == 2) {
lcd.print("Gestensteuerung"); // LCD schreibt "Gestensteuerung"
} else { // Falls die Variable controlmode weder auf 1 noch 2 steht wird als Steuerungsmodus "Unbekannt" aufs LCD geschrieben
lcd.print("Unbekannt"); // LCD schreibt "Unbekannt"
}
}
// Methode um herauszufinden auf welcher Position der Drehschalter gerade steht und für jede Position einen Servomotor zuzuweisen.
void findAxis(int drehschalterVal) {
// Der Drehschalter hat 5 Widerstände angelötet fungierend als Spannungsteiler, das heißt je weiter man dreht desto geringer / höher wird die Spannung. Entsprechend unterscheiden sich die ausgelesenen Werte am analogen Eingang am Arduino Nano Every, welche diese weitersendet an diesen Arduino Uno zur Auswertung
if(drehschalterVal < 100) { // Die If-Abfragen prüfen anhand des analog ausgelesenen Wertes vom Arduino Nano Every, auf welcher Position der Drehschalter steht
axis = 5; // Legt für jede Position einen anderen Servo fest.
lcd.print(" 6"); // Schreibt die Zahl des jeweiligen Servomotors aufs LCD Display in die zweite Zeile
} else if(drehschalterVal < 300) {
axis = 4;
lcd.print(" 5");
} else if(drehschalterVal < 500) {
axis = 3;
lcd.print(" 4");
} else if(drehschalterVal < 700){
axis = 2;
lcd.print(" 3");
} else if(drehschalterVal < 900) {
axis = 1;
lcd.print(" 2");
} else {
axis = 0;
lcd.print(" 1");
}
}Last updated