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);
}
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);
}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);
}
|
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
|
|
|
|
|
|
