Published December 12, 2023 © CC0

Balancing Robot

Robot made with arduino that self-balances using PID control. You can see its construction, behavior with graphs, video, and codes here! 🤖

IntermediateFull instructions provided6 hours478

Things used in this project

Hardware components

Arduino Nano R3

SparkFun Motor Driver - Dual TB6612FNG (1A)

Ethernet Cable, Cat6a

pcb board

Inertial Measurement Unit (IMU) (6 deg of freedom)

Software apps and online services

MATLAB

Story

This was a university project aimed at applying PID control and its graphical behavior. The project focuses on balancing a two-wheeled robot using the MPU 6050 sensor. With this sensor, we can determine the inclination, and through Arduino code, control the motors to counteract the fall error. Ideally, the angle should be 90 degrees, but it is slightly less due to the battery, taking the x-axis of the sensor as a reference. Thanks to the PID control implemented in the code, the robot achieves balance

balancin robot

The following graph shows the response to the impulse that the system is undergoing, it is desired to maintain the system response in steady state, but in this case, the system begins to generate oscillations because the constants kp, ki and kd are not correct so the robot will take longer and longer corrections, also increasing its speed generating this oscilation.

octave graph

Octve code for the graph you can change your constants and see the behaviour

clear all, close all, clc;
pkg load control
Kp = 0.7;
Ki = 100;
Kd = 17;
C = pid(Kp, Ki, Kd);
numerator = 1;
denominator = [1, 1, 1, 0]; % Añade un polo adicional en s^3
P_pend = tf(numerator, denominator);
T = feedback(P_pend * C, 1);
t = 0:0.01:100;
impulse(T, t);
title({'Response of Pendulum Position to an Impulse Disturbance'; 'under PID Control: Kp = 0.7, Ki = 100, Kd = 17'});

Always in each project, I recommend conducting tests where you can save time and analyze errors before creating the robot on a PCB. This is the first design, but of course, before this, I followed my electrical schematic

1 / 2

To position the battery at the back, I recommend securing it with a flexible cable of an appropriate gauge, which can be molded to hold the battery. This is the final design after ensuring that it works well on a breadboard

Code

PID robot Arduino code

#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <SoftwareSerial.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define MIN_ABS_SPEED 30
MPU6050 mpu;
double MotorVelocidadIzq = 0.5;
double MotorVelocidadDer = 0.5;
double PuntoEquilibrio = 185.3;
SoftwareSerial Serial_2 (4, 3);
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 9;
int IN4 = 8;
int ENB = 10;
double Kp =0 .7;
double Kd = 17;
double Ki = 100;
int estado = 'g';
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3];
double originalSetpoint = PuntoEquilibrio;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);
double motorSpeedFactorLeft = MotorVelocidadIzq;
double motorSpeedFactorRight = MotorVelocidadDer;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
mpuInterrupt = true;
}
void setup()
{
Serial_2.begin(9600);
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24;
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

dmpReady = true;

packetSize = mpu.dmpGetFIFOPacketSize();
//setup PID
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
}
else
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void loop()
{
if (!dmpReady) return;
while (!mpuInterrupt && fifoCount < packetSize)
{
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);
}

mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if ((mpuIntStatus & 0x10) || fifoCount == 1024)
{
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else if (mpuIntStatus & 0x02)
{
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
input = ypr[1] * 180/M_PI + 180;
}
}