I'm pretty new to Arduino. I am trying to build a self-balancing robot. I am using A4988 drivers, an 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) {
  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) {
  Serial.println("MPU6050 Found!");
  pinMode(DIR_IZQ, OUTPUT);
  pinMode(DIR_DER, OUTPUT);

  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);
  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);
  • 1
    Just a guess, but can it be, that your loop() is running to slow for it to keep up with the necessary steps? If for example the sensor readings are taking too long (in the range of the period between two consecutive steps), then each execution of runSpeed() would find, that it always was time to do a step, doing exactly one step and then exiting to the rest of loop(). That would limit your speed to one step per loop() iteration, which probably has a constant duration. You could try to only get new measurements at set intervals (like every x ms)
    – chrisl
    Aug 28, 2023 at 12:28
  • Agree with @chrisl. Try wrapping the measurement & calculations in an if (millis() - last > interval){...} throttle.
    – Dave X
    Aug 28, 2023 at 17:58
  • I added this: <code> currentMillis = millis(); if(currentMillis - previousMillis >=100){ previousMillis = currentMillis; 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 } </code> And it worked. Thank you all for ypur help
    – camu
    Aug 28, 2023 at 18:00

1 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

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.

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.