22 April 2023

ROBOTIKA 3: Robot Line Follower dengan 8 Sensor

A. Alat dan Bahan:

1. Sensor 8 Channel dari Protronik
2. Modul Drivel L298N
3. Arduino UNO
4. Kabel Jumper
5. Baterai Lithium 2 Buah
6. Holder Baterai Lithium
7. Saklar

B. Wiring

ENA = 10; // pengatur kecepatan motor kiri di pin 10 pwm
IN1 = 9; //IN1 motor kiri 
IN2 = 8; //IN2 motor kiri 
IN3 = 5; //IN3 motor kanan
IN4 = 4; //IN4 motor kanan
ENB = 3; // pengatur kecepatan motor kanan di pin 3 pwm

//posisi wiring sensor depan 
int inh=A5;           //ena
int selA=A4;          //A
int selB=A3;          //B
int selC=A2;          //C
int sensor_depan=A1;  //Out

C. Sketch yang Sudah diketik:

LINE FOLLOWER TYPE BIASA

int sensor0;
int sensor1;
int sensor2;
int sensor3;
int sensor4;
int sensor5;
int sensor6;
int sensor7;

// Inisialisasi nilai sensor
int sensor[8] = {0, 0, 0, 0, 0, 0, 0, 0 };

// Penetapan Nama Variabel motor dan pin yang ditentukan
int ENA = 10; // pengatur kecepatan motor kiri di pin 10 pwm
int IN1 = 9; //IN1 motor kiri 
int IN2 = 8; //IN2 motor kiri 
int IN3 = 5; //IN3 motor kanan
int IN4 = 4; //IN4 motor kanan
int ENB = 3; // pengatur kecepatan motor kanan di pin 3 pwm

//inisialisasi penetapan power kecepatan motor
int Speed_Utama = 200; // Speed_Utama bernilai 0 hingga 250

float error = 0;

//posisi wiring sensor depan 
int inh=A5;           //ena
int selA=A4;          //A
int selB=A3;          //B
int selC=A2;          //C
int sensor_depan=A1;  //Out

int data_sensor_depan[8];

void read_sensor_values(){
  //void sensing
  digitalWrite(inh, LOW);
  
  digitalWrite(selA, LOW);
  digitalWrite(selB, LOW);
  digitalWrite(selC, LOW);
  data_sensor_depan[0]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, HIGH);
  digitalWrite(selB, LOW);
  digitalWrite(selC, LOW);
  data_sensor_depan[1]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, LOW);
  digitalWrite(selB, HIGH);
  digitalWrite(selC, LOW);
  data_sensor_depan[2]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, HIGH);
  digitalWrite(selB, HIGH);
  digitalWrite(selC, LOW);
  data_sensor_depan[3]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, LOW);
  digitalWrite(selB, LOW);
  digitalWrite(selC, HIGH);
  data_sensor_depan[4]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, HIGH);
  digitalWrite(selB, LOW);
  digitalWrite(selC, HIGH);
  data_sensor_depan[5]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, LOW);
  digitalWrite(selB, HIGH);
  digitalWrite(selC, HIGH);
  data_sensor_depan[6]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, HIGH);
  digitalWrite(selB, HIGH);
  digitalWrite(selC, HIGH);
  data_sensor_depan[7]=analogRead(sensor_depan);
  delay(20);

  
 //definisikan menjadi angka biner
  
  if(data_sensor_depan[0]>750){sensor0=1;}else{sensor0=0;}

  if(data_sensor_depan[1]>750){sensor1=1;}else{sensor1=0;}

  if(data_sensor_depan[2]>750){sensor2=1;}else{sensor2=0;}

  if(data_sensor_depan[3]>750){sensor3=1;}else{sensor3=0;}

  if(data_sensor_depan[4]>750){sensor4=1;}else{sensor4=0;}

  if(data_sensor_depan[5]>750){sensor5=1;}else{sensor5=0;}

  if(data_sensor_depan[6]>750){sensor6=1;}else{sensor6=0;}

  if(data_sensor_depan[7]>750){sensor7=1;}else{sensor7=0;}

  
  //definisi ke sensor 0-7
  sensor[0] = sensor0;
  sensor[1] = sensor1;
  sensor[2] = sensor2;
  sensor[3] = sensor3;
  sensor[4] = sensor4;
  sensor[5] = sensor5;
  sensor[6] = sensor6;
  sensor[7] = sensor7;

  //tampilkan ke serial monitor
  Serial.print(sensor0);
  Serial.print(sensor1);
  Serial.print(sensor2);
  Serial.print(sensor3);
  Serial.print(sensor4);
  Serial.print(sensor5);
  Serial.print(sensor6);
  Serial.print(sensor7);
  
 //menentukan nilai definisi error dari hasil sensor menjadi nilai error
    if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0) && (sensor[5]==0) && (sensor[6] == 0)&&(sensor[7]==0)) 
    error = 6; // 10000000
    else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 5; // 11000000 
    else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 4; // 01000000 
    else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 3; // 01100000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 2; // 00100000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1)&& (sensor[4] == 1)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 1; // 00110000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 1)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 0; // 00010000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 1)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 0; // 00011000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 1)&&(sensor[5]==1)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 0; // 00001000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==1)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = -1; // 00001100
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==1)&& (sensor[6] == 1)&&(sensor[7]==0))
    error = -2; // 00000100
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 1)&&(sensor[7]==0))
    error = -3; // 00000110 
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 1)&&(sensor[7]==1))
    error = -4; // 00000010
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==1))
    error = -5; // 00000011
    // kondisi khas tertentu        
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0) && (sensor[6] == 0)&&(sensor[7]==1)) 
    error = -6; // 00000001 

    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0) && (sensor[6] == 0)&&(sensor[7]==0)) 
    error = 10; // 00000000
}

void tampil_sensor(){
  Serial.print(" ERROR: ");
  Serial.print(error);
  
  Serial.print(data_sensor_depan[0]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[1]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[2]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[3]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[4]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[5]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[6]);
  Serial.print(", ");
  Serial.println(data_sensor_depan[7]);
}

void setup() {
  Serial.begin(9600);
  pinMode (inh, OUTPUT);
  pinMode (selA, OUTPUT);
  pinMode (selB, OUTPUT);
  pinMode (selC, OUTPUT);
}

void loop(){
  read_sensor_values();
  
  if(error==0){
  forward();
  }else if(error==5){
    sharpLeftTurn();
  }else if(error==-5){
    sharpRightTurn();
  }else if(error==10){
     stop_bot();
  }
}


void forward(){
  analogWrite(ENA, Speed_Utama); 
  analogWrite(ENB, Speed_Utama); 
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void reverse(){
  analogWrite(ENA, Speed_Utama); 
  analogWrite(ENB, Speed_Utama);
  digitalWrite(IN1, LOW); 
  digitalWrite(IN2, HIGH); 
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void right(){
  analogWrite(ENA, Speed_Utama); 
  analogWrite(ENB, Speed_Utama);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}
void left(){
  analogWrite(ENA, Speed_Utama); 
  analogWrite(ENB, Speed_Utama);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void sharpLeftTurn(){
  analogWrite(ENA, Speed_Utama); 
  analogWrite(ENB, Speed_Utama);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void sharpRightTurn() {
  analogWrite(ENA, Speed_Utama); 
  analogWrite(ENB, Speed_Utama);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
  
}
void stop_bot(){
  analogWrite(ENA, 0); //Left Motor Speed
  analogWrite(ENB, 0); //Right Motor Speed
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}




ROBOT LINE FOLLOWER PID:

int sensor0;
int sensor1;
int sensor2;
int sensor3;
int sensor4;
int sensor5;
int sensor6;
int sensor7;

// Inisialisasi nilai sensor
int sensor[8] = {0, 0, 0, 0, 0, 0, 0, 0 };

// Penetapan Nama Variabel motor dan pin yang ditentukan
int ENA = 10; // pengatur kecepatan motor kiri di pin 10 pwm
int IN1 = 9; //IN1 motor kiri 
int IN2 = 8; //IN2 motor kiri 
int IN3 = 5; //IN3 motor kanan
int IN4 = 4; //IN4 motor kanan
int ENB = 3; // pengatur kecepatan motor kanan di pin 3 pwm

//inisialisasi penetapan power kecepatan motor
int Speed_Utama = 130; // Speed_Utama bernilai 0 hingga 250
int Speed_Kiri = Speed_Utama; //penentuan kecepatan motor kiri
int Speed_Kanan = Speed_Utama; //penentuan kecepatan motor kanan

int left_motor_speed; // membuat variabel kecepatan motor kiri
int right_motor_speed; // membuat variabel kecepatan motor kanan

// konstanta proporsional, integral, dan derivatif. disini nilai disesuaikan
float Kp = 0 ;
float Ki = 0 ;
float Kd = 0 ;

float error = 0, P = 0, I = 0, D = 0, PID_value = 0; // penetapan nilai awal = 0
float previous_error = 0, previous_I = 0;


//posisi wiring sensor depan 
int inh=A5;           //ena
int selA=A4;          //A
int selB=A3;          //B
int selC=A2;          //C
int sensor_depan=A1;  //Out
int data_sensor_depan[8];

void read_sensor_values(){
  //void sensing
  digitalWrite(inh, LOW);
  
  digitalWrite(selA, LOW);
  digitalWrite(selB, LOW);
  digitalWrite(selC, LOW);
  data_sensor_depan[0]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, HIGH);
  digitalWrite(selB, LOW);
  digitalWrite(selC, LOW);
  data_sensor_depan[1]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, LOW);
  digitalWrite(selB, HIGH);
  digitalWrite(selC, LOW);
  data_sensor_depan[2]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, HIGH);
  digitalWrite(selB, HIGH);
  digitalWrite(selC, LOW);
  data_sensor_depan[3]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, LOW);
  digitalWrite(selB, LOW);
  digitalWrite(selC, HIGH);
  data_sensor_depan[4]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, HIGH);
  digitalWrite(selB, LOW);
  digitalWrite(selC, HIGH);
  data_sensor_depan[5]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, LOW);
  digitalWrite(selB, HIGH);
  digitalWrite(selC, HIGH);
  data_sensor_depan[6]=analogRead(sensor_depan);
  delay(20);
  digitalWrite(selA, HIGH);
  digitalWrite(selB, HIGH);
  digitalWrite(selC, HIGH);
  data_sensor_depan[7]=analogRead(sensor_depan);
  delay(20);

  
 //definisikan menjadi angka biner
  
  if(data_sensor_depan[0]>750){sensor0=1;}else{sensor0=0;}

  if(data_sensor_depan[1]>750){sensor1=1;}else{sensor1=0;}

  if(data_sensor_depan[2]>750){sensor2=1;}else{sensor2=0;}

  if(data_sensor_depan[3]>750){sensor3=1;}else{sensor3=0;}

  if(data_sensor_depan[4]>750){sensor4=1;}else{sensor4=0;}

  if(data_sensor_depan[5]>750){sensor5=1;}else{sensor5=0;}

  if(data_sensor_depan[6]>750){sensor6=1;}else{sensor6=0;}

  if(data_sensor_depan[7]>750){sensor7=1;}else{sensor7=0;}

  
  //definisi ke sensor 0-7
  sensor[0] = sensor0;
  sensor[1] = sensor1;
  sensor[2] = sensor2;
  sensor[3] = sensor3;
  sensor[4] = sensor4;
  sensor[5] = sensor5;
  sensor[6] = sensor6;
  sensor[7] = sensor7;

  //tampilkan ke serial monitor
  Serial.print(sensor0);
  Serial.print(sensor1);
  Serial.print(sensor2);
  Serial.print(sensor3);
  Serial.print(sensor4);
  Serial.print(sensor5);
  Serial.print(sensor6);
  Serial.print(sensor7);
  
 //menentukan nilai definisi error dari hasil sensor menjadi nilai error
    if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0) && (sensor[5]==0) && (sensor[6] == 0)&&(sensor[7]==0)) 
    error = 6; // 10000000
    else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 5; // 11000000 
    else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 4; // 01000000 
    else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 3; // 01100000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 2; // 00100000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1)&& (sensor[4] == 1)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 1; // 00110000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 1)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 0; // 00010000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 1)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 0; // 00011000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 1)&&(sensor[5]==1)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = 0; // 00001000
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==1)&& (sensor[6] == 0)&&(sensor[7]==0))
    error = -1; // 00001100
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==1)&& (sensor[6] == 1)&&(sensor[7]==0))
    error = -2; // 00000100
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 1)&&(sensor[7]==0))
    error = -3; // 00000110 
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 1)&&(sensor[7]==1))
    error = -4; // 00000010
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0)&& (sensor[6] == 0)&&(sensor[7]==1))
    error = -5; // 00000011
    // kondisi khas tertentu        
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0)&&(sensor[5]==0) && (sensor[6] == 0)&&(sensor[7]==0)) 
    error = -6; // 00000001  
}

void tampil_sensor(){
  Serial.print(data_sensor_depan[0]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[1]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[2]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[3]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[4]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[5]);
  Serial.print(", ");
  Serial.print(data_sensor_depan[6]);
  Serial.print(", ");
  Serial.println(data_sensor_depan[7]);
}
void setup() {
  Serial.begin(9600);
  pinMode (inh, OUTPUT);
  pinMode (selA, OUTPUT);
  pinMode (selB, OUTPUT);
  pinMode (selC, OUTPUT);
}

void loop(){
  read_sensor_values();
  calculate_pid();
  motor_control();
  forward();
 
}
void calculate_pid(){ //perhitungan nilai PID
  previous_I = I;
  previous_error = error;
  P = error;
  I = I + previous_I; 
  D = error - previous_error;

  PID_value = (Kp * P) + (Ki * I) + (Kd * D);
  }

void motor_control()
{
  // Calculating the effective motor speed:
  int left_motor_speed = Speed_Utama - PID_value;
  int right_motor_speed = Speed_Utama + PID_value;

  // The motor speed should not exceed the max PWM value
  left_motor_speed = constrain(left_motor_speed, 0, Speed_Kiri);
  right_motor_speed = constrain(right_motor_speed, 0, Speed_Kanan);
  
  analogWrite(ENA, left_motor_speed); //kecepatan motor kiri yang sudah dipengaruhi PID 
  analogWrite(ENB, right_motor_speed); //Kecepatan motor kanan yang sudah dipengaruhi PID

  // Menampilkan nilai-nilai di serial monitor
  Serial.print(" ERROR: ");
  Serial.print(error);
  Serial.print(" PID: ");
  Serial.print(PID_value);
  Serial.print(" KEC KIRI: ");
  Serial.print(left_motor_speed);
  Serial.print(" KEC KANAN: ");
  Serial.println(right_motor_speed);
}

void forward(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void reverse(){
  digitalWrite(IN1, LOW); 
  digitalWrite(IN2, HIGH); 
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void right(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}
void left(){
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void sharpLeftTurn(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void sharpRightTurn() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
  
}
void stop_bot(){
  analogWrite(ENA, 0); //Left Motor Speed
  analogWrite(ENB, 0); //Right Motor Speed
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

By -

21 April 2023

ROBOTIKA 2: Robot Line Follower

A.     BAHAN DAN ALAT YANG DIPERLUKAN 

No

BAHAN

jml

Perkiraan Harga (Rp)

1

Kit Smart Chasis 2WD Robot Car

1

 

2

Arduino Uno

1

 

3

Sensor TCRT5000 5 channel

1

 

4

Arduino Protoshield

1

 

5

Holder Battery 18650 2 slot

1

 

6

Baterai Lithium 18650 3,7 Volt

2

 

7

Driver Motor DC L298N

1

 

8

Spacer dan Bracket sensor

1

 

9

Kabel Jumper

1

 

10

Saklar

1

 

Total Rp.

 


B. TOPOLOGI ROBOT LINE FOLLOWER (khusus garis hitam menggunakan selotip)


C. PENENTUAN LOKASI PIN UNTUK SENSOR DAN DRIVER


No.

Sensor TCRT5000 5 channel

Arduino UNO

1

VCC

5V

2

GND

GND

3

S1

A0

4

S2

A1

5

S3

A2

6

S4

A3

7

S5

A4




No.

Driver Motor L298N

Arduino UNO

1

ENA

10

2

IN1

9

3

IN2

8

4

IN3

5

5

IN4

4

6

ENB

3

7

Power 12V

Vin

8

Power GND

GND

D. SKETCH PENGUJIAN FUNGSI UTAMA ROBOT BERODA

   Dalam pengujian ini diharapkan untuk mengetahui fungsi dasar robot beroda yang meliputi fungsi sensor garis, fungsi robot lurus, fungsi lurus/mundur/belok. Untuk fungsi sensor garis yang diuji ke lintasan bergaris hitam, nilai yang keluar dari sensor ditayangkan di Serial Monitor pada software Arduino IDE sebagai dasar untuk mengetahui sensor apa yang merespon garis.

1. SKETCH UJI SENSOR GARIS (5 BUAH SENSOR)

SKETCH YANG PERLU DIKETIK:
// Inisialisasi Sensor TCRT5000 5 channel
int sensor1 = A0; // Sensor paling kiri S1 ke A0
int sensor2 = A1; // S2 ke A1
int sensor3 = A2; // S3 ke A2
int sensor4 = A3; // S4 ke A3
int sensor5 = A4; // sensor paling kanan S5 ke A4

// Inisialisasi nilai sensor
int sensor[5] = {0, 0, 0, 0, 0}; // susunan nilai sensor dari S1,S2,S3,S4,S5 secara Array
                                 // dan menetapkan nilai awal masing-masing adalah 0

void setup()
{
  pinMode(sensor1, INPUT); // S1 sbg input
  pinMode(sensor2, INPUT); // S2 sbg input
  pinMode(sensor3, INPUT); // S3 sbg input
  pinMode(sensor4, INPUT); // S4 sbg input
  pinMode(sensor5, INPUT); // S5 sbg input

  Serial.begin(9600);//setting serial monitor at a default baund rate of 9600
  delay(500);
 }
void loop(){ // ketika robot dinyalakan eksekusi perintah berikut
  read_sensor_values();
  }

void read_sensor_values()
{
  sensor[0] = !digitalRead(sensor1);
  sensor[1] = !digitalRead(sensor2);
  sensor[2] = !digitalRead(sensor3);
  sensor[3] = !digitalRead(sensor4);
  sensor[4] = !digitalRead(sensor5);

    Serial.print(sensor[0]);
    Serial.print(sensor[1]);
    Serial.print(sensor[2]);
    Serial.print(sensor[3]);
    Serial.println(sensor[4]);
    delay(100);    
}

2. SKETCH UJI MAJU, MUNDUR, BELOK, DAN STOP

SKETCH YANG PERLU DIKETIK:

// Penetapan Nama Variabel motor dan pin yang ditentukan
int ENA = 10; // pengatur kecepatan motor kiri di pin 10 pwm
int IN1 = 9; //IN1 motor kiri 
int IN2 = 8; //IN2 motor kiri 
int IN3 = 5; //IN3 motor kanan
int IN4 = 4; //IN4 motor kanan
int ENB = 3; // pengatur kecepatan motor kanan di pin 3 pwm

void setup()
{
  pinMode(IN1, OUTPUT); // in1 kiri 
  pinMode(IN2, OUTPUT); // in2 kiri
  pinMode(IN3, OUTPUT); // in3 kanan
  pinMode(IN4, OUTPUT); // in4 kanan
  pinMode(ENA, OUTPUT); // nilai kecepatan motor kiri
  pinMode(ENB, OUTPUT); // nilai kecepatan motor kanan
 }

void loop(){ 
  
    forward(); //MAJU selama 3 detik
    delay(3000);
    reverse();//MUNDUR selama 3 detik
    delay(3000);
    right(); //BELOKKANAN selama 2 detik
    delay(2000);
    left(); //BELOKKIRI selama 2 detik
    delay(2000);
    stop_bot(); //STOP selama 2 detik
    delay(2000);
}
  
void forward(){
  analogWrite(ENA, 250); //Left Motor Speed
  analogWrite(ENB, 250); //Right Motor Speed
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void reverse(){
  analogWrite(ENA, 250); //Left Motor Speed
  analogWrite(ENB, 250); //Right Motor Speed
  digitalWrite(IN1, LOW); 
  digitalWrite(IN2, HIGH); 
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void right(){
  analogWrite(ENA, 250); //Left Motor Speed
  analogWrite(ENB, 250); //Right Motor Speed
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}
void left(){
  analogWrite(ENA, 250); //Left Motor Speed
  analogWrite(ENB, 250); //Right Motor Speed
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void sharpRightTurn(){
  analogWrite(ENA, 250); //Left Motor Speed
  analogWrite(ENB, 250); //Right Motor Speed
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void sharpLeftTurn() {
  analogWrite(ENA, 250); //Left Motor Speed
  analogWrite(ENB, 250); //Right Motor Speed
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
  
}
void stop_bot(){
  analogWrite(ENA, 0); //Left Motor Speed
  analogWrite(ENB, 0); //Right Motor Speed
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

3. SKETCH UJI LURUS ROBOT (L298N)

SKETCH YANG PERLU DIKETIK:

// Penetapan Nama Variabel motor dan pin yang ditentukan
int ENA = 10; // pengatur kecepatan motor kiri di pin 5 pwm
int IN1 = 9; //IN1 motor kiri 
int IN2 = 8; //IN2 motor kiri 
int IN3 = 5; //IN3 motor kanan
int IN4 = 4; //IN4 motor kanan
int ENB = 3; // pengatur kecepatan motor kanan di pin 3 pwm

//inisialisasi penetapan power kecepatan motor
int Speed_Utama = 130; // Speed_Utama bernilai 0 hingga 250
int Speed_Kiri = Speed_Utama; //penentuan kecepatan motor kiri
int Speed_Kanan = Speed_Utama+5; //penentuan kecepatan motor kanan

int left_motor_speed; // membuat variabel kecepatan motor kiri
int right_motor_speed; // membuat variabel kecepatan motor kanan

void setup()
{
  pinMode(IN1, OUTPUT); // in1 kiri 
  pinMode(IN2, OUTPUT); // in2 kiri
  pinMode(IN3, OUTPUT); // in3 kanan
  pinMode(IN4, OUTPUT); // in4 kanan
  pinMode(ENA, OUTPUT); // nilai kecepatan motor kiri
  pinMode(ENB, OUTPUT); // nilai kecepatan motor kanan
 }

void loop(){ 
  
    motor_control(); //kendali kecepatan
    forward(); //MAJU selama 3 detik
    delay(3000);
 
    stop_bot(); //STOP selama 3 detik
    delay(3000);
}

 void motor_control()
{
  int left_motor_speed = Speed_Kiri;
  int right_motor_speed = Speed_Kanan;

  analogWrite(ENA, left_motor_speed);
  analogWrite(ENB, right_motor_speed);
}

void forward(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void reverse(){
  digitalWrite(IN1, LOW); 
  digitalWrite(IN2, HIGH); 
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void right(){
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}
void left(){
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void sharpRightTurn(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void sharpLeftTurn() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
  
}
void stop_bot(){
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

E. ROBOT LINE FOLLOWER TYPE BIASA

       Robot Line Follower type biasa menggunakan logika untuk menentukan arah robot, gerakannya lambat dan terbata-bata namun dapat kita jadikan indikator keberhasilan gerak robot menyusuri garis sebelum diprogram dengan PID Controller (Proportional, Integral, Derivative).

1.       Analisis Gerak Line Follower jika nilai 1 = KENA GARIS

No.

SENSOR TCRT5000

ARAH ROBOT

S1

S2

S3

S4

S5

1

0

0

1

0

0

LURUS ↑

2

0

1

1

0

0

LURUS ↑

3

0

0

1

1

0

LURUS ↑

4

0

1

1

1

0

LURUS ↑

5

1

1

1

1

1

STOP

6

0

0

0

0

1

KE KIRI←

7

0

0

0

1

0

KE KIRI←

8

0

0

0

1

1

KE KIRI←

9

0

0

1

1

1

KE KIRI 90 DERAJAT←

10

1

0

0

0

0

KE KANAN→

11

 

1

0

0

0

KE KANAN→

12

1

1

0

0

0

KE KANAN→

13

1

1

1

0

0

KE KANAN 90 DERAJAT→

14

0

0

0

0

0

BELOK KANAN 180 DERAJAT→↓

2.       Analisis Gerak Line Follower jika nilai 0 = KENA GARIS

No.

SENSOR TCRT5000

ARAH ROBOT

S1

S2

S3

S4

S5

1

1

1

0

1

1

LURUS ↑

2

1

0

0

1

1

LURUS ↑

3

1

1

0

0

1

LURUS ↑

4

1

0

0

0

1

LURUS ↑

5

0

0

0

0

0

STOP

6

1

1

1

1

0

KE KIRI←

7

1

1

1

0

1

KE KIRI←

8

1

1

1

0

0

KE KIRI←

9

1

1

0

0

0

KE KIRI 90 DERAJAT←

10

0

1

1

1

1

KE KANAN→

11

1

0

1

1

1

KE KANAN→

12

0

0

1

1

1

KE KANAN→

13

0

0

0

1

1

KE KANAN 90 DERAJAT→

14

1

1

1

1

1

BELOK KANAN 180 DERAJAT→↓


F.  ROBOT LINE FOLLOWER KENDALI BIASA (Status: Berhasil)

          Sebelum menggunakan sketch robot line follower biasa dan PID maka sebaiknya robot di uji lurus terlebih dahulu.

SKETCH ROBOT LINE FOLLOWER BIASA

// line follower biasa 5 sensor
// sketch khusus prototype R-Madina-01
//12 maret 2021
//SMK TERPADU MADINA
// sensor line follower type TCRT5000 5channel
// arduino uno dan protoshield
// modul L298N

#define enA 10
#define in1 9
#define in2 8
#define in3 5
#define in4 4
#define enB 3

// sensor TCRT5000 5channel, VCC ke 5V, GND ke GND
#define left A0 // S1 ke A0
#define mid1 A1 // S2 ke A1
#define mid2 A2 // S3 ke A2
#define mid3 A3 // S4 ke A3
#define right A4 // S5 ke A4

int speed_maju=120;
int a;
int b;
int c;
int d;
int e;


void setup() {
  
  pinMode(enA, OUTPUT); 
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT); 
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT); 
  pinMode(in4, OUTPUT);
  
  pinMode(left, INPUT); 
  pinMode(mid1, INPUT);
  pinMode(mid2, INPUT);
  pinMode(mid3, INPUT); 
  pinMode (right, INPUT);
  Serial.begin(9600);
    
}

void loop() {
  int a=digitalRead(left);
  int b=digitalRead(mid1);
  int c=digitalRead(mid2);
  int d=digitalRead(mid3);
  int e=digitalRead(right);
  
  Serial.print(a);
  Serial.print(" : ");
  Serial.print(b);
  Serial.print(" : ");
  Serial.print(c);
  Serial.print(" : ");
  Serial.print(d);
  Serial.print(" : ");
  Serial.println(e);
     
  // gerakan robot    
  if((a==1)&&(b==1)&&(c==0)&&(d==1)&&(e==1)){
    gerak_maju();
  } //11011 gerak maju
  
   else if((a==1)&&(b==1)&&(c==1)&&(d==1)&&(e==1)){
   gerak_maju();
  } // 11111 gerak maju
  
   else if((a==1)&&(b==0)&&(c==0)&&(d==1)&&(e==1)){
    gerak_maju();
  } // 10011 gerak maju
  
  else if((a==1)&&(b==1)&&(c==0)&&(d==0)&&(e==1)){
    gerak_maju();
  } // 11001 gerak maju
  
  else if((a==1)&&(b==1)&&(c==1)&&(d==1)&&(e==0)){
    belok_kiri();
    delay(50);
    motor_stop();
    delay(250);
    belok_kiri();
  } //11110 belok kiri
  
  else if((a==1)&&(b==1)&&(c==1)&&(d==0)&&(e==1)){
    belok_kiri();
    delay(50);
    motor_stop();
    delay(250);
    belok_kiri();
  } // 11101 belok kiri
  
  else if((a==1)&&(b==1)&&(c==1)&&(d==0)&&(e==0)){
    belok_kiri();
    delay(50);
    motor_stop();
    delay(250);
    belok_kiri();
  } // 11100 belok kiri
  
  else if((a==1)&&(b==1)&&(c==0)&&(d==0)&&(e==0)){
    belok_kiri();
    delay(50);
    motor_stop();
    delay(250);
    belok_kiri();
  } // 11000 belok kiri 90 derajat

   else if((a==0)&&(b==1)&&(c==1)&&(d==1)&&(e==1)){
    belok_kanan();
    delay(50);
    motor_stop();
    delay(250);
    belok_kanan();   
  } // 01111 belok kanan
  
    else if((a==0)&&(b==0)&&(c==1)&&(d==1)&&(e==1)){
    belok_kanan();
    delay(50);
    motor_stop();
    delay(250);
    belok_kanan();   
  } // 00111 belok kanan
  
   else if((a==1)&&(b==0)&&(c==1)&&(d==1)&&(e==1)){
    belok_kanan();
    delay(50);
    motor_stop();
    delay(250);
    belok_kanan();   
  } // 10111 belok kanan
  
   else if((a==0)&&(b==0)&&(c==0)&&(d==1)&&(e==1)){
    belok_kanan();
    delay(50);
    motor_stop();
    delay(250);
    belok_kanan();   
  }  // 00011 belok kanan
   else if((a==0)&&(b==0)&&(c==0)&&(d==0)&&(e==0)){
    motor_stop();
  } //00000 stop
}

void gerak_maju(){
  analogWrite (enA, speed_maju);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite (enB, speed_maju);
  } // fungsi robot berjalan maju
  
void belok_kanan(){
  analogWrite (enA, 200);
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW); 
  analogWrite (enB, 200);
} // fungsi robot berbelok kanan

void belok_kiri(){
  analogWrite (enA, 200);
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite (enB, 200);
 } // fungsi robot berbelok kiri
 
void motor_stop(){
  analogWrite (enA, 0);
  analogWrite (enB, 0);
} // fungsi robot berhenti


 

G. ROBOT LINE FOLLOWER DENGAN KENDALI PID (PROPORTIONAL, INTEGRAL,DERIVATIVE) (status:berhasil)

Analisis Gerak Line Follower PID jika nilai 1 = KENA GARIS

 

No.

SENSOR TCRT5000

ARAH ROBOT

S1

S2

S3

S4

S5

1

0

0

0

0

0

error = 5

2

1

0

0

0

0

error = 4

3

1

1

0

0

0

error = 3

4

0

1

0

0

0

error = 2

5

0

1

1

0

0

error = 1

6

0

0

1

0

0

error = 0

7

0

0

1

1

0

error = -1

8

0

0

0

1

0

error = -2

9

0

0

0

1

1

error = -3

10

0

0

0

0

1

error = -4

11

0

0

0

0

0

error = -5

12

1

1

1

0

0

error = 100

13

0

0

1

1

1

error = 101

14

1

1

1

1

1

error = 103

SKETCH ROBOT LINE FOLLOWER PENGENDALI PID

// Inisialisasi Sensor TCRT5000 5 channel
int sensor1 = A0; // Sensor paling kiri S1 ke A0
int sensor2 = A1; // S2 ke A1
int sensor3 = A2; // S3 ke A2
int sensor4 = A3; // S4 ke A3
int sensor5 = A4; // sensor paling kanan S5 ke A4

// Inisialisasi nilai sensor
int sensor[5] = {0, 0, 0, 0, 0}; // susunan nilai sensor dari S1,S2,S3,S4,S5 secara Array
                                 // dan menetapkan nilai awal masing-masing adalah 0
// Penetapan Nama Variabel motor dan pin yang ditentukan
int ENA = 10; // pengatur kecepatan motor kiri di pin 10 pwm
int IN1 = 9; //IN1 motor kiri 
int IN2 = 8; //IN2 motor kiri 
int IN3 = 5; //IN3 motor kanan
int IN4 = 4; //IN4 motor kanan
int ENB = 3; // pengatur kecepatan motor kanan di pin 3 pwm

//inisialisasi penetapan power kecepatan motor
int Speed_Utama = 130; // Speed_Utama bernilai 0 hingga 250
int Speed_Kiri = Speed_Utama; //penentuan kecepatan motor kiri
int Speed_Kanan = Speed_Utama+5; //penentuan kecepatan motor kanan

int left_motor_speed; // membuat variabel kecepatan motor kiri
int right_motor_speed; // membuat variabel kecepatan motor kanan

// menambahkan led penanda (optional)
int ledPin1 = 11; // led1 di pin 11
int ledPin2 = 12; // led2 di pin 12

// konstanta proporsional, integral, dan derivatif. disini nilai disesuaikan

float Kp = 39;
float Ki = 13 ;
float Kd = 30 ;

float error = 0, P = 0, I = 0, D = 0, PID_value = 0; // penetapan nilai awal = 0
float previous_error = 0, previous_I = 0;
int flag = 0;

void setup()
{
  pinMode(sensor1, INPUT); // S1 sbg input
  pinMode(sensor2, INPUT); // S2 sbg input
  pinMode(sensor3, INPUT); // S3 sbg input
  pinMode(sensor4, INPUT); // S4 sbg input
  pinMode(sensor5, INPUT); // S5 sbg input

  pinMode(IN1, OUTPUT); // in1 kiri 
  pinMode(IN2, OUTPUT); // in2 kiri
  pinMode(IN3, OUTPUT); // in3 kanan
  pinMode(IN4, OUTPUT); // in4 kanan
  pinMode(ENA, OUTPUT); // nilai kecepatan motor kiri
  pinMode(ENB, OUTPUT); // nilai kecepatan motor kanan

  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);

  digitalWrite(ledPin1, LOW);
  digitalWrite(ledPin2, LOW);

  Serial.begin(9600);//setting serial monitor at a default baund rate of 9600
  delay(500);
 }
void loop(){ // ketika robot dinyalakan eksekusi perintah berikut
  read_sensor_values();
  calculate_pid();
  
  if (error == 100) {  
    do {
      read_sensor_values();
      analogWrite(ENA, left_motor_speed); //Left Motor Speed
      analogWrite(ENB, right_motor_speed); //Right Motor Speed 
      sharpLeftTurn();
    } while (error != 0);

  } else if (error == 101) {          
    analogWrite(ENA, left_motor_speed); //Left Motor Speed
    analogWrite(ENB, right_motor_speed); //Right Motor Speed
    forward();
    delay(200);
    stop_bot();
    
  }else if (error == 103) {
    if (flag == 0) {
      analogWrite(ENA, left_motor_speed); //Left Motor Speed
      analogWrite(ENB, right_motor_speed); //Right Motor Speed
      forward();
      delay(200);
      stop_bot();
      read_sensor_values();
      if (error == 103) {     
        stop_bot();
        digitalWrite(ledPin1, HIGH);
        digitalWrite(ledPin2, HIGH);
        flag = 1;
      } else {        
        analogWrite(ENA, left_motor_speed); //Left Motor Speed
        analogWrite(ENB, right_motor_speed); //Right Motor Speed
        sharpLeftTurn();
        delay(200);
        do {
          read_sensor_values();
          analogWrite(ENA, left_motor_speed); //Left Motor Speed
          analogWrite(ENB, right_motor_speed); //Right Motor Speed
          sharpLeftTurn();
        } while (error != 0);
      }
    }
  } else {
    calculate_pid();
    motor_control();
    forward();
  }
}

void read_sensor_values()
{
  sensor[0] = !digitalRead(sensor1);
  sensor[1] = !digitalRead(sensor2);
  sensor[2] = !digitalRead(sensor3);
  sensor[3] = !digitalRead(sensor4);
  sensor[4] = !digitalRead(sensor5);

    
  //menentukan nilai definisi error dari hasil sensor
  if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0))
    error = 4; // 01111 = 10000
    else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0))
    error = 3; // 00111 = 11000
    else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0))
    error = 2; // 10111 = 01000
    else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0)&& (sensor[4] == 0))
    error = 1; // 10011 = 01100
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0)&& (sensor[4] == 0))
    error = 0; // 11011 = 00100
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1)&& (sensor[4] == 0))
    error = -1; // 11001 = 00110
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1)&& (sensor[4] == 0))
    error = -2; // 11101 = 00010
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1)&& (sensor[4] == 1))
    error = -3; // 11100 = 00011
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 1))
    error = -4; // 11110 = 00001
    
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0)&& (sensor[4] == 0))
    if(previous_error==(-4)) {error = -5;} // jika semua sensor mengenai garis putih, jika error sebelumnya -4 maka error = -5
    if(previous_error==(4)) {error = 5;} // jika semua sensor mengenai garis putih, jika error sebelumnya 4 maka error = 5
    
    // kondisi khas tertentu        
    else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0)&& (sensor[4] == 0)) // Turn robot left side
    error = 100; //1110 -00011 = 11100
    else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1)&& (sensor[4] == 1)) // Turn robot right side
    error = 101; //0111 -11000 = 00111
    else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1)&& (sensor[4] == 1)) // Turn left side or stop
    error = 103; //1111 -00000 = 11111
}

void calculate_pid(){ //perhitungan nilai PID
  previous_I = I;
  previous_error = error;
  P = error;
  I = I + previous_I; 
  D = error - previous_error;

  PID_value = (Kp * P) + (Ki * I) + (Kd * D);
  }

void motor_control()
{
  // Calculating the effective motor speed:
  int left_motor_speed = Speed_Utama - PID_value;
  int right_motor_speed = Speed_Utama + PID_value;

  // The motor speed should not exceed the max PWM value
  left_motor_speed = constrain(left_motor_speed, 0, Speed_Kiri);
  right_motor_speed = constrain(right_motor_speed, 0, Speed_Kanan);
  
  analogWrite(ENA, left_motor_speed); //kecepatan motor kiri yang sudah dipengaruhi PID 
  analogWrite(ENB, right_motor_speed); //Kecepatan motor kanan yang sudah dipengaruhi PID

  // Menampilkan nilai-nilai di serial monitor
  Serial.print("ERROR: ");
  Serial.print(error);
  Serial.print("PID: ");
  Serial.print(PID_value);
  Serial.print("KEC KIRI: ");
  Serial.print(left_motor_speed);
  Serial.print("KEC KANAN: ");
  Serial.println(right_motor_speed);
}

void forward(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void reverse(){
  digitalWrite(IN1, LOW); 
  digitalWrite(IN2, HIGH); 
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void right(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}
void left(){
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}
void sharpRightTurn(){
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}
void sharpLeftTurn() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW); 
  
}
void stop_bot(){
  analogWrite(ENA, 0); //Left Motor Speed
  analogWrite(ENB, 0); //Right Motor Speed
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

H. TABEL PENGUJIAN PID CONTROLLER (PROPORTIONAL, INTEGRAL, DERIVATIVE) DAN KECEPATAN UMUM

NO

KP

KI

KD

SPEED

KETERANGAN

1

39

13

30

130

Max speed 250

2

60

1

160

150

STABIL

3

80

1

320

250

Non stabil

4

 

 

 

 

 

5

 

 

 

 

 

6

 

 

 

 

 

7

 

 

 

 

 

8

 

 

 

 

 

9

 

 

 

 

 

10

 

 

 

 

 

11

 

 

 

 

 

12

 

 

 

 

 

13

 

 

 

 

 

14

 

 

 

 

 

15

 

 

 

 

 

16

 

 

 

 

 

17

 

 

 

 

 

18

 

 

 

 

 

19

 

 

 

 

 

20

 

 

 

 

 

By -
Langganan: Komentar (Atom)