AccelStepper.h - How can I quickly increase the speed of the NEMA 17 motor?

Question

I'm pretty new to Arduino. I am trying to build a Self-balancing robot. I am using A4988 drivers, ESP32 microcontroller, NEMA 17 motors and a 6050 MPU.

The problem is that, although the speed variable changes according to the position of the MPU. The motors rotate at a slow and constant speed, without varying according to the inclination of the robot.

Here is the code:

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <AccelStepper.h>

#define STEP_IZQ 26
#define DIR_IZQ 27
#define STEP_DER 12
#define DIR_DER 13

AccelStepper stepperizq(AccelStepper::FULL2WIRE,STEP_IZQ,DIR_IZQ);
AccelStepper stepperder(AccelStepper::FULL2WIRE,STEP_DER,DIR_DER);
Adafruit_MPU6050 mpu;

float kp = 20; 
float ki = 0.5;
float kd = 30;

float angulo_grad, error,cumError,rateError,output,lastError;
float pid_error_temp, pid_i_mem, pid_setpoint, gyro_input, pid_output, pid_last_d_error;
int speed,outputAbs;

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
 
  pinMode(DIR_IZQ, OUTPUT);
  pinMode(DIR_DER, OUTPUT);

  stepperizq.setMaxSpeed(3000);
  stepperder.setMaxSpeed(3000);
  stepperizq.setAcceleration(1000); //1000
  stepperder.setAcceleration(1000); //1000
}

void loop() {
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  angulo_grad = atan((a.acceleration.x)/(a.acceleration.z))*(180/3.14);            //0 to 90 degrees
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  // pid 
  /////////////////////////////////////////////////////////////////////////////////////////////////

  pid_error_temp = angulo_grad;
  if((pid_error_temp > -2.0 )&&( pid_error_temp < 2.0)) pid_error_temp = 0.0;

  pid_i_mem += ki * pid_error_temp;                                 //Calculate the I-controller value and add it to the pid_i_mem variable
  if(pid_i_mem > 800)pid_i_mem = 800;                                       //Limit the I-controller to the maximum controller output
  else if(pid_i_mem < -800)pid_i_mem = -800;
  //Calculate the PID output value
  pid_output = kp * pid_error_temp + pid_i_mem + kd * (pid_error_temp - pid_last_d_error);
  if(pid_output > 5000)pid_output = 5000;                                     //Limit the PI-controller to the maximum controller output
  else if(pid_output < -5000)pid_output = -5000;

  if(output < 50 && output > 50)output = 0;
  pid_last_d_error = pid_error_temp;  
  ////////////////////////////////////////////////////////////////////////////////////////////////////
  //motors nema 17
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////
  outputAbs = (int)abs(pid_output);
  speed = map(outputAbs,0,5000,0,3000);
  
  stepperizq.setSpeed(speed);
  stepperder.setSpeed(speed);
  
  if(pid_output<0){
    digitalWrite(DIR_IZQ,LOW);
    digitalWrite(DIR_DER,HIGH);
  }
  else if(pid_output>0){
    digitalWrite(DIR_IZQ,HIGH);
    digitalWrite(DIR_DER,LOW);
  }
  stepperizq.runSpeed();
  stepperder.runSpeed();
}

Answer 1

To write down my guess from the comments in a proper answer:

When a stepper motor driven with AccelStepper shows a constant speed, though the speed is set to different values, it suggests, that the time interval of the steps is defined by something else.

The runSpeed() function works by saving a timestamp of the last step and then checking the elapsed time on the basis of the set speed, to check if the time for another step has come and if yes, then execute exactly one step. For this to work correctly, you need to call this function within intervals smaller than the smallest period between two consecutive steps for your speed.

If for whatever reason you call the function with larger intervals, the runSpeed() function will always see, that the time for a new step has come, but it will always only do one step per execution. Thus you overall get one step per runSpeed() execution, and this is determined by the rest of your code, not the set speed.

The rest of the code in loop() is for getting the measurements of the MPU and calculating the resulting speeds from that. Probably this is taking too long and thus hindering the AccelStepper library from running the motor at the specified speeds. So you need to provide a larger timeslot for the stepper driving on your ESP by cutting down on your MPU sampling rate. This is easiest done by using

// Running MPU sampling and calculation only every 100ms
if(millis() - previousMillis >= 100){
  previousMillis += 100;
  // MPU and calculation code here
}
// Running the stepper code on every loop iteration
stepperizq.runSpeed();
stepperder.runSpeed();

which is the non-blocking coding style used in the BlinkWithoutDelay example. If you don't know how to use millis(), there are many tutorials online for that.