Go Down

Topic: Balancing Robot falls if moved (Read 105 times) previous topic - next topic

praveen_khm

Oct 07, 2018, 02:53 pm Last Edit: Oct 07, 2018, 03:10 pm by praveen_khm
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.

Code: [Select]

#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

Go Up