ROBOTIKA 3: Robot Line Follower dengan 8 Sensor
- Dapatkan link
- X
- Aplikasi Lainnya
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
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); }