Hallo Zusammen,
ich habe erst vor kurzem mit dem Arduino angefangen zu arbeiten. Ich versuche eine Drehzahlregelung zu programmieren. Dafür habe ich einen Nema23 mit dem Treiber TB6600, den Arduino und die Soll-Drehzahl wird über einen Encoder eingestellt. Das hat alles auch soweit funktioniert und ich konnte Drehzahlen von -200 bis 200 mm/s erreichen, jedoch habe ich festgestellt, dass bei geringen Geschwindigkeiten wie bei 5 mm/s nicht tatsächlich 5 mm/s abgefahren werden, sondern die Geschwindigkeit langsamer ist. In meinem Fall braucht der gedrehte Gegenstand für eine Umdrehung 25 Sekunden in der Theorie in der Praxis braucht die eine Umdrehung ca. 5-6 Sekunden mehr. Aus diesem Grund habe ich den AMT102 V2 besorgt um die tatsächliche Geschwindigkeit zu messen und mit Hilfe eines PID Reglers diese Regeldifferenz auszugleichen. Jedoch passiert nichts bei Veränderung der Kontanten des P,I und D Anteils. Meine tatsächliche Geschwindigkeit befindet sich nach Plot immer im Bereich -2 und 2 und das bei einer Sollgeschwindigkeit von 5 mm/s. Ich weiß so langsam nicht mehr weiter, da eigentlich alles richtig angeschlossen ist.
Ich füge den Code mal hinzu und eventuell findet einer den Fehler oder kann mir seine Erfahrungen teilen. Ich wäre total dankbar und freue mich auch eine Antwort
#include <AccelStepper.h>
#include <PID_v1.h>
#include <Wire.h>
/* ──────────────────────────────────────────────
MOTORPARAMETER
────────────────────────────────────────────── */
const int motorFullSteps = 200;
const int microsteps = 16;
const float rohrDurchmesser = 41;
const int stepsPerRev = motorFullSteps * microsteps;
const float mmPerRev = 3.1416 * rohrDurchmesser;
const float mmPerStep = mmPerRev / stepsPerRev;
/* ── Encoder Parameter ─────────────────────── */
const int encoderPPR = 1000; // Pulses per Revolution (ANPASSEN!)
const float mmPerEncStep = mmPerRev / encoderPPR;
/* ── Pins ───────────────────────────── */
const int encA = 18;
const int encB = 19;
const int rotaryCLK = 2;
const int rotaryDT = 4;
const int rotarySW = 5;
const int dirPin = 6;
const int stepPin = 7;
/* ── Variablen ──────────────────────── */
volatile long motorEncoderCount = 0;
long lastEncoderCount = 0;
float measuredSpeed = 0;
int lastRotaryCLK;
volatile int rotaryValue = 0;
float targetSpeed = 0;
float mmPerSecPerClick = 1.0;
/* ── PID Variablen ──────────────────── */
double input, output, setpoint;
double Kp = 10, Ki = 0.5, Kd = 0.0;
PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
/* ── Zusatz für Glättung / Deadband ───────── */
float filtSpeed = 0;
const float alpha = 0.3;
const float deadband_mmps = 0.5;
const float min_cmd_steps = 400; // erstmal 0 zum Testen!
/* ── Objekte ────────────────────────── */
LiquidCrystal_I2C lcd(0x27,20,4);
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);
/* ── Interrupt Routine ───── */
void readMotorEncoder() {
bool a = digitalRead(encA);
bool b = digitalRead(encB);
if (a == b) motorEncoderCount++;
else motorEncoderCount--;
}
/* ── SETUP ─────────────────────────── */
void setup() {
Serial.begin(115200);
lcd.init();
lcd.backlight();
lcd.setCursor(0,0);
lcd.print("PID Drehzahlregelung");
pinMode(rotaryCLK, INPUT_PULLUP);
pinMode(rotaryDT, INPUT_PULLUP);
pinMode(rotarySW, INPUT_PULLUP);
lastRotaryCLK = digitalRead(rotaryCLK);
pinMode(encA, INPUT_PULLUP);
pinMode(encB, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(encA), readMotorEncoder, CHANGE);
attachInterrupt(digitalPinToInterrupt(encB), readMotorEncoder, CHANGE);
stepper.setMaxSpeed(8000);
stepper.setAcceleration(1500);
setpoint = 0;
myPID.SetMode(AUTOMATIC);
myPID.SetSampleTime(20);
myPID.SetOutputLimits(-8000, 8000); // NEGATIVE erlaubt!
}
/* ── LOOP ─────────────────────────── */
void loop() {
int clkState = digitalRead(rotaryCLK);
if (clkState != lastRotaryCLK && clkState == HIGH) {
if (digitalRead(rotaryDT) == LOW) rotaryValue--;
else rotaryValue++;
rotaryValue = constrain(rotaryValue, -200, 200); // NEGATIVE Werte erlaubt
targetSpeed = rotaryValue * mmPerSecPerClick;
}
lastRotaryCLK = clkState;
static unsigned long lastTime = millis();
unsigned long now = millis();
float dt = (now - lastTime) / 1000.0;
if (dt >= 0.02) {
long delta = motorEncoderCount - lastEncoderCount;
lastEncoderCount = motorEncoderCount;
float rawSpeed = (delta * mmPerEncStep) / dt;
filtSpeed += alpha * (rawSpeed - filtSpeed);
measuredSpeed = filtSpeed;
lastTime = now;
input = measuredSpeed;
if (fabs(targetSpeed) < deadband_mmps) {
setpoint = 0;
} else {
setpoint = targetSpeed;
}
myPID.Compute();
double cmd = output;
if (setpoint == 0 && fabs(input) < deadband_mmps) {
cmd = 0;
} else if (cmd > 0 && cmd < min_cmd_steps) {
cmd = min_cmd_steps;
} else if (cmd < 0 && cmd > -min_cmd_steps) {
cmd = -min_cmd_steps;
}
stepper.setSpeed(cmd);
stepper.runSpeed();
// 🔎 Debug-Ausgabe
double error = setpoint - input;
Serial.print(setpoint); Serial.print("\t");
Serial.print(input); Serial.print("\t");
Serial.print(output); Serial.print("\t");
Serial.print(cmd); Serial.print("\t");
Serial.print(delta); Serial.print("\t");
Serial.println(rawSpeed);
}
static unsigned long lastLCD = 0;
if (millis() - lastLCD > 300) {
lcd.setCursor(0,1);
lcd.print("Soll: "); lcd.print(setpoint,1); lcd.print(" mm/s ");
lcd.setCursor(0,2);
lcd.print("Ist : "); lcd.print(measuredSpeed,1); lcd.print(" mm/s ");
lcd.setCursor(0,3);
lcd.print("Out : "); lcd.print(output,0); lcd.print(" steps/s ");
lastLCD = millis();
}
}