Hello everyone,
I recently started working with Arduino. I am trying to program a speed control system. For this, I have a Nema23 with the TB6600 driver, the Arduino, and the target speed is set via an encoder. Everything has worked so far, and I was able to achieve speeds from -200 to 200 mm/s, but I noticed that at low speeds such as 5 mm/s, the speed is not actually 5 mm/s, but slower. In my case, the rotated object theoretically needs 25 seconds for one revolution, but in practice, one revolution takes about 5-6 seconds longer. For this reason, I purchased the AMT102 V2 to measure the actual speed and compensate for this control difference with the help of a PID controller. However, nothing happens when I change the constants of the P, I, and D components. According to the plot, my actual speed is always in the range of -2 and 2, even at a target speed of 5 mm/s. I'm slowly running out of ideas, as everything is actually connected correctly.
I'll add the code and maybe someone will find the error or can share their experience with me. I would be very grateful and look forward to a response.
#include <LiquidCrystal_I2C.h>
#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();
}
}