Self Balancing Robot Using PID Loop

Using Arduino Programming Questions
1

Hi all, I've been developing a pid loop self balancing robot and im having trouble with the code (i think). Ive currently got an esp32 dev board hooked up to an mpu6050 for gyroscope angle reading, and 2 stepper motors which are connected through a4988 drivers.

The code runs well and does what i want it to do however the motor's max speed seem to be capped very low, nothing i change in the code increases the max speed. they are on 1/8 microstepping and can run much faster to the speed i need when i run them in a separate program outside a pid loop. I originally had an arduino nano in place of the esp32, but i thought it was possibly the processing power of the arduino nano that was slowing the loop down causing the motor to spin slower, but the issue hasnt gone away with the esp32.

Ive tried code with and without stepper motor libraries to get full control of the motors but its still not working as i want it. If anyone can recognise my issue I would greatly appreciate it.

here is my current code (with libraries)

#include <Wire.h>
#include <MPU6050_light.h>
#include <AccelStepper.h>
#include <PID_v1.h>

// Stepper motor pins
#define DIR_PIN 2
#define STEP_PIN 4
#define DIR_PIN2 12
#define STEP_PIN2 14

// MPU6050 object
MPU6050 mpu(Wire);
// Stepper objects
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
AccelStepper stepper2(AccelStepper::DRIVER, STEP_PIN2, DIR_PIN2);

// PID variables
double setpoint = 0; // Desired angle of 0 degrees
double input, output;

// PID gain control
double Kp = 25.0;
double Ki = 0.0;
double Kd = 4.0; 

// PID controller object
PID myPID(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(115200);
  
  // Initialize MPU6050
  Wire.begin();
  byte status = mpu.begin();
  if (status != 0) {
    Serial.println("MPU6050 connection failed");
    while (1); // Stop if connection fails
  }
  Serial.println("MPU6050 initialized successfully");
  
  // Calibrate MPU6050
  mpu.calcOffsets(true, true); // Calibrate gyro and accelerometer

  // Initialize steppers
  stepper.setMaxSpeed(7000);
  stepper.setAcceleration(5000);
  
  stepper2.setMaxSpeed(7000);
  stepper2.setAcceleration(5000);
  
  // Initialize PID controller
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(-1500, 1500); // Adjust output limits
}

void loop() {
  delay(0.4);
  // Update MPU6050 angle
  mpu.update();
  double angle = mpu.getAngleX(); // Get the Y-axis angle
  
  // Update PID controller
  input = angle;
  myPID.Compute();
  
  // Map PID output to motor speeds
  int motorSpeed = (int)output;
  
  stepper.setSpeed(motorSpeed); // Motor 1 speed
  stepper2.setSpeed(motorSpeed); // Motor 2 speed
  
  // Run the motors
  stepper.runSpeed();
  stepper2.runSpeed();
  
  // Print angle and motor speed
  Serial.print("Angle: ");
  Serial.print(angle);
  Serial.print(" Motor Speed: ");
  Serial.println(motorSpeed);
}

here is my current code (without libraries)

#include <Wire.h>
#include <MPU6050_light.h>

// Stepper motor pins
#define DIR_PIN 2
#define STEP_PIN 4
#define DIR_PIN2 12
#define STEP_PIN2 14

// MPU6050 object
MPU6050 mpu(Wire);

// PID control variables
double Kp = 62.0; // Keep Kp low to ensure stability
double setpoint = 0.0; // Target angle (upright position)
double angle, error, speed;

// Variables for step timing
volatile long stepDelay; // Delay between steps in microseconds
unsigned long lastStepTime1 = 0;
unsigned long lastStepTime2 = 0;

void setup() {
  Serial.begin(115200);

  // Initialize MPU6050
  Wire.begin();
  byte status = mpu.begin();
  if (status != 0) {
    Serial.println("MPU6050 connection failed");
    while (1); // Stop if connection fails
  }
  Serial.println("MPU6050 initialized successfully");

  // Calibrate MPU6050
  mpu.calcOffsets(true, true); // Calibrate gyro and accelerometer

  // Set motor pins
  pinMode(DIR_PIN, OUTPUT);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN2, OUTPUT);
  pinMode(STEP_PIN2, OUTPUT);
}

void loop() {
  mpu.update(); // Update MPU6050 angle
  angle = mpu.getAngleX(); // Get X-axis angle

  // Calculate error
  error = angle - setpoint;

  // Calculate speed based on proportional control
  speed = Kp * error;

  // Constrain speed to avoid overflow or hardware limits
  speed = constrain(speed, -8000, 8000); // Adjust for your stepper motor and driver limits

  // Determine motor directions
  digitalWrite(DIR_PIN, speed >= 0 ? HIGH : LOW);
  digitalWrite(DIR_PIN2, speed >= 0 ? HIGH : LOW);

  // Calculate step delay (higher speed = shorter delay)
  stepDelay = speed != 0 ? abs(1000000 / abs(speed)) : 1000000;

  // Generate step pulses for Motor 1
  if (micros() - lastStepTime1 >= stepDelay) {
    lastStepTime1 = micros();
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(1); // Minimal HIGH pulse
    digitalWrite(STEP_PIN, LOW);
  }

  // Generate step pulses for Motor 2
  if (micros() - lastStepTime2 >= stepDelay) {
    lastStepTime2 = micros();
    digitalWrite(STEP_PIN2, HIGH);
    delayMicroseconds(1); // Minimal HIGH pulse
    digitalWrite(STEP_PIN2, LOW);
  }

  // Print angle and speed for debugging
  Serial.print("Angle: ");
  Serial.print(angle);
  Serial.print(" Speed: ");
  Serial.println(speed);
}
2

You should describe, or measure:

  • very low
  • "nothing" (I change)
  • max speed
  • much faster
  • separate program

"stepDelay" is type "long", so I would put an "L" after the 1,000,000 so the value is not treated as an "int"... like this

3

first off

does nothing since delay() takes an int value and 0.4 truncates to 0.

You are re-calculating your speed every time through loop() so your acceleration has to start over. I think a better strategy would be to call .runSpeed() more often than you update your PID and speed settings.

1 Like
4

To determine why, put print statements into the code to report the values of motor control input, intermediate and output variables. Use that information to fix the errors that become apparent.

5

But:

Then:

So the speed is capped at 1500.

6

With a 2 wheel balancing robot, it's not the speed that counts. It's the acceleration. Acceleration / deceleration are what tip the robot one way or the other.

Normally you would set up your PID to compute a change in speed and add that to your current speed.

Here's some discussion around one that we built earlier this year. It's pretty long, but there are several different discussions about how to make the code work. If you can use git, the code is all on a github and you can roll back through the versions to see how the code came together. It's pretty complicated now, but in the earlier versions it is a lot more simple. We used a different accelerometer, and later on switched to a different PID library, but the basics of what you need to make a balancer are all there.

Really, go and look at the early versions where I was using the same PID library you're using.

7

Delta_G.

When you get a minute, please post the source code for the app, and please make sure the balance bot is the latest.
Thanks,
Mike

8

Cant thank you enough blh64, worked the charm