Rancang Bangun Robot Keseimbangan Menggunakan Sensor Mpu6050 Berbasis Atmega328

53

LAMPIRAN 1
PROGRAM LENGKAP

#include "Wire.h"
#include "MPU6050.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gry_offset 0
#define Gyr_Gain 0.00763358
#define Angle_offset 0
#define RMotor_offset 20
#define LMotor_offset 20
#define pi 3.14159
long data;
int x, y;
char recev;
float kp, ki, kd;
float r_angle, f_angle, omega;

float Turn_Speed = 0, Turn_Speed_K = 0;
float Run_Speed = 0, Run_Speed_K = 0, Run_Speed_T = 0;
float LOutput,ROutput;
unsigned long preTime = 0;
float SampleTime = 0.08;
unsigned long lastTime;
float Input, Output;
float errSum, dErr, error, lastErr;
int timeChange;
int ENA=9;
int ENB=6;

Universitas Sumatera Utara

54

int ENC=10;
int END=5;

void setup() {

Serial.begin(9600);
Wire.begin();
accelgyro.initialize();
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(ENC,OUTPUT);
pinMode(END,OUTPUT);
}
void loop() {
Recive();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
r_angle = (atan2(ay, az) * 180 / pi + Angle_offset);
omega =

Gyr_Gain * (gx +

Gry_offset);

if (abs(r_angle)= SampleTime){
Input = f_angle;

error = Input;
errSum += error * timeChange;
dErr = (error - lastErr) / timeChange;
Output = kp * error + ki * errSum + kd * dErr;
LOutput = Output + Run_Speed + (Turn_Speed);
ROutput = Output + Run_Speed - (Turn_Speed);
lastErr = error;
lastTime = now;
}
}
void PWMControl(){
if(LOutput > 0){
analogWrite(ENA, min(255, abs(LOutput) +
LMotor_offset));
analogWrite(ENB, 0);
}
else if(LOutput < 0){
analogWrite(ENA, 0);
analogWrite(ENB, min(255, abs(LOutput) +
LMotor_offset));

}
else{

Universitas Sumatera Utara

57

analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
if(ROutput > 0){
analogWrite(ENC, min(255, abs(LOutput) +
LMotor_offset));
analogWrite(END, 0);
}
else if(ROutput < 0){
analogWrite(ENC, 0);
analogWrite(END, min(255, abs(LOutput) +
LMotor_offset));
}

else{
analogWrite(ENC, 0);
analogWrite(END,0);
}

}

Universitas Sumatera Utara

58

LAMPIRAN 2
RANGKAIAN LENGKAP

Universitas Sumatera Utara