Self balancing robot Arduino code ~ Arduino Geek

Hello everyone! Welcome to Arduino Geek. Today we will discuss about the Arduino code for self balancing robot. So let's get started.

Self balancing robot Arduino code:

Here is a basic Arduino code for a self balancing robot using an MPU6050 accelerometer and gyroscope module:

Arduino Code:

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

#define MOTOR1_PIN1 6
#define MOTOR1_PIN2 5
#define MOTOR2_PIN1 10
#define MOTOR2_PIN2 9

Adafruit_MPU6050 mpu;

float angle, gyro, acc, accAngle, gyroAngle, angleError, previousError, motorSpeed, Kp, Ki, Kd;

void setup() {
pinMode(MOTOR1_PIN1, OUTPUT);
pinMode(MOTOR1_PIN2, OUTPUT);
pinMode(MOTOR2_PIN1, OUTPUT);
pinMode(MOTOR2_PIN2, OUTPUT);

Serial.begin(9600);

while (!Serial) {
delay(10);
}

Serial.println("Initializing MPU6050...");
mpu.begin();
Serial.println("MPU6050 initialized.");
delay(1000);

Kp = 10; // Proportional gain
Ki = 0.2; // Integral gain
Kd = 0.1; // Derivative gain
}

void loop() {
angle = getAngle();
angleError = angle - 0;
acc = getAcc();
accAngle = atan2(acc, sqrt(pow(mpu.getAccelerationY(), 2) + pow(mpu.getAccelerationZ(), 2))) * 180 / M_PI;
gyro = getGyro();
gyroAngle += gyro * 0.0000611;
angle = 0.98 * (angle + gyro * 0.0000611) + 0.02 * accAngle;

motorSpeed = Kp * angleError + Ki * previousError + Kd * (angleError - previousError);
previousError = angleError;

if (motorSpeed > 255) {
motorSpeed = 255;
} else if (motorSpeed < -255) {
motorSpeed = -255;
}

if (motorSpeed > 0) {
analogWrite(MOTOR1_PIN1, motorSpeed);
analogWrite(MOTOR2_PIN1, motorSpeed);
digitalWrite(MOTOR1_PIN2, LOW);
digitalWrite(MOTOR2_PIN2, LOW);
} else {
analogWrite(MOTOR1_PIN2, -motorSpeed);
analogWrite(MOTOR2_PIN2, -motorSpeed);
digitalWrite(MOTOR1_PIN1, LOW);
digitalWrite(MOTOR2_PIN1, LOW);
}

Serial.print("Angle: ");
Serial.print(angle);
Serial.print(" Motor speed: ");
Serial.println(motorSpeed);
}

float getAngle() {
float roll = mpu.getRotationX() * 0.0000611;
float pitch = mpu.getRotationY() * 0.0000611;
gyro = mpu.getRotationZ() * 0.0000611;
float temp = mpu.getTemperature() / 340.00 + 36.53;
return atan2(-roll, -pitch) * 180 / M_PI;
}

float getGyro() {
return mpu.getRotationZ() * 0.0000611;
}

float getAcc() {
return mpu.getAccelerationX() * 9.81;
}

Arduino Code Description:

This code reads data from an MPU6050 accelerometer and gyroscope, and uses that data to balance a two-wheeled robot. The robot is controlled by two motors connected to pins 5, 6, 9, and 10 on the Arduino board.

The code calculates the angle of the robot by combining data from the accelerometer and gyroscope. It then calculates the motor speed needed to balance the robot using a PID controller, which adjusts the motor speed based on the difference between the desired angle (0 degrees) and the current angle.

Self balancing robot Arduino code