Line Follower Using Servo, RFID and Ultrasonic Sensor

Build a line follower robot using Arduino Uno with a continuous servo as a motor, and RFID to tag point and stop LF and ping sensor.

Jan 29, 2017

•

9720 views

•

2 respects

rfid

ultrasonic

servo

Components and supplies

RGB Backlight LCD - 16x2

Ultrasonic Sensor - HC-SR04 (Generic)

Arduino UNO

RobotGeek Continuous Rotation Servo

Project description

Code

SENSOR

arduino

IR Sensor i use modeul 4 sensor IR

1void motor_setup() {
2  servo_kiri.attach(pin1);
3  servo_kanan.attach(pin2);
4}
5
6void  buzzer(){
7  tone(buzzerPin, 3000);
8  delay(500);  
9  noTone(buzzerPin);
10}
11
12void  motor_maju(int speed) {
13  if(speed==1) {
14     speed = velocity_high;
15  }  else if(speed==2) {
16     speed = velocity_med;
17  } else {
18     speed =  velocity_slow;
19  }
20  
21  servo_kiri.write(stop_position - speed -5);
22  servo_kanan.write(stop_position + speed -10);
23}
24
25void motor_mundur(int  speed) {
26  if(speed==1) {
27     speed = velocity_high;
28  } else if(speed==2)  {
29     speed = velocity_med;
30  } else {
31     speed = velocity_slow;
32  }
33  
34  servo_kiri.write(stop_position + speed -5);
35  servo_kanan.write(stop_position  - speed -10);  
36}
37
38void belok_kanan(int speed) {
39  if(speed==1) {
40     speed = velocity_high;
41  } else if(speed==2) {
42     speed = velocity_med;
43  } else {
44     speed = velocity_slow;
45  }
46  
47  servo_kiri.write(stop_position  - speed -5);
48  servo_kanan.write(stop_position - speed -10);
49}
50
51void  belok_kanan_patah(int speed) {
52  if(speed==1) {
53     speed = velocity_high;
54  } else if(speed==2) {
55     speed = velocity_med;
56  } else {
57     speed  = velocity_slow;
58  }
59  servo_kiri.write(stop_position - speed -5);
60  servo_kanan.write(stop_position  -10);
61}
62
63void belok_kiri(int speed) {
64  if(speed==1) {
65     speed  = velocity_high;
66  } else if(speed==2) {
67     speed = velocity_med;
68  }  else {
69     speed = velocity_slow;
70  }
71  
72  servo_kiri.write(stop_position  + speed -5);
73  servo_kanan.write(stop_position + speed -10);  
74}
75
76void  belok_kiri_patah(int speed) {
77  if(speed==1) {
78     speed = velocity_high;
79  } else if(speed==2) {
80     speed = velocity_med;
81  } else {
82     speed  = velocity_slow;
83  }
84  
85  servo_kiri.write(stop_position -5);
86  servo_kanan.write(stop_position  + speed -10);  
87}
88
89void motor_stop() {
90  servo_kiri.write(stop_position  - 5);
91  servo_kanan.write(stop_position - 10);
92}
93
94void motor_stop2()  {
95  servo_kiri.write(stop_position - 5);
96  servo_kanan.write(stop_position  - 5);
97}
98
99void readSensor() {
100  xsensor = sensor;
101  sensor = 0;
102  trackWarnaPutih = trackMode;
103  batas = batasEE;
104
105  data1 = analogRead(sensorPin1);
106  data2 = analogRead(sensorPin2);
107  data3 = analogRead(sensorPin3);
108  data4  = analogRead(sensorPin4); 
109  
110  if (data1>batas) sensor |= 0x01;
111  if (data2>batas)  sensor |= 0x02;
112  if (data3>batas) sensor |= 0x04;
113  if (data4>batas) sensor  |= 0x08;
114
115  lcd.setCursor(0,1);
116  
117   if (trackWarnaPutih) {
118     sensor  = sensor ^ 0x0F;      
119   } 
120   if(sensor == 0x01) {    
121    // hasil pembacaan  sensor "0001" -> track berada di ujung kanan sensor      
122      belok_kanan(1);
123      Serial.print("0001");
124      lcd.print("Pengiriman Surat");
125   }
126   else if (sensor == 0x03) {    
127    // hasil pembacaan sensor "0011" -> track  berada di ujung kanan sensor      
128      belok_kanan(1);
129      Serial.print("0011");
130      lcd.print("Pengiriman Surat");
131   }
132   else if (sensor == 0x06) {
133      // hasil pembacaan sensor "0110" -> track tepat berada di tengah   
134      motor_maju(3);
135      Serial.print("0110");
136      lcd.print("Pengiriman  Surat");
137   }
138   else if (sensor == 0x07) {
139      // hasil pembacaan sensor  "0111" -> track tepat berada di tengah   
140      belok_kanan(1);
141      Serial.print("0111");
142      lcd.print("Pembacaan Sensor");
143   }
144   else if (sensor == 0x08) {
145      // hasil pembacaan sensor "1000" -> track berada di ujung kiri sensor
146      belok_kiri(1);
147      Serial.print("1000");
148      lcd.print("Pengiriman  Surat");
149   }
150   else if (sensor == 0x0C) {
151      // hasil pembacaan sensor  "1100" -> track berada di ujung kiri sensor
152      belok_kiri(1);
153      Serial.print("1100");
154      lcd.print("Pengiriman Surat");
155   }
156   else if (sensor == 0x0E) {
157      // hasil pembacaan sensor "1110" -> track berada di ujung kiri sensor
158      belok_kiri(1);
159      Serial.print("1110");
160      lcd.print("Pengiriman  Surat");
161   }
162   else if ((sensor == 4001) || (sensor == 2001) || (sensor  == 2100)|| (sensor == 5110)) {
163      belok_kanan(1);
164   }else{
165      motor_maju(3);
166   }
167}
168

void motor_setup() {
  servo_kiri.attach(pin1);
  servo_kanan.attach(pin2);
}

void  buzzer(){
  tone(buzzerPin, 3000);
  delay(500);  
  noTone(buzzerPin);
}

void  motor_maju(int speed) {
  if(speed==1) {
     speed = velocity_high;
  }  else if(speed==2) {
     speed = velocity_med;
  } else {
     speed =  velocity_slow;
  }
  
  servo_kiri.write(stop_position - speed -5);
  servo_kanan.write(stop_position + speed -10);
}

void motor_mundur(int  speed) {
  if(speed==1) {
     speed = velocity_high;
  } else if(speed==2)  {
     speed = velocity_med;
  } else {
     speed = velocity_slow;
  }
  
  servo_kiri.write(stop_position + speed -5);
  servo_kanan.write(stop_position  - speed -10);  
}

void belok_kanan(int speed) {
  if(speed==1) {
     speed = velocity_high;
  } else if(speed==2) {
     speed = velocity_med;
  } else {
     speed = velocity_slow;
  }
  
  servo_kiri.write(stop_position  - speed -5);
  servo_kanan.write(stop_position - speed -10);
}

void  belok_kanan_patah(int speed) {
  if(speed==1) {
     speed = velocity_high;
  } else if(speed==2) {
     speed = velocity_med;
  } else {
     speed  = velocity_slow;
  }
  servo_kiri.write(stop_position - speed -5);
  servo_kanan.write(stop_position  -10);
}

void belok_kiri(int speed) {
  if(speed==1) {
     speed  = velocity_high;
  } else if(speed==2) {
     speed = velocity_med;
  }  else {
     speed = velocity_slow;
  }
  
  servo_kiri.write(stop_position  + speed -5);
  servo_kanan.write(stop_position + speed -10);  
}

void  belok_kiri_patah(int speed) {
  if(speed==1) {
     speed = velocity_high;
  } else if(speed==2) {
     speed = velocity_med;
  } else {
     speed  = velocity_slow;
  }
  
  servo_kiri.write(stop_position -5);
  servo_kanan.write(stop_position  + speed -10);  
}

void motor_stop() {
  servo_kiri.write(stop_position  - 5);
  servo_kanan.write(stop_position - 10);
}

void motor_stop2()  {
  servo_kiri.write(stop_position - 5);
  servo_kanan.write(stop_position  - 5);
}

void readSensor() {
  xsensor = sensor;
  sensor = 0;
  trackWarnaPutih = trackMode;
  batas = batasEE;

  data1 = analogRead(sensorPin1);
  data2 = analogRead(sensorPin2);
  data3 = analogRead(sensorPin3);
  data4  = analogRead(sensorPin4); 
  
  if (data1>batas) sensor |= 0x01;
  if (data2>batas)  sensor |= 0x02;
  if (data3>batas) sensor |= 0x04;
  if (data4>batas) sensor  |= 0x08;

  lcd.setCursor(0,1);
  
   if (trackWarnaPutih) {
     sensor  = sensor ^ 0x0F;      
   } 
   if(sensor == 0x01) {    
    // hasil pembacaan  sensor "0001" -> track berada di ujung kanan sensor      
      belok_kanan(1);
      Serial.print("0001");
      lcd.print("Pengiriman Surat");
   }
   else if (sensor == 0x03) {    
    // hasil pembacaan sensor "0011" -> track  berada di ujung kanan sensor      
      belok_kanan(1);
      Serial.print("0011");
      lcd.print("Pengiriman Surat");
   }
   else if (sensor == 0x06) {
      // hasil pembacaan sensor "0110" -> track tepat berada di tengah   
      motor_maju(3);
      Serial.print("0110");
      lcd.print("Pengiriman  Surat");
   }
   else if (sensor == 0x07) {
      // hasil pembacaan sensor  "0111" -> track tepat berada di tengah   
      belok_kanan(1);
      Serial.print("0111");
      lcd.print("Pembacaan Sensor");
   }
   else if (sensor == 0x08) {
      // hasil pembacaan sensor "1000" -> track berada di ujung kiri sensor
      belok_kiri(1);
      Serial.print("1000");
      lcd.print("Pengiriman  Surat");
   }
   else if (sensor == 0x0C) {
      // hasil pembacaan sensor  "1100" -> track berada di ujung kiri sensor
      belok_kiri(1);
      Serial.print("1100");
      lcd.print("Pengiriman Surat");
   }
   else if (sensor == 0x0E) {
      // hasil pembacaan sensor "1110" -> track berada di ujung kiri sensor
      belok_kiri(1);
      Serial.print("1110");
      lcd.print("Pengiriman  Surat");
   }
   else if ((sensor == 4001) || (sensor == 2001) || (sensor  == 2100)|| (sensor == 5110)) {
      belok_kanan(1);
   }else{
      motor_maju(3);
   }
}

ULTRASONIC

arduino

Ultrasonic / Ping Sensor

1#define trigPin 4
2#define echoPin 2
3
4void ping_setup() {
5  Serial.begin (9600);
6  pinMode(trigPin, OUTPUT);
7  pinMode(echoPin, INPUT);
8}
9
10void  ping_loop() {
11  int duration, distance;
12  digitalWrite(trigPin, HIGH);
13  delayMicroseconds(1000);
14  digitalWrite(trigPin, LOW);
15  duration = pulseIn(echoPin,  HIGH);
16  distance = (duration/2) / 29.1;
17
18  if (distance > 5 || distance  <= 0){
19    noTone(buzzerPin);
20  }
21  else {
22    Serial.println("object  detected");
23    motor_stop();
24    tone(buzzerPin, 5000); // play 400 Hz tone  for 500 ms
25    delay(10);
26  }
27}
28

#define trigPin 4
#define echoPin 2

void ping_setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void  ping_loop() {
  int duration, distance;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin,  HIGH);
  distance = (duration/2) / 29.1;

  if (distance > 5 || distance  <= 0){
    noTone(buzzerPin);
  }
  else {
    Serial.println("object  detected");
    motor_stop();
    tone(buzzerPin, 5000); // play 400 Hz tone  for 500 ms
    delay(10);
  }
}

LCD

arduino

LCD CODE

1String menuItems[] = {" Start", " (+)Jurusan", " (-)Jurusan"};
2String  menuJurusan[] = {"MESIN", "ELEKTRO", "INFORMATIKA", "INDUSTRI", "SIPIL",  "ARSITEK", "BATAL"};
3int ledPin = 13; // choose the pin for the LED
4int  inPin1 = 7;   // choose the input pin (for a pushbutton)
5int inPin2 = 6;   //  choose the input pin (for a pushbutton)
6int val = 0;     // variable for reading  the pin status
7
8void lcd_setup() {
9  lcd.begin(16,2);
10  lcd.clear();
11  pinMode(inPin1, INPUT);
12  pinMode(inPin2, INPUT);
13}
14
15void drawLCD()  {
16  if(digitalRead(inPin1)==HIGH) {
17    digitalWrite(inPin1, LOW);
18    lcd.clear();
19    Serial.println("HOME");
20    lcd.print("HOME");
21    motor_stop();
22    is_home = 1;
23    motor_stop();
24  }else if(digitalRead(inPin2)==HIGH) {
25    digitalWrite(inPin1, LOW);
26    lcd.clear();
27    Serial.println("INTERUPT  TO START");
28    lcd.print("INTERUPT TO START");
29    interupt_status ==  1;
30    is_home = 0;
31    motor_stop();
32  }
33}
34

String menuItems[] = {" Start", " (+)Jurusan", " (-)Jurusan"};
String  menuJurusan[] = {"MESIN", "ELEKTRO", "INFORMATIKA", "INDUSTRI", "SIPIL",  "ARSITEK", "BATAL"};
int ledPin = 13; // choose the pin for the LED
int  inPin1 = 7;   // choose the input pin (for a pushbutton)
int inPin2 = 6;   //  choose the input pin (for a pushbutton)
int val = 0;     // variable for reading  the pin status

void lcd_setup() {
  lcd.begin(16,2);
  lcd.clear();
  pinMode(inPin1, INPUT);
  pinMode(inPin2, INPUT);
}

void drawLCD()  {
  if(digitalRead(inPin1)==HIGH) {
    digitalWrite(inPin1, LOW);
    lcd.clear();
    Serial.println("HOME");
    lcd.print("HOME");
    motor_stop();
    is_home = 1;
    motor_stop();
  }else if(digitalRead(inPin2)==HIGH) {
    digitalWrite(inPin1, LOW);
    lcd.clear();
    Serial.println("INTERUPT  TO START");
    lcd.print("INTERUPT TO START");
    interupt_status ==  1;
    is_home = 0;
    motor_stop();
  }
}

RFID

arduino

RFID CODE, use MFRC522 LIBRARY

1MFRC522 mfrc522(SS_PIN, RST_PIN);   // Create MFRC522 instance.
2
3String  read_rfid;
4String rfid_card[] = {"fa8d56f9", "9a775af9", "ca8a55f9", "424a7cd2",  "ea748af9", "4a8229f9"};
5String sekjur[] = {"MESIN", "ELEKTRO", "INFORMATIKA",  "INDUSTRI", "SIPIL", "ARSITEK"};
6
7/*
8   Initialize.
9*/
10void  rfid_setup() {
11  SPI.begin();                // Init SPI bus
12  mfrc522.PCD_Init();         // Init MFRC522 card
13}
14
15/*
16   Helper routine to dump a byte  array as hex values to Serial.
17*/
18void dump_byte_array(byte *buffer, byte  bufferSize) {
19  read_rfid = "";
20  for (byte i = 0; i < bufferSize; i++)  {
21    read_rfid = read_rfid + String(buffer[i], HEX);
22  }
23}
24
25void  rfid_scan() {
26
27  // Look for new cards
28  if ( ! mfrc522.PICC_IsNewCardPresent()){
29    readSensor();
30  }
31
32  // Select one of the cards
33  if ( ! mfrc522.PICC_ReadCardSerial()){
34    readSensor();
35  }
36  
37  dump_byte_array(mfrc522.uid.uidByte, mfrc522.uid.size);
38  Serial.println(read_rfid);
39  for (int x = 0; x < 6; x++) {
40    if(read_rfid  == "8a4098f9"){
41        lcd.clear();
42        lcd.print("HOME");    
43        is_home = 1;
44        motor_stop2();
45        buzzer();
46    }else{      
47      if (read_rfid == rfid_card[x]) {
48        buzzer();
49        lcd.clear();
50        lcd.setCursor(0, 0);
51        lcd.print("Surat Terkirim");
52        lcd.setCursor(0,  1);
53        lcd.print("SEKJUR ");
54        lcd.print(sekjur[x]);
55        interupt_status  = 1;
56      } 
57    }
58
59  }
60}
61
62

MFRC522 mfrc522(SS_PIN, RST_PIN);   // Create MFRC522 instance.

String  read_rfid;
String rfid_card[] = {"fa8d56f9", "9a775af9", "ca8a55f9", "424a7cd2",  "ea748af9", "4a8229f9"};
String sekjur[] = {"MESIN", "ELEKTRO", "INFORMATIKA",  "INDUSTRI", "SIPIL", "ARSITEK"};

/*
   Initialize.
*/
void  rfid_setup() {
  SPI.begin();                // Init SPI bus
  mfrc522.PCD_Init();         // Init MFRC522 card
}

/*
   Helper routine to dump a byte  array as hex values to Serial.
*/
void dump_byte_array(byte *buffer, byte  bufferSize) {
  read_rfid = "";
  for (byte i = 0; i < bufferSize; i++)  {
    read_rfid = read_rfid + String(buffer[i], HEX);
  }
}

void  rfid_scan() {

  // Look for new cards
  if ( ! mfrc522.PICC_IsNewCardPresent()){
    readSensor();
  }

  // Select one of the cards
  if ( ! mfrc522.PICC_ReadCardSerial()){
    readSensor();
  }
  
  dump_byte_array(mfrc522.uid.uidByte, mfrc522.uid.size);
  Serial.println(read_rfid);
  for (int x = 0; x < 6; x++) {
    if(read_rfid  == "8a4098f9"){
        lcd.clear();
        lcd.print("HOME");    
        is_home = 1;
        motor_stop2();
        buzzer();
    }else{      
      if (read_rfid == rfid_card[x]) {
        buzzer();
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Surat Terkirim");
        lcd.setCursor(0,  1);
        lcd.print("SEKJUR ");
        lcd.print(sekjur[x]);
        interupt_status  = 1;
      } 
    }

  }
}

ROBOT_LF

arduino

main program of LF

1// LCD
2#include <LiquidCrystal_I2C.h>
3#include <Servo.h>
4#include  <SPI.h>
5#include <MFRC522.h>
6LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6,  7, 3, POSITIVE);
7
8#define RST_PIN         9           // Configurable, see  typical pin layout above
9#define SS_PIN          10          // Configurable,  see typical pin layout above
10
11int pin1 = 3;
12int pin2 = 5;
13const int  stop_position = 100; 
14const int velocity_high = 85;
15const int velocity_med  = 55;
16const int velocity_slow = 25;
17const int buzzerPin = 8;
18
19Servo  servo_kiri;
20Servo servo_kanan;
21
22// FUNCTION
23#define MAX 5
24int top  = -1;
25int status;
26int a = 10;
27bool full = false;
28
29// SENSOR
30const  int sensorPin1 = A0;
31const int sensorPin2 = A1;
32const int sensorPin3 = A2;
33const  int sensorPin4 = A3;
34int interupt_status = 0;
35int is_home = 0;
36unsigned  char data1,data2,data3,data4;
37unsigned char sensor,batas,xsensor,xstatus;
38unsigned  char trackWarnaPutih;
39unsigned char  trackMode = 0, batasEE = 100;
40#define   lurus         0
41#define   tikunganKiri  1
42#define   tikunganKanan 2
43
44void  setup() {
45  Serial.begin(9600);
46  lcd_setup();
47  ping_setup();  
48  rfid_setup();
49  motor_setup();
50}
51
52void loop() {
53 // motor_maju(2);
54  drawLCD();  
55  if(is_home==0){
56    ping_loop();
57    if(interupt_status == 0){
58        readSensor();
59        rfid_scan();
60    }else if(interupt_status ==  1){
61      motor_stop();
62      buzzer();
63      interupt_status = 0;
64    }else{
65      motor_stop();
66      buzzer();
67    }
68  }else{
69    motor_stop2;
70  }    
71}
72
73
74

// LCD
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include  <SPI.h>
#include <MFRC522.h>
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6,  7, 3, POSITIVE);

#define RST_PIN         9           // Configurable, see  typical pin layout above
#define SS_PIN          10          // Configurable,  see typical pin layout above

int pin1 = 3;
int pin2 = 5;
const int  stop_position = 100; 
const int velocity_high = 85;
const int velocity_med  = 55;
const int velocity_slow = 25;
const int buzzerPin = 8;

Servo  servo_kiri;
Servo servo_kanan;

// FUNCTION
#define MAX 5
int top  = -1;
int status;
int a = 10;
bool full = false;

// SENSOR
const  int sensorPin1 = A0;
const int sensorPin2 = A1;
const int sensorPin3 = A2;
const  int sensorPin4 = A3;
int interupt_status = 0;
int is_home = 0;
unsigned  char data1,data2,data3,data4;
unsigned char sensor,batas,xsensor,xstatus;
unsigned  char trackWarnaPutih;
unsigned char  trackMode = 0, batasEE = 100;
#define   lurus         0
#define   tikunganKiri  1
#define   tikunganKanan 2

void  setup() {
  Serial.begin(9600);
  lcd_setup();
  ping_setup();  
  rfid_setup();
  motor_setup();
}

void loop() {
 // motor_maju(2);
  drawLCD();  
  if(is_home==0){
    ping_loop();
    if(interupt_status == 0){
        readSensor();
        rfid_scan();
    }else if(interupt_status ==  1){
      motor_stop();
      buzzer();
      interupt_status = 0;
    }else{
      motor_stop();
      buzzer();
    }
  }else{
    motor_stop2;
  }    
}

RFID

arduino

RFID CODE, use MFRC522 LIBRARY

1MFRC522 mfrc522(SS_PIN, RST_PIN);   // Create MFRC522 instance.
2
3String   read_rfid;
4String rfid_card[] = {"fa8d56f9", "9a775af9", "ca8a55f9", "424a7cd2",   "ea748af9", "4a8229f9"};
5String sekjur[] = {"MESIN", "ELEKTRO", "INFORMATIKA",   "INDUSTRI", "SIPIL", "ARSITEK"};
6
7/*
8   Initialize.
9*/
10void   rfid_setup() {
11  SPI.begin();                // Init SPI bus
12  mfrc522.PCD_Init();          // Init MFRC522 card
13}
14
15/*
16   Helper routine to dump a byte   array as hex values to Serial.
17*/
18void dump_byte_array(byte *buffer, byte   bufferSize) {
19  read_rfid = "";
20  for (byte i = 0; i < bufferSize; i++)   {
21    read_rfid = read_rfid + String(buffer[i], HEX);
22  }
23}
24
25void   rfid_scan() {
26
27  // Look for new cards
28  if ( ! mfrc522.PICC_IsNewCardPresent()){
29     readSensor();
30  }
31
32  // Select one of the cards
33  if ( ! mfrc522.PICC_ReadCardSerial()){
34     readSensor();
35  }
36  
37  dump_byte_array(mfrc522.uid.uidByte, mfrc522.uid.size);
38   Serial.println(read_rfid);
39  for (int x = 0; x < 6; x++) {
40    if(read_rfid   == "8a4098f9"){
41        lcd.clear();
42        lcd.print("HOME");    
43         is_home = 1;
44        motor_stop2();
45        buzzer();
46    }else{       
47      if (read_rfid == rfid_card[x]) {
48        buzzer();
49        lcd.clear();
50         lcd.setCursor(0, 0);
51        lcd.print("Surat Terkirim");
52        lcd.setCursor(0,   1);
53        lcd.print("SEKJUR ");
54        lcd.print(sekjur[x]);
55        interupt_status   = 1;
56      } 
57    }
58
59  }
60}
61
62

MFRC522 mfrc522(SS_PIN, RST_PIN);   // Create MFRC522 instance.

String   read_rfid;
String rfid_card[] = {"fa8d56f9", "9a775af9", "ca8a55f9", "424a7cd2",   "ea748af9", "4a8229f9"};
String sekjur[] = {"MESIN", "ELEKTRO", "INFORMATIKA",   "INDUSTRI", "SIPIL", "ARSITEK"};

/*
   Initialize.
*/
void   rfid_setup() {
  SPI.begin();                // Init SPI bus
  mfrc522.PCD_Init();          // Init MFRC522 card
}

/*
   Helper routine to dump a byte   array as hex values to Serial.
*/
void dump_byte_array(byte *buffer, byte   bufferSize) {
  read_rfid = "";
  for (byte i = 0; i < bufferSize; i++)   {
    read_rfid = read_rfid + String(buffer[i], HEX);
  }
}

void   rfid_scan() {

  // Look for new cards
  if ( ! mfrc522.PICC_IsNewCardPresent()){
     readSensor();
  }

  // Select one of the cards
  if ( ! mfrc522.PICC_ReadCardSerial()){
     readSensor();
  }
  
  dump_byte_array(mfrc522.uid.uidByte, mfrc522.uid.size);
   Serial.println(read_rfid);
  for (int x = 0; x < 6; x++) {
    if(read_rfid   == "8a4098f9"){
        lcd.clear();
        lcd.print("HOME");    
         is_home = 1;
        motor_stop2();
        buzzer();
    }else{       
      if (read_rfid == rfid_card[x]) {
        buzzer();
        lcd.clear();
         lcd.setCursor(0, 0);
        lcd.print("Surat Terkirim");
        lcd.setCursor(0,   1);
        lcd.print("SEKJUR ");
        lcd.print(sekjur[x]);
        interupt_status   = 1;
      } 
    }

  }
}

ULTRASONIC

arduino

Ultrasonic / Ping Sensor

1#define trigPin 4
2#define echoPin 2
3
4void ping_setup() {
5   Serial.begin (9600);
6  pinMode(trigPin, OUTPUT);
7  pinMode(echoPin, INPUT);
8}
9
10void   ping_loop() {
11  int duration, distance;
12  digitalWrite(trigPin, HIGH);
13   delayMicroseconds(1000);
14  digitalWrite(trigPin, LOW);
15  duration = pulseIn(echoPin,   HIGH);
16  distance = (duration/2) / 29.1;
17
18  if (distance > 5 || distance   <= 0){
19    noTone(buzzerPin);
20  }
21  else {
22    Serial.println("object   detected");
23    motor_stop();
24    tone(buzzerPin, 5000); // play 400 Hz tone   for 500 ms
25    delay(10);
26  }
27}
28

#define trigPin 4
#define echoPin 2

void ping_setup() {
   Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void   ping_loop() {
  int duration, distance;
  digitalWrite(trigPin, HIGH);
   delayMicroseconds(1000);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin,   HIGH);
  distance = (duration/2) / 29.1;

  if (distance > 5 || distance   <= 0){
    noTone(buzzerPin);
  }
  else {
    Serial.println("object   detected");
    motor_stop();
    tone(buzzerPin, 5000); // play 400 Hz tone   for 500 ms
    delay(10);
  }
}

SENSOR

arduino

IR Sensor i use modeul 4 sensor IR

1void motor_setup() {
2  servo_kiri.attach(pin1);
3  servo_kanan.attach(pin2);
4}
5
6void   buzzer(){
7  tone(buzzerPin, 3000);
8  delay(500);  
9  noTone(buzzerPin);
10}
11
12void   motor_maju(int speed) {
13  if(speed==1) {
14     speed = velocity_high;
15  }   else if(speed==2) {
16     speed = velocity_med;
17  } else {
18     speed =   velocity_slow;
19  }
20  
21  servo_kiri.write(stop_position - speed -5);
22   servo_kanan.write(stop_position + speed -10);
23}
24
25void motor_mundur(int   speed) {
26  if(speed==1) {
27     speed = velocity_high;
28  } else if(speed==2)   {
29     speed = velocity_med;
30  } else {
31     speed = velocity_slow;
32   }
33  
34  servo_kiri.write(stop_position + speed -5);
35  servo_kanan.write(stop_position   - speed -10);  
36}
37
38void belok_kanan(int speed) {
39  if(speed==1) {
40      speed = velocity_high;
41  } else if(speed==2) {
42     speed = velocity_med;
43   } else {
44     speed = velocity_slow;
45  }
46  
47  servo_kiri.write(stop_position   - speed -5);
48  servo_kanan.write(stop_position - speed -10);
49}
50
51void   belok_kanan_patah(int speed) {
52  if(speed==1) {
53     speed = velocity_high;
54   } else if(speed==2) {
55     speed = velocity_med;
56  } else {
57     speed   = velocity_slow;
58  }
59  servo_kiri.write(stop_position - speed -5);
60  servo_kanan.write(stop_position   -10);
61}
62
63void belok_kiri(int speed) {
64  if(speed==1) {
65     speed   = velocity_high;
66  } else if(speed==2) {
67     speed = velocity_med;
68  }   else {
69     speed = velocity_slow;
70  }
71  
72  servo_kiri.write(stop_position   + speed -5);
73  servo_kanan.write(stop_position + speed -10);  
74}
75
76void   belok_kiri_patah(int speed) {
77  if(speed==1) {
78     speed = velocity_high;
79   } else if(speed==2) {
80     speed = velocity_med;
81  } else {
82     speed   = velocity_slow;
83  }
84  
85  servo_kiri.write(stop_position -5);
86  servo_kanan.write(stop_position   + speed -10);  
87}
88
89void motor_stop() {
90  servo_kiri.write(stop_position   - 5);
91  servo_kanan.write(stop_position - 10);
92}
93
94void motor_stop2()   {
95  servo_kiri.write(stop_position - 5);
96  servo_kanan.write(stop_position   - 5);
97}
98
99void readSensor() {
100  xsensor = sensor;
101  sensor = 0;
102   trackWarnaPutih = trackMode;
103  batas = batasEE;
104
105  data1 = analogRead(sensorPin1);
106   data2 = analogRead(sensorPin2);
107  data3 = analogRead(sensorPin3);
108  data4   = analogRead(sensorPin4); 
109  
110  if (data1>batas) sensor |= 0x01;
111  if (data2>batas)   sensor |= 0x02;
112  if (data3>batas) sensor |= 0x04;
113  if (data4>batas) sensor   |= 0x08;
114
115  lcd.setCursor(0,1);
116  
117   if (trackWarnaPutih) {
118     sensor   = sensor ^ 0x0F;      
119   } 
120   if(sensor == 0x01) {    
121    // hasil pembacaan   sensor "0001" -> track berada di ujung kanan sensor      
122      belok_kanan(1);
123       Serial.print("0001");
124      lcd.print("Pengiriman Surat");
125   }
126    else if (sensor == 0x03) {    
127    // hasil pembacaan sensor "0011" -> track   berada di ujung kanan sensor      
128      belok_kanan(1);
129      Serial.print("0011");
130       lcd.print("Pengiriman Surat");
131   }
132   else if (sensor == 0x06) {
133       // hasil pembacaan sensor "0110" -> track tepat berada di tengah   
134       motor_maju(3);
135      Serial.print("0110");
136      lcd.print("Pengiriman   Surat");
137   }
138   else if (sensor == 0x07) {
139      // hasil pembacaan sensor   "0111" -> track tepat berada di tengah   
140      belok_kanan(1);
141      Serial.print("0111");
142       lcd.print("Pembacaan Sensor");
143   }
144   else if (sensor == 0x08) {
145       // hasil pembacaan sensor "1000" -> track berada di ujung kiri sensor
146       belok_kiri(1);
147      Serial.print("1000");
148      lcd.print("Pengiriman   Surat");
149   }
150   else if (sensor == 0x0C) {
151      // hasil pembacaan sensor   "1100" -> track berada di ujung kiri sensor
152      belok_kiri(1);
153      Serial.print("1100");
154       lcd.print("Pengiriman Surat");
155   }
156   else if (sensor == 0x0E) {
157       // hasil pembacaan sensor "1110" -> track berada di ujung kiri sensor
158       belok_kiri(1);
159      Serial.print("1110");
160      lcd.print("Pengiriman   Surat");
161   }
162   else if ((sensor == 4001) || (sensor == 2001) || (sensor   == 2100)|| (sensor == 5110)) {
163      belok_kanan(1);
164   }else{
165      motor_maju(3);
166    }
167}
168

void motor_setup() {
  servo_kiri.attach(pin1);
  servo_kanan.attach(pin2);
}

void   buzzer(){
  tone(buzzerPin, 3000);
  delay(500);  
  noTone(buzzerPin);
}

void   motor_maju(int speed) {
  if(speed==1) {
     speed = velocity_high;
  }   else if(speed==2) {
     speed = velocity_med;
  } else {
     speed =   velocity_slow;
  }
  
  servo_kiri.write(stop_position - speed -5);
   servo_kanan.write(stop_position + speed -10);
}

void motor_mundur(int   speed) {
  if(speed==1) {
     speed = velocity_high;
  } else if(speed==2)   {
     speed = velocity_med;
  } else {
     speed = velocity_slow;
   }
  
  servo_kiri.write(stop_position + speed -5);
  servo_kanan.write(stop_position   - speed -10);  
}

void belok_kanan(int speed) {
  if(speed==1) {
      speed = velocity_high;
  } else if(speed==2) {
     speed = velocity_med;
   } else {
     speed = velocity_slow;
  }
  
  servo_kiri.write(stop_position   - speed -5);
  servo_kanan.write(stop_position - speed -10);
}

void   belok_kanan_patah(int speed) {
  if(speed==1) {
     speed = velocity_high;
   } else if(speed==2) {
     speed = velocity_med;
  } else {
     speed   = velocity_slow;
  }
  servo_kiri.write(stop_position - speed -5);
  servo_kanan.write(stop_position   -10);
}

void belok_kiri(int speed) {
  if(speed==1) {
     speed   = velocity_high;
  } else if(speed==2) {
     speed = velocity_med;
  }   else {
     speed = velocity_slow;
  }
  
  servo_kiri.write(stop_position   + speed -5);
  servo_kanan.write(stop_position + speed -10);  
}

void   belok_kiri_patah(int speed) {
  if(speed==1) {
     speed = velocity_high;
   } else if(speed==2) {
     speed = velocity_med;
  } else {
     speed   = velocity_slow;
  }
  
  servo_kiri.write(stop_position -5);
  servo_kanan.write(stop_position   + speed -10);  
}

void motor_stop() {
  servo_kiri.write(stop_position   - 5);
  servo_kanan.write(stop_position - 10);
}

void motor_stop2()   {
  servo_kiri.write(stop_position - 5);
  servo_kanan.write(stop_position   - 5);
}

void readSensor() {
  xsensor = sensor;
  sensor = 0;
   trackWarnaPutih = trackMode;
  batas = batasEE;

  data1 = analogRead(sensorPin1);
   data2 = analogRead(sensorPin2);
  data3 = analogRead(sensorPin3);
  data4   = analogRead(sensorPin4); 
  
  if (data1>batas) sensor |= 0x01;
  if (data2>batas)   sensor |= 0x02;
  if (data3>batas) sensor |= 0x04;
  if (data4>batas) sensor   |= 0x08;

  lcd.setCursor(0,1);
  
   if (trackWarnaPutih) {
     sensor   = sensor ^ 0x0F;      
   } 
   if(sensor == 0x01) {    
    // hasil pembacaan   sensor "0001" -> track berada di ujung kanan sensor      
      belok_kanan(1);
       Serial.print("0001");
      lcd.print("Pengiriman Surat");
   }
    else if (sensor == 0x03) {    
    // hasil pembacaan sensor "0011" -> track   berada di ujung kanan sensor      
      belok_kanan(1);
      Serial.print("0011");
       lcd.print("Pengiriman Surat");
   }
   else if (sensor == 0x06) {
       // hasil pembacaan sensor "0110" -> track tepat berada di tengah   
       motor_maju(3);
      Serial.print("0110");
      lcd.print("Pengiriman   Surat");
   }
   else if (sensor == 0x07) {
      // hasil pembacaan sensor   "0111" -> track tepat berada di tengah   
      belok_kanan(1);
      Serial.print("0111");
       lcd.print("Pembacaan Sensor");
   }
   else if (sensor == 0x08) {
       // hasil pembacaan sensor "1000" -> track berada di ujung kiri sensor
       belok_kiri(1);
      Serial.print("1000");
      lcd.print("Pengiriman   Surat");
   }
   else if (sensor == 0x0C) {
      // hasil pembacaan sensor   "1100" -> track berada di ujung kiri sensor
      belok_kiri(1);
      Serial.print("1100");
       lcd.print("Pengiriman Surat");
   }
   else if (sensor == 0x0E) {
       // hasil pembacaan sensor "1110" -> track berada di ujung kiri sensor
       belok_kiri(1);
      Serial.print("1110");
      lcd.print("Pengiriman   Surat");
   }
   else if ((sensor == 4001) || (sensor == 2001) || (sensor   == 2100)|| (sensor == 5110)) {
      belok_kanan(1);
   }else{
      motor_maju(3);
    }
}

ROBOT_LF

arduino

main program of LF

1// LCD
2#include <LiquidCrystal_I2C.h>
3#include <Servo.h>
4#include   <SPI.h>
5#include <MFRC522.h>
6LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6,   7, 3, POSITIVE);
7
8#define RST_PIN         9           // Configurable, see   typical pin layout above
9#define SS_PIN          10          // Configurable,   see typical pin layout above
10
11int pin1 = 3;
12int pin2 = 5;
13const int   stop_position = 100; 
14const int velocity_high = 85;
15const int velocity_med   = 55;
16const int velocity_slow = 25;
17const int buzzerPin = 8;
18
19Servo   servo_kiri;
20Servo servo_kanan;
21
22// FUNCTION
23#define MAX 5
24int top   = -1;
25int status;
26int a = 10;
27bool full = false;
28
29// SENSOR
30const   int sensorPin1 = A0;
31const int sensorPin2 = A1;
32const int sensorPin3 = A2;
33const   int sensorPin4 = A3;
34int interupt_status = 0;
35int is_home = 0;
36unsigned   char data1,data2,data3,data4;
37unsigned char sensor,batas,xsensor,xstatus;
38unsigned   char trackWarnaPutih;
39unsigned char  trackMode = 0, batasEE = 100;
40#define    lurus         0
41#define   tikunganKiri  1
42#define   tikunganKanan 2
43
44void   setup() {
45  Serial.begin(9600);
46  lcd_setup();
47  ping_setup();  
48  rfid_setup();
49   motor_setup();
50}
51
52void loop() {
53 // motor_maju(2);
54  drawLCD();   
55  if(is_home==0){
56    ping_loop();
57    if(interupt_status == 0){
58         readSensor();
59        rfid_scan();
60    }else if(interupt_status ==   1){
61      motor_stop();
62      buzzer();
63      interupt_status = 0;
64     }else{
65      motor_stop();
66      buzzer();
67    }
68  }else{
69    motor_stop2;
70   }    
71}
72
73
74

// LCD
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include   <SPI.h>
#include <MFRC522.h>
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6,   7, 3, POSITIVE);

#define RST_PIN         9           // Configurable, see   typical pin layout above
#define SS_PIN          10          // Configurable,   see typical pin layout above

int pin1 = 3;
int pin2 = 5;
const int   stop_position = 100; 
const int velocity_high = 85;
const int velocity_med   = 55;
const int velocity_slow = 25;
const int buzzerPin = 8;

Servo   servo_kiri;
Servo servo_kanan;

// FUNCTION
#define MAX 5
int top   = -1;
int status;
int a = 10;
bool full = false;

// SENSOR
const   int sensorPin1 = A0;
const int sensorPin2 = A1;
const int sensorPin3 = A2;
const   int sensorPin4 = A3;
int interupt_status = 0;
int is_home = 0;
unsigned   char data1,data2,data3,data4;
unsigned char sensor,batas,xsensor,xstatus;
unsigned   char trackWarnaPutih;
unsigned char  trackMode = 0, batasEE = 100;
#define    lurus         0
#define   tikunganKiri  1
#define   tikunganKanan 2

void   setup() {
  Serial.begin(9600);
  lcd_setup();
  ping_setup();  
  rfid_setup();
   motor_setup();
}

void loop() {
 // motor_maju(2);
  drawLCD();   
  if(is_home==0){
    ping_loop();
    if(interupt_status == 0){
         readSensor();
        rfid_scan();
    }else if(interupt_status ==   1){
      motor_stop();
      buzzer();
      interupt_status = 0;
     }else{
      motor_stop();
      buzzer();
    }
  }else{
    motor_stop2;
   }    
}

Comments

Only logged in users can leave comments

dedi74132

0 Followers

•

0 Projects

Intro

Trademarks & CopyrightsWhistleblowingDigital Services ActTerms of ServicePrivacy PolicySecurityCookie Settings

Report content