NEMA 17 too slow, input from PID on TB6600

Hardware
1

Hello all,

I'm writing there as I am kinda desperate to find a solution after many trials, back and forth with online solutions and ChatGPT, so maybe someone here can help.

My project is quite simple: I'm trying to make an inverted pendulum with an Arduino, a NEMA 17 and a TB6600 controller. The angle of the pendulum is given by an encoder, which works very well. My mechanical system works quite well too, no issue there.

My problem is that I am unable to correctly control the motor to answer to fast commands. I want the motor to move according to a basic PID output, and while my PID is not set yet, I am unable to make the motor move fast enough at all. No matter how I tune my PID, the motor wont be fast enough to keep the pendulum upright. Otherwise, the motor moves well, right when the pendulum is falling on the right, and left when it's falling on the left, so I have no issue controlling the direction of the motor. Also, I know that my cabling should be good, as prior to using the PID, I have an initialisation phase where I place the pendulum in the middle of the rails, which requires me to move the motor. And I am able to do it very fast using the AccelStepper library, so I know that my hardware is sufficient for my need.

The TB6600 is set on full steps and allows up to 3A, which I know is more than enough as said earlier.

Here is the code that I am using :

#include <AccelStepper.h>
#include <Encoder.h>
#include <PID_v2.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

// PID
double SetpointPID, InputPID, OutputPID;
double   Kp=2, Ki=0, Kd=0;
PID myPID(&InputPID, &OutputPID, &SetpointPID, Kp, Ki, Kd, DIRECT);

// 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

 SetpointPID = 0;
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-255,255);
}

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 );
  }
  */
 
 /* Method 3
 InputPID=error;

  myPID.Compute();
  Serial.println(OutputPID);
  if (error > 0) {
      digitalWrite(motor_dirPin, HIGH);  // Sens du mouvement
    } 
    else if (error < 0) {
      digitalWrite(motor_dirPin, LOW);  // Sens inverse  
    }
  analogWrite(motor_stepPin, OutputPID);
 */


}

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);
}

As you can see, I tried different approaches :

For the control output, I tried:

  • Computing an error by myself (used in the code)
  • Using a homemade PID found online (commented in the code, method 2)
  • Using the PID_V2 Arduino library (commented in the code, method 3)

For the control of the motor, I tried:

  • Using the AccelStepper library with moveTo() and run() (not visible in the code)
  • Using a digitalWrite/analogWrite directly in the loop (used in the code)
  • Using a for loop for digitalWrite (commented in the loop)
  • Using an analogWrite in a for loop

I tried playing with all the values seen above, and to add delays here and there. I really did a lot of possible combination of values, nothing really works.

Thank you very much for your help in advance, and let me know if you have any question. Thanks !