Scribd
Upload
149 views2 pages

Coding Self Balancing Robot

This document presents Arduino code for balancing a mobile robot using an MPU6050 sensor. It implements a PID control algorithm to maintain a target angle by adjusting motor speeds based on sensor input. The code includes setup and loop functions for reading sensor data and controlling motor outputs accordingly.

Uploaded by

Alif Faturohman
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
149 views2 pages

Coding Self Balancing Robot

This document presents Arduino code for balancing a mobile robot using an MPU6050 sensor. It implements a PID control algorithm to maintain a target angle by adjusting motor speeds based on sensor input. The code includes setup and loop functions for reading sensor data and controlling motor outputs accordingly.

Uploaded by

Alif Faturohman
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd

Research Report Malang, 17 Maret 2017

Balancing Mobile Robot Arduino Code

#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

float setPoint = 0; // target sudut


float input, output;
int16_t ax, ay, az, gx, gy, gz;
float kp = 20;
float ki = 1;
float kd = 0.5;

float previousError = 0, integral = 0;


unsigned long lastTime;

int motorLeft1 = 5;
int motorLeft2 = 6;
int motorRight1 = 9;
int motorRight2 = 10;

void setup()
{
[Link](9600);
[Link]();
[Link]();

pinMode(motorLeft1, OUTPUT);
pinMode(motorLeft2, OUTPUT);
pinMode(motorRight1, OUTPUT);
pinMode(motorRight2, OUTPUT);

lastTime = millis();
}

void loop()
{
//Ambil data MPU6050
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
input = atan2(ay, az) * 180 / PI;

1|Page
Research Report Malang, 17 Maret 2017

// PID
float error = setPoint - input;
unsigned long now = millis();
float dt = (now - lastTime) / 1000.0;

integral += error * dt;


float derivative = (error - previousError) / dt;
output = kp * error + ki * integral + kd * derivative;

previousError = error;
lastTime = now;

// Motor control
driveMotors(output);

delay(10);
}

void driveMotors(float pidValue)


{
int pwm = min(255, abs(pidValue));

if (pidValue > 0) {
// Move forward
analogWrite(motorLeft1, pwm);
analogWrite(motorLeft2, 0);
analogWrite(motorRight1, pwm);
analogWrite(motorRight2, 0);
} else {
// Move backward
analogWrite(motorLeft1, 0);
analogWrite(motorLeft2, pwm);
analogWrite(motorRight1, 0);
analogWrite(motorRight2, pwm);
}

2|Page

You might also like