Hi,
I have made a balancing robot using MPU6050. The robot balances perfectly for hours. However, if I push it a little, it moves and accelerates and falls off. If I made it stand again, it will stand still for hours. If found the code online and made few changes to code and PID values to balance it. I have not set I and D as the robot is balancing at 0 values.
Any idea what might be the reason for the robot to not balance? Or am I missing something?
I am not aware of the concept. If someone can clarify why, I will try to code accordingly.
Accelerometer & Gyro : MPU6050
Motors : 25mm Gear Motors with encoders
Board : Arduino Mega
Motor Controller : L298
Though I need just the concept, or reasoning of why it might fall on push, I have added the below code which is the main control logic.
#include <EEPROM.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "EnableInterrupt.h"
#include "balance_robot.h"
#include "PID.h"
#include "Kalman.h"
#include "motor.h"
#include "EEPROMAnything.h"
#include "utils.h"
#include "LowPassFilter.h"
#include "Bluetooth.h"
MPU6050 mpu;
float G_Dt;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float rot_y, rot_z;
float filtered_rot_z;
int16_t yaw_du = 0;
PID pid;
PID pid_yaw;
Kalman kalman;
LowPassFilterFloat lpf_rot_z;
float m1_rpm = 0;
float m2_rpm = 0;
#define VALID 0xABCD
typedef struct config_t
{
int valid;
float Kp;
float Ki;
float Kd;
} pid_config;
pid_config balance_pid_values;
pid_config yaw_rate_pid_values;
float u; //pid control output
float offset_angle = 4.2f;
float target_angle = 0.0f + offset_angle;
float target_yaw_rate = 0.0f;
void setup()
{
Wire.begin();
balance_pid_values.Kp = 5.0f;
balance_pid_values.Ki = 0.0f;
balance_pid_values.Kd = 0.0f;
yaw_rate_pid_values.Kp = 0.0f;
yaw_rate_pid_values.Ki = 0.0f;
yaw_rate_pid_values.Kd = 0.0f;
Serial.begin(38400);
Serial2.begin(9600); //bluetooth
mpu.initialize();
pid = PID(balance_pid_values.Kp, balance_pid_values.Ki, balance_pid_values.Kd);
pid_yaw = PID(yaw_rate_pid_values.Kp, yaw_rate_pid_values.Ki, yaw_rate_pid_values.Kd);
initMotors();
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
float angleByAcc = atan2(ax, az);
kalman.setAngle(angleByAcc);
//4 hz cutoff freq for angular velocity in z
lpf_rot_z.set_cutoff_frequency(2);
//calibrate();
kalmanTimer = millis();
previous100HzTimer = kalmanTimer;
previous200HzTimer = kalmanTimer;
imuTimer = millis();
encoderTimer = imuTimer;
reportTimer = imuTimer;
}
void balance(float angle_sensed)
{
float w_sensed_unbias = kalman.getRate();
u = pid.cascade_control(target_angle, angle_sensed, w_sensed_unbias, G_Dt);
}
void yaw_control()
{
if (target_yaw_rate == 0.0f)
yaw_du = 0;
else
yaw_du = pid_yaw.control(target_yaw_rate, filtered_rot_z, G_Dt);
}
void stopAndReset()
{
u = 0;
yaw_du = 0;
motors_stop();
pid.reset();
}
void loop()
{
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
rot_y = -(float)gy / GYRO_SENS; //to degree
//roation rate at yaw
rot_z = -(float)gz / GYRO_SENS; //to degree
float angleByAcc = atan2(ax, az);
currentTime = millis();
G_Dt = (currentTime - kalmanTimer) / 1000.0f;
//kalman filter is running at MCU's full speed
float pitch = kalman.getAngle(angleByAcc * Rad2Deg, rot_y, G_Dt);
kalmanTimer = currentTime;
if (currentTime - previous200HzTimer > TASK_200HZ)
{
G_Dt = (currentTime - previous200HzTimer) / 1000.0f;
if (abs(pitch) < 35) { //if error less than 35 degree, try to balance
balance(pitch);
motors_control_sep(u - yaw_du, u + yaw_du);
}
else {
stopAndReset();
}
previous200HzTimer = currentTime;
}
if (currentTime - previous100HzTimer > TASK_100HZ)
{
G_Dt = (currentTime - previous100HzTimer) / 1000.0f;
filtered_rot_z = lpf_rot_z.apply(rot_z, G_Dt);
if (abs(pitch) < 35) {
yaw_control();
}
previous100HzTimer = currentTime;
}
if (currentTime - reportTimer >= TASK_20HZ)
{
processBluetooth();
target_yaw_rate = bt_yaw_rate;
target_angle = bt_pitch + offset_angle;
reportTimer = currentTime;
}
}
Best Regards,
Praveend