Управление NEMA 17 с ПИД на TB6600, двигатель слишком медленный

Я пишу здесь, поскольку отчаялся найти решение после многочисленных попыток, как с онлайн-решениями, так и с ChatGPT, так что, возможно, кто-то здесь сможет помочь.

Мой проект довольно прост: я пытаюсь сделать перевёрнутый маятник с помощью Arduino, NEMA 17 и контроллера TB6600. Угол наклона маятника задаётся энкодером, который работает очень хорошо. Моя механическая система тоже работает отлично, никаких проблем.

Моя проблема в том, что я не могу правильно управлять двигателем, чтобы он реагировал на быстрые команды. Я хочу, чтобы двигатель вращался в соответствии с базовым выходным сигналом ПИД-регулятора, и пока ПИД-регулятор не настроен, я не могу добиться достаточно быстрого вращения двигателя. Как бы я ни настраивал ПИД-регулятор, двигатель не будет достаточно быстрым, чтобы удерживать маятник в вертикальном положении. В противном случае двигатель вращается правильно: вправо, когда маятник падает справа, и влево, когда он падает слева, поэтому у меня нет проблем с управлением направлением двигателя. Кроме того, я знаю, что мои кабели должны быть в порядке, так как перед использованием ПИД-регулятора у меня есть фаза инициализации, где я помещаю маятник в середину направляющих, что требует перемещения двигателя. И я могу сделать это очень быстро с помощью библиотеки AccelStepper, поэтому я знаю, что моего оборудования достаточно для моих нужд.

TB6600 настроен на полный шаг и допускает до 3 А, чего, как я знаю, более чем достаточно, как я уже говорил ранее.

Вот код, который я использую:

    #include <AccelStepper.h>
    #include <Encoder.h>
    
    //################# CONNECTION HARDWARE ################
    // Connection encoder
    #define enc_green_pin 2
    #define enc_white_pin 3
    
    // Connection motor
    #define motor_dirPin 8 // Connected to DIR+(+5V)
    #define motor_stepPin 9 // Connected to PUL+(+5V)
    #define motorInterfaceType 1 // stepper driver
    
    // Create a new instance of the AccelStepper class:
    AccelStepper stepper = AccelStepper(motorInterfaceType, motor_stepPin, motor_dirPin);
    
    // Encoder
    Encoder encod(enc_green_pin,enc_white_pin);
    
    
    void setup() {
      Serial.begin (115200);
    
      // motor settings
      stepper.setMaxSpeed(5000);
      stepper.setAcceleration(50000);
    
      delay(2000); // seconds to wait before starting
    
      initialisation();
    }
    
    // Initialisation, moves stepper and init encoder
    void initialisation(){
      Serial.print("Position before : ");
      Serial.println(stepper.currentPosition());
    
      // Moves stepper to the middle
      stepper.moveTo(-1000);
      stepper.runToPosition();
      Serial.print("Position : ");
      Serial.println(stepper.currentPosition());
      stepper.setCurrentPosition(0);
    
      delay(2000); // seconds to wait before starting
    }
    
    void loop() {
      
      // Read encoder and convert angle
      long pos_brut = encod.read();
      float angle_deg = int(conv_angle(pos_brut));
    
      int error = -(180-angle_deg); // error : setpoint=180
    
      /*
      float error =-5*(180 - angle_deg_int);
      errSum += (error * 0.01);
      float dErr = (error - lastErr) / 0.01;
    
      output = Kp * error + Ki * errSum + Kd * dErr;
      lastErr = error;
    
      // Limits output
      if (output > 255) output = 255;
      if (output < -255) output = -255;  
    
      */
    
      // Method 1
      if (error  > 0) {
        digitalWrite(motor_dirPin, HIGH);
        analogWrite (motor_stepPin,error);
      }
      else {
        digitalWrite(motor_dirPin, LOW);
        analogWrite (motor_stepPin,-error);
      }
    
      /* Method 2
      if (output  > 0) {
        digitalWrite(motor_dirPin, HIGH);
        stepMotor(output );
      } else {
        digitalWrite(motor_dirPin, LOW);
        stepMotor(-output );
      }
      */
    }
    
    void stepMotor(int steps) {
      for (int i = 0; i < steps; i++) {
        digitalWrite(motor_stepPin, HIGH);
        delayMicroseconds(500); 
        digitalWrite(motor_stepPin, LOW);
        delayMicroseconds(500); 
      }
    }
    
    // Convert angle from encoder
    float conv_angle(long compteur) {
      float val_deg;
      val_deg = float(compteur) * 180./1200.;
      return (val_deg);
    }

Как видите, я пробовал разные подходы:

Для контрольного вывода я попробовал:

• Вычисляю ошибку самостоятельно (используется в коде)

• Использование самодельного PID, найденного в Интернете (прокомментировано в коде)

• Использование библиотеки PID_V2 Arduino (не видно в коде)

Для управления двигателем я попробовал:

• Использование библиотеки AccelStepper с moveTo() и run() (не видно в коде)

• Использование digitalWrite/analogWrite непосредственно в цикле (используется в коде)

• Использование цикла for для digitalWrite (закомментировано в цикле)

Я попробовал поиграть со всеми указанными выше значениями и добавить задержки тут и там.

Боюсь, я не до конца понимаю, как должен управляться степпер, но все эти испытания основаны на том, что я видел в интернете.