#include <Wire.

h>
#include "gyro_accel.h"
//Defining constants
#define dt 0.02
//time difference in milli secon
ds
#define rad2degree 57.3
//Radian to degree conversion
#define Filter_gain 0.95
//e.g. angle=angle_gyro*Filter_gain+
angle_accel*(1-Filter_gain)
unsigned long t=0;
// Time Variables
float
angle_x_gyro=0,angle_y_gyro=0,angle_z_gyro=0,angle_x_accel=0,angle_y_accel=0,ang
le_z_accel=0;
#define maju_cepat 150
#define maju_lambat 100
#define diam 0
#define mundur_lambat -100
#define mundur_cepat -150
#define
#define
#define
#define
#define
#define

E1 5
E2 6
MKa1
MKa2
MKi1
MKi2

4
7
8
9

int error, derror;

int errorseb;
//var fuzzy
int setp = 2;
float nb, nk, nol, pk, pb;
float dnb, dnk, dnol, dpk, dpb;
float mula1, mula2, diam1, diam2, mala1, mala2;
float alfapn, alfap1, alfap2, alfap3, alfap4, alfap5, alfap6, alfap7, alfap8, al
fap9,
alfap10, alfap11, alfap12, alfap13, alfap14, alfap15, alfap16, alfap17, alfap18,
alfap19, alfap20, alfap21
alfap22, alfap23, alfap24, alfap25;
float z, Zn, z1, z2, z3, z4, z5, z6, z7, z8, z9, z10, z11, z12, z13, z14, z15, z
16, z17, z18, z19, z20, z21, z22, z23, z24, z25;
void setup(){
Serial.begin(115200);
Wire.begin();
MPU6050_ResetWake();
MPU6050_SetGains(0,1);
MPU6050_SetDLPF(0);
MPU6050_OffsetCal();
MPU6050_SetDLPF(6);
pinMode
pinMode
pinMode
pinMode
pinMode
pinMode

(E1, OUTPUT);
(E2, OUTPUT);
(MKa1, OUTPUT);
(MKa2, OUTPUT);
(MKi1, OUTPUT);
(MKi2, OUTPUT);

t=micros();
}
void loop(){

t=micros();
MPU6050_ReadData();
angle_x_gyro = (gyro_x_scalled*((float)dt/100)+angle_x);
angle_y_gyro = (gyro_y_scalled*((float)dt/100)+angle_y);
angle_z_gyro = (gyro_z_scalled*((float)dt/100)+angle_z);
angle_z_accel=
atan(accel_z_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_x_scalled*accel
_x_scalled)))*
(float)rad2degree;
angle_y_accel=
atan(accel_z_scalled/(sqrt(accel_y_scalled*accel_y_scalled+accel_z_scalled*accel
_z_scalled)))*
(float)rad2degree;
angle_x_accel=
atan(accel_z_scalled/(sqrt(accel_x_scalled*accel_x_scalled+accel_z_scalled*accel
_z_scalled)))*
(float)rad2degree;
angle_x=Filter_gain*angle_x_gyro+(1-Filter_gain)*angle_x_accel;
angle_y=Filter_gain*angle_y_gyro+(1-Filter_gain)*angle_y_accel;
angle_z=Filter_gain*angle_z_gyro+(1-Filter_gain)*angle_z_accel;
while((millis()-t)<dt){
if(angle_x<=-1 && angle_x>2){
fuzzy();
Serial.print("Output PWM: ");
Serial.println(int(z));
motor(z,z);}
else{
motor(50,50);
}
delay(50);
}
}
void fuzzy(){
error=setp - angle_x;
derror=error-errorseb;
errorseb=error;
Serial.print("error= ");
Serial.println(error);
Serial.print("derror= ");
Serial.println(derror);
if(error>10){
nb=0;
nk=0;
nol=0;
pk=0;
pb=1;
}
else if((error>=5) && (error<=10)){
nb=0;
nk=0;
nol=0;
pk=float((10-error)/5);

}
else if((error>=0))