Hello, I have been working on a 2 wheel balancing robot on my summer time off from high school but am running into difficulties. I am using an arduino uno r3 with a cnc sheild on top. and an MPU 6050 connected to it. I am using a 6ah, 20v, drill motor to power the stepper motors and arduino through the vin pin (with a buck converter set to 9v). whenever i upload the following code, the wheels either spin for a second and then shut off completely, until I reset the robot, or they don't move at all. Here is the code:
#include <Wire.h>
#include <MPU6050.h>
#include <AccelStepper.h>
MPU6050 mpu;
// Define motor interface type
#define motorInterfaceType 1
// Define pins for X and Y steppers
#define stepXPin 2
#define dirXPin 5
#define stepYPin 3
#define dirYPin 6
// Define enable pin
#define enablePin 8
AccelStepper stepperX(motorInterfaceType, stepXPin, dirXPin);
AccelStepper stepperY(motorInterfaceType, stepYPin, dirYPin);
float setpoint = 0; // Setpoint for balancing
float kp = 1.0; // Proportional gain
float ki = 0.0; // Integral gain
float kd = 0.0; // Derivative gain
float lastError = 0;
float integral = 0;
void setup() {
Serial.begin(115200);
Wire.begin(); // Join I2C bus
// Initialize MPU6050
mpu.initialize();
if (!mpu.testConnection()) {
Serial.println("MPU6050 connection failed");
while (1); // Halt execution if MPU6050 is not connected
}
Serial.println("MPU6050 connection successful");
pinMode(enablePin, OUTPUT);
digitalWrite(enablePin, LOW); // Enable the motors
// Invert motor directions
stepperX.setPinsInverted(false, false, false); // Not inverted for X motor
stepperY.setPinsInverted(true, false, false); // Inverted for Y motor
stepperX.setMaxSpeed(1000); // Lowered speed for testing
stepperX.setAcceleration(1000); // Lowered acceleration for testing
stepperY.setMaxSpeed(1000); // Lowered speed for testing
stepperY.setAcceleration(1000); // Lowered acceleration for testing
}
float calculateAngle() {
int16_t ax, ay, az;
mpu.getAcceleration(&ax, &ay, &az);
// Calculate the tilt angle using ax and az
float angle = atan2(ax, az) * 180 / PI;
return angle;
}
float computePID(float error) {
float derivative = error - lastError;
integral += error;
// Anti-windup: Limit integral term
float integralLimit = 500; // Adjust as needed
if (integral > integralLimit) integral = integralLimit;
if (integral < -integralLimit) integral = -integralLimit;
float output = kp * error + ki * integral + kd * derivative;
lastError = error;
return output;
}
void loop() {
float angle = calculateAngle();
float error = setpoint - angle;
float output = computePID(error);
// Convert PID output to steps
int steps = map(output, -255, 255, -1000, 1000);
// Apply a deadband to ensure motors keep moving
if (abs(steps) < 10) {
steps = 0;
}
stepperX.moveTo(steps);
stepperY.moveTo(steps);
stepperX.run();
stepperY.run();
// Debug output
Serial.print("Angle: ");
Serial.print(angle);
Serial.print(" Error: ");
Serial.print(error);
Serial.print(" Output: ");
Serial.println(output);
delay(10); // Small delay to prevent overwhelming the serial output
}
If anyone can help me in any way i would really appreciate it, i'm trying to get this bot working by school time. Let me know if you have any further questions, Thank you!
I don't think your approach of calculating the tilt angle from the acceleration vectors is going to give you reliable results. It will only be correct when the robot is not accelerating, and the only acceleration is due to gravity.
But a self-balancing robot spends a lot of its time making small accelerations, and this will mess up the tilt calculation, and / or make it noisy.
I've had very good results from the Seeed Studio Grove IMU 9DOF library for the MPU6050, and the example named MPU6050_DMP6. This uses the digital motion processor onboard the MPU6050 to do some clever maths to produce a very nice, smooth and reliable tilt angle that reports correctly even when there are accelerations going on.
It seems like you have a pretty good grasp on how to do this. You also have an appropriate power supply. I'm thinking this is probably something simple in the code that you can probably fix pretty easily.
I'm definitely NOT an expert in programming, but, in your initialize mpu statement, is the statement while (1); correct? Should there atleast be an empty curly bracket after that or something? as @xfpd mentioned, what are you getting in the serial monitor?
Another guess - Is it doing what it is supposed to be doing? Running the motor briefly to correct the angle, then thinking it has the right angle and not moving again? Have you moved the sensor to an off-balance position to see if the wheels try and correct again?
Sorry if these seem like dumb questions. I really like that you are using a PID control in your code. I don't have much experience with the programming side of this, but I like your approach here and I'm interested in seeing your project work.
I also want to mention that half of this code is written by chatgpt, because I am a real newby to coding and decided to take the easy way out. So I really could not tell you if MPU statement is correct or not. How do you suggest me changing the code to make the robot keep trying to balance. I also want to point out that even if the motor is sideways and i turn it on, it still stops, so it never really stops when it hits its target position.
I've put what the serial monitor is saying in another reply, and i will try to make a diagram in a bit, i have to do a few things first, thank you for your time so far
I see the Angle, Error and Output are the same value. That could be caused by your calibration and maybe the physical connections. The numbers should never look the same.
I do not see anything wrong with your wiring. Did you go through the A4988 setup? Here is a link to a page of resources... find the video on how to configure the A4988.
Please take this as constructive, as in, I WANT you to learn... however; The problem your statement is, you have put your burden of learning/discovering/knowing on others, rather than learning your way to address your advanced project. ChatGPT "knows" nothing, rather it compiles vague information, not the specific information needed for power, electronics, data and programming. Understanding the Arduino, power, motors, I/O modules, and (for this module) advanced mathematics and natural forces is how to complete a project.
