Go Down

Topic: Problem with conventional balancing robot (youtube video + code included). (Read 1 time) previous topic - next topic

EngiErik

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 https://www.pololu.com/product/2823)
2 pcs Pololu Wheel 90x10mm Pair - Black
The rest is mainly brackets and screws.

Code: [Select]
// 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

Southpark

First of all ...... let me tell you now that your robot is going to be able to balance..... and it is going to be able to balance very well.

Next.....  your code will need to work fast...and work quick as possible.... with a minimum amount of delays due to unnecessary instructions (like serial communications instructions such as lots of serial print statements). These things slow down the control code. So knowing this will help a lot.

This means.... for initial testing of sensors etc..... use a fast serial speed ... like 115200 bit per second... or more.

Then.... in final testing (actual performance testing) ..... you can either turn off serial communications...ie don't use it at all .... or comment-out all the serial print statements.

Once you done all this.... you'll find that your robot may actually respond properly, instead of wasting time handling serial communications instructions etc.

At the moment...... just by looking at your video..... it looks like your robot is well on the way to working properly. You're almost there actually. The stage that you're at right now is actually good. Very good.

Also....... instead of updating very 0.01 second ...... try something like 0.001 second updates.

jremington

One serious problem with your robot is the motor driver.

The L298 cannot handle the 5 Ampere stall current of your motors, which they draw every time they start up. Motors draw TWICE the stall current when they are rapidly reversed.

Pololu has appropriate motor drivers.

Southpark

Good observation from jrem. Before using any devices and equipment, know in advance the requirements of the device and the capabilities of the device. This includes power handling / current handling.

At the moment, from the video ...... it appears that the robot will still work. But if something does burn out in the end..... it will probably be the L298N.

jremington

The L298 will try to work, but may deliver insufficient power to the motors before overheating.

This can make tuning the PID loop impossible.

EngiErik

Thank you for your quick and informative answers.
We will take a look at the mentioned things and come back to
this thread and post results.

The L298 problem sounds like a big factor!


EngiErik

Hi

There has been some progress in the development of our balancing robot but we have not yet fully succeeded.

The changes that have been made are the following:
* The motor driver has been changed from one L298 to two BB-VNH3SP30 which
should be more than capable of handling our desired currents. They can be found here: https://www.olimex.com/Products/RobotParts/MotorDrivers/BB-VNH3SP30/open-source-hardware
* All serial-related code has been removed from our script during pid-tuning.
* The number of iterations/sec have been doubled.

Youtube video of the robot with the new motor drivers, but with the same pid tunings as in our first attempt: https://youtu.be/0_bPE4BGG8M

Youtube videos of the robot with the new motor drivers, but with new tunings according to the new attached code:
First: https://youtu.be/9VA0Xh9nxAY
Second: https://youtu.be/wXAjm8QqH6A

An observation we make is that the robot still has a hard time "saving a fall". If we crank up the proportional part it instead goes unstable around the equilibrium point.

Code: [Select]

// 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 = 7;
int BWD_L = 6;
int BWD_R = 9;
int FWD_R = 8;
int PWM_R = 10;

int EN_L = 11; // The EN/Diag pin of the VNH3SP30 must be set to high in order to send signal to motor.
int EN_R = 12; // -||-

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.005;          // 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 = 15, kii = 175, 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);
        pinMode(EN_L, OUTPUT);
        pinMode(EN_R, OUTPUT);
        digitalWrite(EN_L, HIGH);
        digitalWrite(EN_R, HIGH);

        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 =  23* 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;
        }
}


Do you have any suggestions what we can look into for further improving our robot?

/ E&E

jremington

Quote
An observation we make is that the robot still has a hard time "saving a fall". If we crank up the proportional part it instead goes unstable around the equilibrium point.
If by unstable, you mean it oscillates about the equilibrium point, then that is exactly what you expect.

There are lots of on-line tutorials on how to approach tuning methodically, almost always starting with Kp (with Kd = Ki = 0), then adjusting Kd, etc.

Southpark

double dt_loop = 0.005;          // Time intervall between pid-calculations
Try 0.001 second, or 1 millisecond update time.

Go Up