Arduino Self Balancing Robot controled by PID (Part 1)

Bismillah,

Beberapa hari yang lalu saya membuat sebuah Self Balancing Robot dengan menggunakan komponen yang beredar di toko online, dan saya jelaskan lengkap nya di chanel youtube saya dan disini saya hanya share source code nya saja.



#include <MPU6050_tockn.h>
#include <Wire.h>

MPU6050 mpu6050(Wire);

const int pwm_pin[2] = {9,10};
const int directA_pin[2] = {7,2};
const int directB_pin[2] = {5,4};   

float pid=0;

float kp=13,kd=120,ki=0.5;

float angleLimit=20;

float setPoint=3.5;

int timeSampling=1;

float speedMax=200;//max 255

float offsetValue=2;

float nowError=0;

float lastError=0;

float valueX=0;



int cek=0;

void mundur_all(int value){
  digitalWrite(directA_pin[0], LOW);//kanan
  digitalWrite(directA_pin[1], HIGH);

  digitalWrite(directB_pin[0], HIGH);//kiri
  digitalWrite(directB_pin[1], LOW);

  analogWrite(pwm_pin[0],value);//kanan
  analogWrite(pwm_pin[1],value);//kiri
}

void stop_all(){
  digitalWrite(directA_pin[0], HIGH);
  digitalWrite(directA_pin[1], HIGH);

  digitalWrite(directB_pin[0], HIGH);
  digitalWrite(directB_pin[1], HIGH);

  analogWrite(pwm_pin[0],0);
  analogWrite(pwm_pin[1],0);
}

void maju_all(int value){
  digitalWrite(directA_pin[0], HIGH);//kanan
  digitalWrite(directA_pin[1], LOW);

  digitalWrite(directB_pin[0], LOW);//kiri
  digitalWrite(directB_pin[1], HIGH);

  analogWrite(pwm_pin[0],value);//kanan
  analogWrite(pwm_pin[1],value);//kiri
}

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

  // initialize the LED pin as an output:
  pinMode(pwm_pin[0], OUTPUT);
  pinMode(pwm_pin[1], OUTPUT);
  pinMode(directA_pin[0], OUTPUT);
  pinMode(directA_pin[1], OUTPUT);
  pinMode(directB_pin[0], OUTPUT);
  pinMode(directB_pin[1], OUTPUT);
  stop_all();

  TCCR1A=(1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (0<<WGM11) | (1<<WGM10);
  TCCR1B=(0<<ICNC1) | (0<<ICES1) | (0<<WGM13) | (0<<WGM12) | (0<<CS12) | (1<<CS11) | (1<<CS10);
  TCNT1H=0x00;
  TCNT1L=0x00;
  ICR1H=0x00;
  ICR1L=0x00;
  OCR1AH=0x00;
  OCR1AL=0x00;
  OCR1BH=0x00;
  OCR1BL=0x00;

}

void loop() {
  mpu6050.update();

  valueX=mpu6050.getAngleX();
  //Serial.print("angleX : ");
  //Serial.println(valueX);
  /*Serial.print("\tangleY : ");
  Serial.print(mpu6050.getAngleY());
  Serial.print("\tangleZ : ");
  Serial.println(mpu6050.getAngleZ());
  */

  if(valueX<(setPoint+offsetValue) && valueX>(setPoint-offsetValue)){
    pid=0;
   
  }
  else{
    // PID kontroler

    nowError=setPoint-valueX;

    pid=(kp*nowError)+(kd*(nowError-lastError))+(ki*(nowError+lastError));
   
    lastError=nowError;
   
  }

  pid=abs(pid);
  if(pid>speedMax){
    pid=speedMax;
  }
 
 

  //Serial.print(" pid : ");
  //Serial.print(pid);
 
  if(nowError>0&&nowError<angleLimit){//maju
    //mundur_all(pid);
    maju_all(pid);
    //Serial.println(" maju");
  }

  else if(nowError<0&&nowError>-angleLimit){//mundur
    //maju_all(pid);
    mundur_all(pid);
    //Serial.println(" mundur");
  }

  else if(nowError<-angleLimit){//mundur
    //maju_all(speedMax);
    mundur_all(speedMax);
    //Serial.println(" mundur");
  }

  else if(nowError>angleLimit){//maju
    maju_all(speedMax);
    //mundur_all(speedMax);
    //Serial.println(" maju");
  }

  //time sampling

  delay(timeSampling);
   
}


Source code diatas silahkan di sesuaikan dengan hardware teman-teman, contoh maju dan mundur atau sumbu gyro mana yang di pakai. sebelum menggunakan gyro install dulu lib MPU6050_tockn, bisa di cari di arduino lib manager.

Semoga bermanfaat dan silahkan bertanya..

* link video di chanel youtube saya https://youtu.be/LjdkZc6ddlk