Hi!
Me and a friend are building a balancing robot during our summer holiday from engineering studies in Sweden. However there is trouble since the robot does not balance very well. We are using conventional hardware that is listed below, leading us to believing our coding and control theory is insufficient. We use the “available” PID-library for the controller. Here is a video of it using a PID-controller implemented according to the attached code with parameters given (actually a PD-controller with kp = 40 and kd = 0.1).
Link to video:
www.youtube.com/watch?v=gOwwe1bC1_M
As you can see, the robot is not completely horrible at balancing, but after a moment, it just spirals out of control. In a first attempt, we are hoping to use only one PID-controller that is trying to keep the angle given by the IMU to 0 degrees. We have tried numerous different parameter settings of the PID, but not been able to get a better robot than what you can see in the video. In a later stage, we hope to add a second PID-controller (an outer loop) which controls the speed of the motors.
The hardware used is:
1 pc Arduino Uno
1 pc MPU6050 (IMU)
1 pc 3 cell Lipo battery (11,1 V)
1 pc L298N motor shield
2 pcs Pololu 12V 30:1 Gear Motor W/ 64 CPR Encoder (see Pololu - 30:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Spur Pinion))
2 pcs Pololu Wheel 90x10mm Pair - Black
The rest is mainly brackets and screws.
// MPU6050 - Version: Latest
#include <MPU6050.h>
// I2Cdev - Version: Latest
#include <I2Cdev.h>
// Encoder - Version: Latest
// This optional setting causes Encoder to use more optimized code,
// It must be defined before Encoder.h is included.
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>
// PID - Version: Latest
#include <PID_v1.h>
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// motor pins and pwm-offsets
int PWM_L = 5;
int FWD_L = 6;
int BWD_L = 7;
int BWD_R = 8;
int FWD_R = 9;
int PWM_R = 10;
double mL = 46; //pwm-offset for left motor
double mR = 38; //pwm-offset for right motor
Encoder myEnc(3, 4); // Pins for Encoder input
// Variables
int wheelPos_old = 0, wheelPos; // Position
double wheelVel; // Velocity for wheels
double angle_ref_master = 0; // Anglular reference
double angle_ref_temp; // Temporary anglular reference
double angle;
double speed_ref = 0;
double pwm;
double pwmOut;
double t;
double t_old;
double dt_loop = 0.01; // Time intervall between pid-calculations
double HPF = 0.98; // "High pass filter"
double LPF = 0.02; // "Low pass filter"
double convertToRadians = 938.3954;
double kpo = 0, kio = 0; // pi - constants for outer loop
double kpi = 40, kii = 0, kdi = 0.1; //pid - constants for inner loop
MPU6050 accelgyro;
int16_t ax, ay, az; // acceleration components
int16_t gx, gy, gz; // gyro components
// Define inner and outer pid's
PID outerPID(&wheelVel, &angle_ref_temp, &speed_ref, kpo, kio, 0.0, DIRECT);
PID innerPID(&angle, &pwm, &angle_ref_temp, kpi, kii, kdi, DIRECT);
void setup() {
t_old = micros();
angle = 0;
pwm = 0;
pinMode(FWD_L, OUTPUT);
pinMode(BWD_L, OUTPUT);
pinMode(PWM_L, OUTPUT);
pinMode(FWD_R, OUTPUT);
pinMode(BWD_R, OUTPUT);
pinMode(PWM_R, OUTPUT);
outerPID.SetMode(AUTOMATIC);
innerPID.SetMode(AUTOMATIC);
innerPID.SetOutputLimits(-255, 255);
outerPID.SetOutputLimits(-5, 5);
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
Serial.begin(38400);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// use the code below to change accel/gyro offset values
accelgyro.setXAccelOffset(-1797);
accelgyro.setYAccelOffset(-1290);
accelgyro.setZAccelOffset(981);
accelgyro.setXGyroOffset(-6);
accelgyro.setYGyroOffset(88);
accelgyro.setZGyroOffset(-5);
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
}
/* returns the current velocity (unit: counts/s)
*/
double getVelocity() {
wheelPos = myEnc.read();
double deltaPos = (wheelPos-wheelPos_old)*0.00014398958;
wheelPos_old = wheelPos;
wheelVel = deltaPos/dt_loop;
return wheelVel;
}
/* Returns the current angular offset in radians
*/
double getAngle() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
angle = angle + gy/convertToRadians*dt_loop;
double accel_angle = -atan2(ax, az);
angle = HPF*angle+LPF*accel_angle; // The complementary filter
return angle;
}
/* Gives new command to the motors
*/
void setPWM(double pwm) {
if (pwm < 0) {
digitalWrite(FWD_L, HIGH);
digitalWrite(FWD_R, HIGH);
digitalWrite(BWD_L, LOW);
digitalWrite(BWD_R, LOW);
} else {
digitalWrite(FWD_L, LOW);
digitalWrite(FWD_R, LOW);
digitalWrite(BWD_L, HIGH);
digitalWrite(BWD_R, HIGH);
}
pwmOut = 17* abs(pwm); // Make the motors acquire top speed earlier
analogWrite(PWM_L, pwmOut +mL > 255 ? 255 : pwmOut+mL);
analogWrite(PWM_R, pwmOut +mR> 255 ? 255 : pwmOut+mR);
}
void loop() {
t = micros();
if ((t-t_old)/1000000 >= dt_loop) { // Update every 1/100 sec
// Outer control loop currently cancelled (kpo = 0, kio = 0)
wheelVel = getVelocity();
outerPID.Compute();
// inner loop, angular reference is 0
angle = getAngle();
innerPID.Compute();
setPWM(pwm);
t = t_old;
}
}
We have calibrated the MPU6050 according to the balancing point of the complete robot with battery and everything attached using the sketch found here:
www.i2cdevlib.com/forums/topic/96-arduino-sketch-to-automatically-calculate-mpu6050-offsets/
Our question to you is if you think that our control loop is sufficient to keep the robot balancing and if you have experience of any similar build? Since we have tried so many PID parameter settings we are starting to get a little worried that there is something more fundamentally wrong.
Best regards,
Erik & Erik