Rancang Bangun Robot Keseimbangan (Balancing Robot) Part 2

Setelah seluruh hardware telah terbuhung kita akan memasukan program pada arduino mega. Pertama tama cari dan download library mpu 6050 kemudian extrak fileny ke folder arduino.ide kalian.Setelah itu buka aplikasi arduino.ide kalian kemudian copy paste program yang saya tuliskan berikut.

Code Arduino Robot Keseimbangan :

#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 0 
#define LMotor_offset 0
#define pi 3.14159  

long data; 
int x, y; 
float kp, ki, kd;  
float r_angle, f_angle, omega; 
float LOutput,ROutput; 
unsigned long preTime = 0; 
unsigned long lastTime; 
float Input, Output; 
float errSum, dErr, error, lastErr; 
int timeChange;  
int pin = 4; 
int TN2=12; 
int ENA=10; 
int TN4=13; 
int ENB=11; 

void setup()  
{ 
 Wire.begin(); 
 Serial.begin(115200); 
 accelgyro.initialize(); 
 pinMode(TN2,OUTPUT); 
 pinMode(TN4,OUTPUT); 
 pinMode(ENA,OUTPUT); 
 pinMode(ENB,OUTPUT); 
 pinMode(pin, OUTPUT);  
 dot();  
 dot();  
 dot(); 
 delay(100);  
} 

void loop()  
{  
 filter(); 
 if (abs(f_angle)<35) 
 { 
   myPID(); 
   PWMControl(); 
   digitalWrite(pin, LOW);   
 } 
 else 
 {     
 analogWrite(ENA, 0); 
 analogWrite(ENB, 0); 
 digitalWrite(pin, HIGH); 
 delay(50); 
 digitalWrite(pin, LOW); 
 delay(1000); 

    for(int i = 0; i < 1; i++)   
   { 
     filter(); 
   } 
   if(abs(f_angle) < 45)   
   { 
     for(int i = 0; i <= 1; i++)   
     { 
       omega = r_angle = f_angle = 0; 
       filter(); 
       Output = error = errSum = dErr = 0; 
       myPID(); 
     } 
   } 
  } 
} 

void filter() 
{ 
 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 
 r_angle = (atan2(ay, az) * 180 / pi + Angle_offset); 
 omega =  Gyr_Gain * (gx +  Gry_offset);   
 //Serial.print(”  omega=”); Serial.print(omega); 
 unsigned long now = millis(); 
 float dt = (now -preTime) / 1000.0; 
 preTime = now; 
 float K = 0.8; 
 float A = K / (K + dt); 
 f_angle = A * (f_angle + omega * dt) + (1 – A) * r_angle;   
// Serial.print(”  f_angle=”);Serial.print(f_angle);   
} 
void dot() 
{ 
 digitalWrite(pin, HIGH); 
 delay(50); 
 digitalWrite(pin,); 
 delay(30); 
} 
void myPID() 
{ 
 kp = 46;  //Serial.print(”  kp=”);Serial.print(kp); 
 kd = 680; // Serial.print(”  kd=”);Serial.print(kd); 
 ki = 0.15; //Serial.print(”  ki=”);Serial.print(ki);   
 unsigned long now = millis(); 
 timeChange = (now – lastTime); 
 lastErr= error;   
 lastTime = now; 
   Input = f_angle; 
   error = Input; 
   errSum += error * timeChange; 
   dErr = (error – lastErr) / timeChange; 
   Output = kp * error + ki * errSum + kd * dErr;
   LOutput = Output;   
   ROutput = Output;   
  Serial.print(”  error”);Serial.print(error); 
  Serial.print(”  output”);Serial.print(Output); 
  Serial.print(”  now”);Serial.println(now);     
} 
void PWMControl() 
{ 
 if(LOutput > 0) 
 { 
   digitalWrite(TN2, LOW); 
 } 
 else 
 {   
   digitalWrite(TN2, HIGH); 
 } 
 if(ROutput > 0) 
 {     
   digitalWrite(TN4, LOW); 
 } 
 else 
 {    
   digitalWrite(TN4, HIGH); 
 } 
   analogWrite(ENA, min(255, abs(LOutput) + LMotor_offset)); 
   analogWrite(ENB, min(255, abs(ROutput) + RMotor_offset)); 
} 

Kemudian setelah program selesai di upload, posisikan robot balancing tegak lurus lalu hidupkan switch ke posisi on. Jika kalian mengikuti step by step yang saya tulis maka robot harusnya dapat menyeimbangkan dirinya dengan baik. Jika robot balancing yang kalian buat tidak dapat seimbang silahkan komentar dibawah kendala apa yang kalian dapati, maka saya akan bantu mencarikan solusi agar robot balancing yang anda buat dapat seimbang dengan sempurna.

Berikut adalah hasil robot keseimbangan yang saya buat :

Terimakasih sudah berkunjung, itulah artikel tentang Program Robot Keseimbangan atau Robot Balancing, semoga bermanfaat dan jangan lupa untuk berkunjung kembali.