I am fairly new to Arduino. I’m making an attempt to construct a Self-balancing robotic. I’m utilizing A4988 drivers, ESP32 microcontroller, NEMA 17 motors and a 6050 MPU.

The issue is that, though the pace variable adjustments in line with the place of the MPU. The motors rotate at a gradual and fixed pace, with out various in line with the inclination of the robotic.

Right here is the code:

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

#outline STEP_IZQ 26
#outline DIR_IZQ 27
#outline STEP_DER 12
#outline 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 pace,outputAbs;

void setup(void) {
  Serial.start(115200);
  whereas (!Serial)
    delay(10); // will pause Zero, Leonardo, and so on till serial console opens

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

  // Attempt to initialize!
  if (!mpu.start()) {
    Serial.println("Failed to search out MPU6050 chip");
    whereas (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Discovered!");
  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 occasions 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 levels
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  // 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 worth and add it to the pid_i_mem variable
  if(pid_i_mem > 800)pid_i_mem = 800;                                       //Restrict the I-controller to the utmost controller output
  else if(pid_i_mem < -800)pid_i_mem = -800;
  //Calculate the PID output worth
  pid_output = kp * pid_error_temp + pid_i_mem + kd * (pid_error_temp - pid_last_d_error);
  if(pid_output > 5000)pid_output = 5000;                                     //Restrict the PI-controller to the utmost 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);
  pace = map(outputAbs,0,5000,0,3000);
  
  stepperizq.setSpeed(pace);
  stepperder.setSpeed(pace);
  
  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();
}