Pencari jalur

Kode

• Program

ProgramArduino

#include  // Tambahkan Adafruit Motor Shield untuk perpustakaan kit Arduino.#include  // Tambahkan perpustakaan Servo Motor.#define BuzzPIN A0 // Tetapkan PIN A0 sebagai BuzzPIN (Hubungkan PIN Arduino UNO "A0" dengan PIN Buzzer "+").#definisikan TrigPIN A1 // Tetapkan PIN A1 sebagai TrigPIN (Hubungkan PIN Arduino UNO "A1" dengan Sensor Ultrasonik Sonar "Trig" PIN).#define EchoPIN A2 // Tetapkan PIN A2 sebagai EchoPIN (Hubungkan PIN Arduino UNO "A2" dengan PIN "Trig" Sensor Ultrasonik Sonar).#definisikan LEDBPIN A3 // Tetapkan PIN A3 sebagai LEDBPIN (Hubungkan Arduino UNO "A3" PIN dengan PIN RGB Diffused Common Cathode "LEDB").#definisikan LEDGPIN A4 // Tetapkan PIN A4 sebagai LEDGPIN (Hubungkan PIN Arduino UNO "A4" dengan PIN RGB Diffused Common Cathode "LEDG").#define LEDRPIN A5 // Tetapkan PIN A5 sebagai LEDRPIN (Hubungkan PIN Arduino UNO "A5" dengan PIN RGB Diffused Common Cathode "LEDR").#define DCMROFF 25 // Ini menetapkan Offset untuk memungkinkan perbedaan antara dua Motor traksi DC.AF_DCMo untuk M1 (1, MOTOR12_64KHZ); // Buat DCMotor #1 menggunakan output M1, Setel ke frekuensi PWM 64kHz.AF_DCMotor M2 (2, MOTOR12_64KHZ); // Buat DCMotor #2 menggunakan output M2, Setel ke frekuensi PWM 64kHz.Servo SER1; // Buat objek Servo untuk mengontrol Servo.int Search (void) { // Deklarasi variabel tipe integer. float Durasi =0,0; // Deklarasi variabel tipe float. mengambang CM =0,0; // Deklarasi variabel tipe float. digitalWrite (TrigPIN, RENDAH); // Output TrigPIN sebagai 0V (Logika level rendah). delayMicroseconds (2); // Tunda selama 2us, Kirim 10 pulsa tinggi ke Sensor Sonar Ultrasonik "TrigPIN". digitalWrite (TrigPIN, TINGGI); // Output TrigPIN sebagai 5V (Logika level tinggi). delayMicroseconds (10); // Penundaan selama 10 kita. digitalWrite (TrigPIN, RENDAH); // Output TrigPIN sebagai 0V (Logika level rendah). Durasi =pulseIn (EchoPIN, TINGGI); // Mulai menghitung waktu, Upto lagi EchoPIN kembali ke logika "Tingkat Tinggi" dan menempatkan "Waktu" ke dalam variabel yang disebut "Durasi". CM =(Durasi/58.8); // Konversi Jarak ke CM. kembali CM; // Kembali ke CM.}int RightDistance, LeftDistance; // Jarak di kedua sisi.float Distance =0.00; // Deklarasi variabel tipe float.void setup () { // Setup loop. pinMode (BuzzPIN, OUTPUT); // Deklarasikan BuzzPIN sebagai "PIN Keluaran". pinMode (TrigPIN, OUTPUT); // Deklarasikan TrigPIN sebagai "PIN Keluaran". pinMode (EchoPIN, INPUT); // Deklarasikan EchoPIN sebagai "PIN Keluaran". pinMode (LEDBPIN, OUTPUT); // Deklarasikan LEDBPIN sebagai "PIN Keluaran". pinMode (LEDGPIN, OUTPUT); // Deklarasikan LEDGPIN sebagai "PIN Keluaran". pinMode (LEDRPIN, OUTPUT); // Deklarasikan LEDRPIN sebagai "PIN Keluaran". SER1.attach (10); // Memasang Servo pada pin 10 (SER1 pada Adafruit Motor Shield untuk kit Arduino ke objek Servo).}void loop () { // Main loop. SER1.tulis (80); // Memberitahu Servo ke posisi 80 derajat (Menghadap ke depan). penundaan (100); // Penundaan selama 0,1 detik. Jarak =Cari (); // Mengukur Jarak dalam CM. if (Jarak <30) { // Jika rintangan ditemukan dalam jarak 30cm. digitalWrite (BuzzPIN, TINGGI); // Keluaran BuzzPIN sebagai 5V (Logika tingkat tinggi). digitalWrite (LEDBPIN, RENDAH); // Output LEDBPIN sebagai 0V (Logika level rendah). digitalWrite (LEDGPIN, RENDAH); // Output LEDGPIN sebagai 0V (Logika level rendah). digitalWrite (LEDRPIN, TINGGI); // Output LEDRPIN sebagai 5V (Logika tingkat tinggi). M1.setSpeed ​​(100); // Mempercepat. M2.setSpeed ​​(100); // Mempercepat. Ubah Jalur(); // Jika forward terhalang Ubah arah. } else if ((Jarak>=30) &&(Jarak <60)) { // Jika hambatan ditemukan antara 30cm sampai 60cm. digitalWrite (BuzzPIN, RENDAH); // Keluaran BuzzPIN sebagai 0V (Logika level rendah). digitalWrite (LEDBPIN, TINGGI); // Output LEDBPIN sebagai 5V (Logika tingkat tinggi). digitalWrite (LEDGPIN, RENDAH); // Output LEDGPIN sebagai 0V (Logika level rendah). digitalWrite (LEDRPIN, RENDAH); // Output LEDRPIN sebagai 0V (Logika level rendah). M1.setSpeed ​​(150); // Kecepatan meningkat sedikit. M2.setSpeed ​​(150); // Kecepatan meningkat sedikit. Maju (); // Robot bergerak ke arah Maju. } else if ((Jarak>=60) &&(Jarak <90)) { // Jika hambatan ditemukan antara 60cm sampai 90cm. digitalWrite (BuzzPIN, RENDAH); // Keluaran BuzzPIN sebagai 0V (Logika level rendah). digitalWrite (LEDBPIN, RENDAH); // Output LEDBPIN sebagai 0V (Logika level rendah). digitalWrite (LEDGPIN, TINGGI); // Output LEDGPIN sebagai 5V (Logika tingkat tinggi). digitalWrite (LEDRPIN, RENDAH); // Output LEDRPIN sebagai 0V (Logika level rendah). M1.setSpeed ​​(200); // Mempercepat. M2.setSpeed ​​(200); // Mempercepat. Maju (); // Robot bergerak ke arah Maju. } else { // Jika rintangan tidak dapat ditemukan dalam 90cm. digitalWrite (BuzzPIN, RENDAH); // Keluaran BuzzPIN sebagai 0V (Logika level rendah). digitalWrite (LEDBPIN, TINGGI); // Output LEDBPIN sebagai 5V (Logika tingkat tinggi). digitalWrite (LEDGPIN, TINGGI); // Output LEDGPIN sebagai 5V (Logika tingkat tinggi). digitalWrite (LEDRPIN, TINGGI); // Output LEDRPIN sebagai 5V (Logika tingkat tinggi). M1.setSpeed ​​(250); // Kecepatan meningkat sepenuhnya. M2.setSpeed ​​(250); // Kecepatan meningkat sepenuhnya. Maju (); // Robot bergerak ke arah Maju. }}void ChangePath () { // Perulangan Perubahan Path. Berhenti (); // Robot Berhenti. Mundur (); // Robot berjalan Arah mundur. Berhenti (); // Robot Berhenti. SER1.tulis (12); // Periksa Jarak ke Kanan. penundaan (500); // Penundaan selama 0,5 detik. Jarak Kanan =Pencarian (); // Atur Jarak yang Tepat. penundaan (500); // Penundaan selama 0,5 detik. SER1.tulis (160); // Periksa Jarak ke Kiri. penundaan (1000); // Penundaan selama 1 detik. LeftDistance =Cari (); // Atur Jarak Kiri. penundaan (500); // Penundaan selama 0,5 detik. SER1.tulis (80); // Kembali ke tengah. penundaan (500); // Penundaan selama 0,5 detik. BandingkanJarak(); // Cari jarak terjauh.}void CompareDistance () { // Jarak Membandingkan loop. if (Jarak Kanan> Jarak Kiri) { // Jika Kanan kurang terhalang. Belok kanan (); // Robot Belok ke arah Kanan. } else if (LeftDistance> RightDistance) { // If Left kurang terhalang. Belok kiri (); // Robot Belok ke arah Kiri. } else { // Jika keduanya sama-sama terhalang. Berputar (); // Robot Berbalik. }}void Maju () { // Maju loop. M1.run (MAJU); // Putar DCMotor #1 ke Forward. M2.run (MAJU); // Putar DCMotor #1 ke Forward.}void Backward () { // Backward loop. M1.run (Mundur); // Putar DCMotor #1 ke Mundur. M2.run (Mundur); // Putar DCMotor #2 ke Mundur. penundaan (500); // Penundaan selama 1 detik.}void Belok Kanan () { // Putaran Belok Kanan. M1.run (Mundur); // Putar DCMotor #1 ke Mundur. M2.run (MAJU); // Putar DCMotor #2 ke Forward. M1.setSpeed ​​(100+DCMROFF); // Kalibrasi Kecepatan Motor DC #1. penundaan (300); // Penundaan selama 0,7 detik.}void TurnLeft () { // Putaran Kiri. M1.run (MAJU); // Putar DCMotor #1 ke Forward. M2.run (Mundur); // Putar DCMotor #2 ke Mundur. M2.setSpeed ​​(100+DCMROFF); // Kalibrasi Kecepatan Motor DC #2. penundaan (300); // Penundaan selama 0,7 detik.}void TurnAround () { // Putaran Putaran. M1.run (MAJU); // Putar DCMotor #1 ke Forward. M2.run (Mundur); // Putar DCMotor #2 ke Mundur. M2.setSpeed ​​(100+DCMROFF); // Kalibrasi Kecepatan Motor DC #2. penundaan (700); // Penundaan selama 2,1 detik.}void Stop () { // Stop loop. M1.run (RILIS); // Lepaskan DCMotor #1. M2.run (RILIS); // Lepaskan DCMotor #2. penundaan (100); // Penundaan selama 0,1 detik.}