Im trying to build an auto balancing robot using the GPY521 sensor, an arduino uno, two dc motors and the AutoPID library. The problem is that im only getting output for the motors in one direction when i tilt the robot and cannot get them to reverse in response to me tilting the other way. At this point I feel like im missing something and need help please.
This is my code:
#include <Wire.h>
#include <AutoPID.h>
const int MPU6050_addr = 0x68; // I2C address do sensor MPU-6050
int16_t accelerometer_x, accelerometer_y, accelerometer_z;
double setpoint =0; // Angulo de inclinação desejado (zero).
double input =0; // ANgulo de inclinação presente.
double output =0;
int16_t initial_accel_y;
int16_t accel_y;
// Motor control 1
const int m1Pin1 = A0;
const int m1Pin2 = A1;
const int m1_enable = 5;
// Motor control 2
const int m2Pin1 = A2;
const int m2Pin2 = A3;
const int m2_enable = 6;
// Instanciar AutoPID
AutoPID pid(&input, &setpoint, &output, 0, 255, 500, 1, 600); // a função incorpora os argumentos: moradas das variaveis input, setpoint e output, valores minimo e maximo de analogWrite e por ultimo valores de Kp, Ki e Kd.
void setup() {
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
delay(1000);
pinMode(m1Pin1, OUTPUT);
pinMode(m1Pin2, OUTPUT);
pinMode(m2Pin1, OUTPUT);
pinMode(m2Pin2, OUTPUT);
initial_accel_y = readAccelerometerY();
Serial.begin(9600);
}
void loop() {
// Ler dados do acelerometro e calcular inclinação:
accel_y = readAccelerometerY();
input = (double)(accel_y - initial_accel_y) / 16384.0; // Converter para angulo
// Calcular PID output
pid.run();
// Ajustar resposta dos motores com base no PID:
if (input > 0) {
digitalWrite(m1Pin1, LOW);
digitalWrite(m1Pin2, HIGH);
analogWrite(m1_enable, output);
digitalWrite(m2Pin1, LOW);
digitalWrite(m2Pin2, HIGH);
analogWrite(m2_enable, output);
}
if (input < 0){
digitalWrite(m1Pin1, HIGH);
digitalWrite(m1Pin2, LOW);
analogWrite(m1_enable, output);
digitalWrite(m2Pin1, HIGH);
digitalWrite(m2Pin2, HIGH);
analogWrite(m2_enable, output);
}
Serial.print("Input: ");
Serial.print(input);
Serial.print(" Output: ");
Serial.println(output);
delay(100); // Delay para estabilizar
}
int16_t readAccelerometerY() {
Wire.beginTransmission(MPU6050_addr);
Wire.write(0x3D);
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_addr, 2, true);
return Wire.read() << 8 | Wire.read();
}
Put Serial.print() statements in your program, and hold up the robot to see what input and output are for various tilt angles. Post examples of the printed output.
I agree we are both missing something, at this point I am assuming all parts are good. My view of your project is fuzzy at best and with the fog I cannot see how it is wired. Post an annotated schematic showing how you wired it, be sure to include all connections, power, ground and all power sources. Also links to technical information on the hardware parts will help. A clear photo of a hand drawn schematic would be OK. Be sure all connections are good.
Sorry that I didn’t post a schematic or picture of the robot. I’ll make sure to that later when I return home from work .
I’m using an Arduino Uno with a protoshield I put together using wires and an L293D motor driver, for the sensor an GPY521 and lastly two DC gear Motors.
You have set outputMin to be zero in the constructor, so it can't be negative, as the PID algorithm requires.
AutoPID pid(&input, &setpoint, &output, 0, 255, 500, 1, 600); // a função incorpora os argumentos: moradas das variaveis input, setpoint e output, valores minimo e maximo de analogWrite e por ultimo valores de Kp, Ki e Kd.
Okay I understand. But I don’t want it to be negative. I need it to send an analog signal to my motors whenever I tilt the robot to either side.
Right now it rotates the motors to one direction, thus, showing a reading on serial monitor , but when I tilt the other way it doesn’t give an output to my motors and the serial monitor displays a
The PID output MUST be negative, for positive input. That is how PID works.
The motors have to turn in both directions for your robot to balance. Change the sign of the output and direction of rotation when sending commands to the motors.
Ohh I’m so sorry for my mistake. So you’re saying that if I tilt toning side it should be negative and if I tilt to the other side it should be positive ?
Yes thanks you. I noticed that mistake too corrected it but it didn’t do anything for my main problem which is, not getting any output when my input is positive.
Okay ! So I finally got my motors to run on both directions with the MPU6050 tilt.
I included the MPU6050 library to simplify the code and calculations, and I changed the minimum value in the PID function to -255 instead of 0.
Here is the code :
#include <Wire.h>
#include <AutoPID.h>
#include <MPU6050.h>
const int MPU6050_addr = 0x68; // I2C address of the MPU-6050
int16_t initial_accel_y;
double setpoint = 0; // Desired tilt angle (zero)
double input = 0; // Current tilt angle
double output = 0;
int16_t accel_y;
// Motor control pins (adjust these to match your setup)
const int m1Pin1 = 1;
const int m1Pin2 = 2;
const int m2Pin1 = 3;
const int m2Pin2 = 4;
const int ENA = 5;
const int ENB = 6;
// Initialize AutoPID instance
AutoPID pid(&input, &setpoint, &output, -255, 255, 1000, 10, 15);
// Create an MPU6050 instance
MPU6050 mpu;
void setup() {
Serial.begin(9600);
Wire.begin();
mpu.initialize(); // Initialize the MPU6050
// Calibrate the MPU6050 and set the initial_accel_y
int16_t ax, ay, az;
mpu.getAcceleration(&ax, &ay, &az);
initial_accel_y = ay;
pinMode(m1Pin1, OUTPUT);
pinMode(m1Pin2, OUTPUT);
pinMode(m2Pin1, OUTPUT);
pinMode(m2Pin2, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
}
void loop() {
// Read accelerometer data and calculate input angle
accel_y = readAccelerometerY();
input = (double)(accel_y - initial_accel_y) / 16384.0; // Convert to angle
// Compute PID output
pid.run();
// Control motors based on PID output
if (output > 0) {
analogWrite(ENA, output);
digitalWrite(m1Pin1, HIGH);
digitalWrite(m1Pin2, LOW);
digitalWrite(m2Pin1, HIGH);
digitalWrite(m2Pin2, LOW);
} else {
analogWrite(ENB, -output);
digitalWrite(m1Pin1, LOW);
digitalWrite(m1Pin2, HIGH);
digitalWrite(m2Pin1, LOW);
digitalWrite(m2Pin2, HIGH);
}
Serial.print("Input: ");
Serial.print(input);
Serial.print("Output: ");
Serial.println(output);
delay(100); // Add a delay for stability
}
int16_t readAccelerometerY() {
int16_t ax, ay, az;
mpu.getAcceleration(&ax, &ay, &az);
return ay; // Return the y-axis accelerometer reading
}
.
I’m so sorry for my mistake. So you’re saying that if I tilt toning side it should be negative and if I tilt to the other side it should be positive ?
still have no output when input is positive. Only when its negative.
