Ich bin gerade an einem Arduino Projekt beschäftigt in welchem im versuche einen 2-rädrigen Roboter balancieren zu lassen. Ich habe das ganze auf einem Breadboard getestet und Software und Hardware haben funktioniert. Nun habe ich das Ganze verlötet und es auf eine massive Holzkonstruktion gesetzt um es ein bisschen schöner zu gestalten. Leider funktioniert jetzt gar nichts mehr. Also der Arduino gibt im Serial Monitor nichts aus, obwohl ich den selben Code verwende.
Hier der Code:
#include <AccelStepper.h>
#include <MultiStepper.h>
#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
const int enaPin1 = 4;
const int stepPin1 = 5; // Definiert den Pin 5 am Arduino welcher mit dem Steppin des 1 Motortreiber verbunden ist.
const int stepPin2 = 6; // Definiert den Pin 6 am Arduino welcher mit dem Steppin des 2 Motortreiber verbunden ist.
const int dirPin1 = 7; // Definiert den Pin 7 am Arduino welcher mit dem Dirpin des 1 Motortreiber verbunden ist.
const int dirPin2 = 8; // Definiert den Pin 8 am Arduino welcher mit dem Dirpin des 2 Motortreiber verbunden ist.
AccelStepper stepper1(AccelStepper::DRIVER,stepPin1, dirPin1); // Erstellt ein AccelStepper Objekt und konfiguriert es als Driver(Treiber) unter Verwendung der definierten Pins.
AccelStepper stepper2(AccelStepper::DRIVER,stepPin2, dirPin2); // Erstellt ein AccelStepper Objekt und konfiguriert es als Driver(Treiber) unter Verwendung der definierten Pins.
float setpoint = 103.7; // Definiert die Variabel setpoint(Sollwert) als 0 (Abweichung von der Gleichgewichtslage soll 0 Grad sein)
float Kp = 15.0; // Definiert die PID Reglerkonstante Kp als 50
float Ki = 0.5; // Definiert die PID Reglerkonstante Ki als 0.5
float Kd = 2.0; // Definiert die PID Reglerkonstante Kd als 1.0
float input, output, error; // Deklatiert Variabel für den PID Regler (Aktueller Winkel, PID-Ausgabewert, Aktueller Fehlerwert).
float lastError = 0; // Deklariert Variabel für Fehlerwert der vorherigen Iteration.
float integral = 0; // Deklariert Variabel für den Integralanteil des Fehlers.
unsigned long lastTime; // Deklariert eine Variabel für die Zeitmessung in millis(Millisekunden).
void setup() {
Wire.begin(); // Initialisiert die Wire Library und die I2C-Kommunikation
Serial.begin(115200); // Setzt die Baudrate (bits per second) für die serielle Datenübertragung fest.
mpu.initialize(); // Initialisiert den MPU6050
if (!mpu.testConnection()) { // Überprüft die Verbindung (bei "false" wird der Loop ausgeführt, bei "true" nicht) ("!" wird als Umkehrwert verwendet)
Serial.println("MPU6050 connection failed"); // Falls Verbindung fehlgeschlagen wird "MPU6050 connection failed" ausgegeben
while (1); // Falls Verbindung fehlgeschlagen bleibt das Programm in einer Endlosschlaufe stecken
}
pinMode(enaPin1, OUTPUT);
digitalWrite(enaPin1, LOW);
stepper1.setMaxSpeed(2000); // Setzt die maximale Geschwindigkeit auf 2000 (Schritte pro Sekunde)
stepper1.setAcceleration(1000); // Setzt die maximale Beschleunigung auf 1000 (Schritte pro Sekunde^2)
stepper2.setMaxSpeed(2000); // Setzt die maximale Geschwindigkeit auf 2000 (Schritte pro Sekunde)
stepper2.setAcceleration(1000); // Setzt die maximale Beschleunigung auf 1000 (Schritte pro Sekunde^2)
lastTime = millis(); // Speichert die aktuelle Zeit in Millisekunden
}
void loop() {
unsigned long currentTime = millis(); // Definiert eine Variabel in Millisekunden
float deltaTime = (currentTime - lastTime) /1000.0; // Berechnet die Zeitdifferenz zwischen zwei Loopdurchläufen -> deltaTime
lastTime = currentTime; // Setzt die Variabel lastTime auf den selben Wert wie die Variabel currentTime
int16_t ax, ay, az;
int16_t gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
float angleY = atan2(ax, az) * 180 / PI;
float gyroRateY = gy / 131.0;
const float alpha = 0.98;
static float angle = 0;
angle = (alpha * (angle + gyroRateY * deltaTime) + (1 - alpha) * angleY);
input = angle;
error = setpoint - input; // Unterschied zwischen Sollwert und aktuellem Wert
integral += error * deltaTime; // Die Summe aller vergangenen Fehler, gewichtet mit der Zeit, um den kumulierten Fehler zu berücksichtigen.
float derivative = (error - lastError) / deltaTime; // Die Rate der Änderung des Fehlers, um die Reaktion auf schnelle Änderungen zu berücksichtigen.
output = (Kp * error + Ki * integral + Kd * derivative); // Das kombinierte Ergebnis des PID-Reglers, das als Steuersignal verwendet wird.
lastError = error; // Setzt den Lasterror auf den selben Wert wie Error für den nächsten Umlauf
float motorSpeed = constrain(output, -800, 800); // Begrenzt Output auf den Bereich -1000 bis 1000
stepper1.setSpeed(motorSpeed); // Setzt die Geschwindigkeit des 1 Motors auf motorSpeed
stepper2.setSpeed(motorSpeed); // Setzt die Geschwindigkeit des 2 Motors auf motorSpeed
stepper1.runSpeed(); // Aktualisiert die Geschwindigkeit des 1 Motor basierend auf dem oberen Ergebnis
stepper2.runSpeed(); // Aktualisiert die Geschwindigkeit des 2 Motor basierend auf dem oberen Ergebnis
Serial.print("Angle: "); // Für Debugging
Serial.print(input); // Für Debugging
Serial.print(" Output: "); // Für Debugging
Serial.println(output); // Für Debugging
Serial.print(" motorSpeed: ");
Serial.print(motorSpeed);
delay(20);
}
Ich verwende einen Arduino Nano, Mpu6050, 2x Nema 17 Stepper, 2x a4988 Treiber. Und habe wie folgt verdrahtet:
Das Bluetooth Modul muss nicht beachtet werden, da ich es erst wenn der Roboter läuft einbauen möchte.
Vielen Dank für eure Hilfe und Tipps oder Verbesserungen für den Code sind natürlich erwünscht, auch wenn es nicht mit meinem jetzigen Problem zusammenhängt.
LG 
.