Tentang proyek ini
Hai teman-teman! Berikut adalah tutorial baru untuk memandu Anda langkah demi langkah saat membuat proyek elektronik yang sangat menakjubkan ini, yaitu "robot perayap" yang juga dikenal sebagai "robot laba-laba" atau "robot berkaki empat".
Karena semua orang memperhatikan evolusi kecepatan tinggi dari teknologi robotika, kami memutuskan untuk membawa kalian ke tingkat yang lebih tinggi dalam robotika dan pembuatan robot. kami memulai beberapa waktu lalu dengan membuat beberapa proyek elektronik dasar dan robot dasar seperti PICTO92, robot pengikut garis untuk membuat Anda sedikit akrab dengan barang elektronik dan menemukan diri Anda mampu menciptakan proyek Anda sendiri.
Pindah ke level lain, kami telah memulai dengan robot ini yang merupakan konsep dasar tetapi akan menjadi sedikit rumit jika Anda mempelajari programnya lebih dalam. Dan karena gadget ini sangat mahal di toko web, kami menyediakan panduan langkah demi langkah untuk memandu kalian membuat Spiderbot sendiri .
Proyek ini sangat berguna untuk dibuat khusus setelah mendapatkan PCB khusus yang kami pesan dari JLCPCB untuk meningkatkan penampilan robot kami dan juga ada cukup dokumen dan kode dalam panduan ini untuk memungkinkan Anda membuat perayap dengan mudah.
Kami telah membuat proyek ini hanya dalam 7 hari saja, hanya dua hari untuk menyelesaikan pembuatan perangkat keras dan perakitan, kemudian lima hari untuk menyiapkan kode dan aplikasi android. untuk mengendalikan robot melewatinya. Sebelum memulai mari kita lihat dulu
Apa yang Akan Anda Pelajari: - Memilih komponen yang tepat bergantung pada fungsionalitas proyek Anda
- Membuat rangkaian untuk menghubungkan semua komponen yang dipilih
- Rakit semua bagian proyek
- Penskalaan keseimbangan robot
- Menggunakan aplikasi Android. untuk terhubung melalui Bluetooth dan mulai memanipulasi sistem
Langkah 1:Apa itu "Robot Laba-laba?"
Seperti namanya, robot kami adalah representasi dasar dari gerakan sipder tetapi tidak akan melakukan gerakan tubuh yang sama persis karena kami hanya menggunakan empat kaki, bukan delapan kaki.
Disebut juga sebagai Berempat Empat robot karena memiliki empat kaki dan melakukan gerakan menggunakan kaki tersebut, maka gerakan setiap kaki berhubungan dengan kaki lainnya untuk mengidentifikasi posisi tubuh robot dan juga untuk mengontrol keseimbangan tubuh robot.
Robot berkaki menangani medan lebih baik daripada robot beroda dan bergerak dengan cara yang bervariasi dan kebinatangan. Namun, ini membuat robot berkaki lebih rumit, dan kurang dapat diakses oleh banyak pembuat. dan juga biaya pembuatan dan biaya tinggi yang harus dikeluarkan pembuat untuk membuat tubuh berkaki empat penuh karena didasarkan pada motor servo atau motor stepper dan keduanya lebih mahal daripada motor DC yang dapat digunakan pada robot beroda.
Keuntungan
Anda akan menemukan hewan berkaki empat berlimpah di alam, karena empat kaki memungkinkan stabilitas pasif, atau kemampuan untuk tetap berdiri tanpa secara aktif menyesuaikan posisi. Hal yang sama berlaku untuk robot. Robot berkaki empat lebih murah dan sederhana daripada robot dengan lebih banyak kaki, namun tetap dapat mencapai stabilitas.
Langkah 2:Motor Servo Adalah Aktuator Utama
Sebuah servomotor seperti yang didefinisikan dalam wikipedia, adalah aktuator putar atau aktuator linier yang memungkinkan kontrol yang tepat dari posisi sudut atau linier, kecepatan dan percepatan. Ini terdiri dari motor yang cocok digabungkan ke sensor untuk umpan balik posisi. Ini juga membutuhkan pengontrol yang relatif canggih, seringkali modul khusus yang dirancang khusus untuk digunakan dengan servomotor.
Motor servo bukanlah kelas motor tertentu meskipun istilah motor servo sering digunakan untuk merujuk pada motor yang cocok untuk digunakan dalam sistem kontrol loop tertutup.
Secara umum sinyal kontrol adalah rangkaian pulsa gelombang persegi. Frekuensi umum untuk sinyal kontrol adalah 44Hz, 50Hz, dan 400Hz. Lebar pulsa positif inilah yang menentukan posisi servo. Lebar pulsa positif sekitar 0,5 ms akan menyebabkan tanduk servo membelokkan sebanyak mungkin ke kiri (umumnya sekitar 45 hingga 90 derajat tergantung pada servo yang bersangkutan). Lebar pulsa positif sekitar 2,5 ms hingga 3,0 ms akan menyebabkan servo membelok ke kanan sejauh mungkin. Lebar pulsa sekitar 1,5 ms akan menyebabkan servo menahan posisi netral pada 0 derajat. Output tegangan tinggi umumnya antara 2,5 volt dan 10 volt (dengan tipikal 3V). Tegangan keluaran rendah berkisar dari -40mV hingga 0V.
Langkah 3:Pembuatan PCB (Diproduksi oleh JLCPCB)
Tentang JLCPCB
JLCPCB (Shenzhen JIALICHUANG Electronic Technology Development Co., Ltd.), adalah perusahaan prototipe PCB terbesar di Cina dan produsen teknologi tinggi yang mengkhususkan diri dalam prototipe PCB cepat dan produksi PCB batch kecil.
Dengan lebih dari 10 tahun pengalaman di bidang manufaktur PCB, JLCPCB memiliki lebih dari 200.000 pelanggan di dalam dan luar negeri, dengan lebih dari 8.000 pesanan online pembuatan prototipe PCB dan produksi PCB dalam jumlah kecil per hari. Kapasitas produksi tahunan adalah 200.000 sq.m. untuk berbagai PCB 1-layer, 2-layer atau multi-layer. JLC adalah produsen PCB profesional yang menampilkan skala besar, peralatan sumur, manajemen ketat, dan kualitas unggul.
Kembali ke Proyek Kami
Untuk memproduksi PCB, saya telah membandingkan harga dari banyak produsen PCB dan saya memilih JLCPCB pemasok PCB terbaik dan penyedia PCB termurah untuk memesan sirkuit ini. Yang perlu saya lakukan hanyalah beberapa klik sederhana untuk mengunggah file gerber dan mengatur beberapa parameter seperti warna dan kuantitas ketebalan PCB, lalu saya hanya membayar 2 Dolar untuk mendapatkan PCB saya setelah lima hari saja.
Seperti yang menunjukkan gambar skema terkait, saya telah menggunakan Arduino Nano untuk mengontrol seluruh sistem juga saya telah merancang bentuk robot laba-laba untuk membuat proyek ini jauh lebih baik.
Anda bisa mendapatkan file Sirkuit (PDF) dari sini. Seperti yang Anda lihat pada gambar di atas, PCB dibuat dengan sangat baik dan saya memiliki bentuk laba-laba PCB yang sama dengan yang kami rancang dan semua label serta logo ada untuk memandu saya selama langkah penyolderan.
Anda juga dapat mengunduh file Gerber untuk sirkuit ini dari sini jika Anda ingin memesan desain sirkuit yang sama.
Langkah 4:Bahan
Sekarang mari kita tinjau komponen yang diperlukan yang kita butuhkan untuk proyek ini, jadi seperti yang saya katakan, saya menggunakan Arduino Nano untuk menjalankan semua 12 motor servo dari robot empat kaki. Proyek ini juga menyertakan layar OLED untuk menampilkan wajah Cozmo dan modul bluetooth untuk mengontrol robot melalui aplikasi android.
Untuk membuat proyek semacam ini kita perlu :
- - PCB yang kami pesan dari JLCPCB
- - 12 motor Servo seperti yang Anda ingat 3 servos untuk setiap kaki :https://amzn.to/2B25XbG
- - Satu Arduino Nano :https://amzn.to/2MmZsVg
- - Modul Bluetooth HC-06 :https://amzn.to/2B1Z3CY
- - Satu layar Tampilan OLED :https://amzn.to/2OySnyn
- - LED RGB 5mm :https://amzn.to/2B56hq3
- - Beberapa konektor header :https://amzn.to/2nyZg7i
- - Dan tubuh robot ini memungkinkan Anda untuk mencetaknya menggunakan printer 3D
Langkah 5:Robot Merakit
Sekarang kita sudah menyiapkan PCB dan semua komponen disolder dengan sangat baik, setelah itu kita perlu merakit badan robot, prosedurnya sangat mudah jadi ikuti saja langkah-langkah yang saya tunjukkan, pertama-tama kita perlu mempersiapkan setiap kaki samping dan membuat satu led kami membutuhkan dua motor servo untuk sambungan dan bagian yang dicetak Coxa, Femur dan Tibia dengan bagian pemasangan kecil ini.
Tentang potongan Tubuh robot, Anda dapat mengunduh file STL-nya dari sini.
Dimulai dari servo pertama, masukkan ke dalam soketnya dan tahan dengan sekrupnya, setelah itu putar kapak servo ke 180 ° tanpa memasang sekrup untuk menempel dan pindah ke bagian berikutnya yaitu Femur untuk menghubungkannya ke tibia menggunakan kapak sambungan servo pertama dan bagian yang dilampirkan. Langkah terakhir untuk melengkapi kaki adalah menempatkan sendi kedua yang saya maksud adalah servo kedua untuk menahan bagian kaki ketiga yang merupakan potongan Coxa.
Sekarang ulangi hal yang sama untuk semua kaki untuk menyiapkan empat kaki. Setelah itu ambil sasis atas dan letakkan sisa servos di soketnya dan kemudian sambungkan setiap kaki ke servo yang sesuai. Hanya ada satu bagian tercetak terakhir yang merupakan sasis robot bawah tempat kami akan meletakkan papan sirkuit kami
Langkah 6:Aplikasi Android
Berbicara tentang android up memungkinkan Anda untuk
sambungkan ke robot Anda melalui Bluetooth dan lakukan gerakan maju dan mundur serta belok kiri ke kanan, ini memungkinkan Anda juga untuk mengontrol warna cahaya robot secara real time dengan memilih warna yang diinginkan dari roda warna ini.
Anda dapat mengunduh aplikasi Android secara gratis dari tautan ini:di sini
Langkah 7:Kode Arduino dan Validasi Uji
Sekarang kita memiliki robot yang hampir siap untuk dijalankan tetapi kita perlu mengatur sudut sambungan terlebih dahulu, jadi unggah kode pengaturan yang memungkinkan Anda untuk menempatkan setiap servo di posisi yang tepat dengan memasang servo dalam 90 derajat jangan lupa untuk menghubungkan 7V Baterai DC untuk menjalankan robot.
Selanjutnya kita perlu mengupload program utama untuk mengontrol robot menggunakan aplikasi android. Kedua program tersebut dapat Anda unduh dari tautan berikut:
- Scaling kode servo :link download- Program utama robot Spider :link download
Setelah mengupload kode, saya menghubungkan layar OLED untuk menampilkan senyum robot Cozmo yang saya buat di kode utama.
Seperti yang Anda lihat pada gambar di atas, robot mengikuti semua instruksi yang dikirim dari ponsel cerdas saya dan masih ada beberapa perbaikan lain yang harus dilakukan agar jauh lebih baik.
Kode
- Kode utama Robot Laba-laba
Kode utama Robot Laba-labaArduino
/********************************************* ************************************************** ************************************************** ********************** * - Penulis :BELKHIR Mohamed * * - Profesi :( Ingineer Listrik) Pemilik MEGA DAS * * - Tujuan Utama :Aplikasi Industri * * - Pemegang Hak Cipta (c) :All rights reserved * * - Lisensi :Lisensi 2 Klausul BSD * * - Tanggal :20/04/2017 * * ****************** ************************************************** ************************************************** *************************************************/ /*********************************** CATATAN ************* *************************/// Redistribusi dan penggunaan dalam bentuk sumber dan biner, dengan atau tanpa// modifikasi, diizinkan dengan ketentuan sebagai berikut kondisi terpenuhi:// * Redistribusi kode sumber harus mempertahankan pemberitahuan hak cipta di atas, ini// daftar kondisi dan penafian berikut.// * Redistribusi dalam bentuk biner harus direproduksi ce pemberitahuan hak cipta di atas,// daftar ketentuan ini dan penafian berikut dalam dokumentasi// dan/atau materi lain yang disediakan bersama distribusi.// PERANGKAT LUNAK INI DISEDIAKAN OLEH PEMEGANG HAK CIPTA DAN KONTRIBUTOR "SEBAGAIMANA ADANYA"// DAN SETIAP JAMINAN TERSURAT MAUPUN TERSIRAT, TERMASUK, NAMUN TIDAK TERBATAS PADA, JAMINAN TERSIRAT UNTUK DIPERDAGANGKAN DAN KESESUAIAN UNTUK TUJUAN TERTENTU DIABAIKAN/* */#termasuk //untuk mendefinisikan dan mengontrol servos#include //untuk mengatur timer untuk mengelola semua servos#include #include #include // OLED display TWI address#define OLED_ADDR 0x3CAdafruit_SSD1306 display(-1); /* Servo ----------------------------------------------- ---------------------*///menentukan 12 servos untuk 4 kakiServo servo[4][3];//menentukan port servosconst int servo_pin[4] [3] ={ {11, 12, 13}, {2, 4, 7}, {14, 15, 16},{8, 9, 10} };/* Ukuran robot ------ -------------------------------------------------- -*/const float l ength_a =50;const float length_b =77.1;const float length_c =27.5;const float length_side =71;const float z_absolute =-28;/* Konstanta untuk gerakan ---------------- ------------------------------------*/const float z_default =-50, z_up =-30, z_boot =z_absolute;const float x_default =62, x_offset =0;const float y_start =0, y_step =40;const float y_default =x_default;/* variabel untuk pergerakan --------------- -------------------------------------*/volatile float site_now[4][3]; //koordinat real-time dari akhir setiap legvolatile float site_expect[4][3]; //koordinat yang diharapkan dari akhir setiap legfloat temp_speed[4][3]; //kecepatan setiap sumbu, perlu dihitung ulang sebelum setiap gerakanmengambang move_speed; //pergerakan speedfloat speed_multiple =1; //kecepatan gerakan multipleconst float spot_turn_speed =4;const float leg_move_speed =8;const float body_move_speed =3;const float stand_seat_speed =1;volatile int rest_counter; //+1/0.02s, untuk istirahat otomatis//parameter fungsiconst float KEEP =255;//define PI untuk perhitunganconst float pi =3.1415926;/* Konstanta untuk turn ------------- -------------------------------------------*///temp lengthconst float temp_a =sqrt(pow(2 * x_default + length_side, 2) + pow(y_step, 2));const float temp_b =2 * (y_start + y_step) + length_side;const float temp_c =sqrt(pow(2 * x_default + length_side , 2) + pow(2 * y_start + y_step + length_side, 2));const float temp_alpha =acos((pow(temp_a, 2) + pow(temp_b, 2) - pow(temp_c, 2)) / 2 / temp_a / temp_b);//situs untuk turnconst float turn_x1 =(temp_a - length_side) / 2;const float turn_y1 =y_start + y_step / 2;const float turn_x0 =turn_x1 - temp_b * cos(temp_alpha);const float turn_y0 =temp_b * sin (temp_alpha) - turn_y1 - length_side;const int lightR=3;const int lightG=5;const int lightB=6;int LedR=0;int LedG=0;int LedB=0;char SerialData; // Gunakan variabel ini untuk membaca setiap caractere yang diterima melalui serial portString data="";void setup() { Serial.begin(9600); display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR); tampilan.clearDisplay(); tampilan.display(); penundaan (10000); set_site(0, x_default - x_offset, y_start + y_step, z_boot); set_site(1, x_default - x_offset, y_start + y_step, z_boot); set_site(2, x_default + x_offset, y_start, z_boot); set_site(3, x_default + x_offset, y_start, z_boot); for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { site_now[i][j] =site_expect[i][j]; } } //mulai layanan servo FlexiTimer2::set(20, servo_service); FlexiTimer2::mulai(); //inisialisasi servos servo_attach(); berdiri();// penundaan(3000);// duduk();// penundaan(3000);// berdiri();// penundaan(3000); senang(); penundaan (acak (500, 1000)); cierra(); penundaan (150); enfado(); penundaan (acak (1000, 3000)); cierra(); penundaan (150); entorna(); penundaan (acak (1000, 3000)); cierra(); penundaan (150); enfado1(); penundaan (acak (1000, 3000)); cierra(); penundaan (150); triste(); penundaan (acak (1000, 3000)); cierra(); penundaan (150); abre(); penundaan (acak (500, 3000)); cierra(); penundaan (150); senang(); delay (random (500, 1000));}void loop() { while(Serial.available()) // Selama data serial tersedia, kami menyimpannya { delay(10); SerialData=Serial.read(); if(SerialData=='b') LedR=Serial.parseInt(); else if(SerialData=='g') LedG=Serial.parseInt(); else if(SerialData=='r') LedB=Serial.parseInt(); data lain+=SerialData; } if(data=="f") // Jika data yang disimpan adalah gerakan maju { cierra(); penundaan (150); senang(); langkah_maju(1); } if(data=="p") // Jika data yang disimpan adalah gerakan mundur { cierra(); penundaan (150); triste(); langkah_belakang(1); } if(data=="l") // Jika data yang disimpan adalah belok kiri mobil { cierra(); penundaan (150); enfado1(); belok_kiri(1); } if(data=="m") // Jika data yang disimpan adalah belok kanan mobil { cierra(); penundaan (150); enfado(); belok_kanan(5); } data=""; analogWrite(lightR,LedR); analogWrite(lightG,LedG); analogWrite(lightB,LedB);}void servo_attach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j]. lampirkan(servo_pin[i][j]); penundaan(100); } }}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].detach(); penundaan(100); } }}void duduk(void){ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_boot); } wait_all_reach();}/* - berdiri - fungsi pemblokiran ------------------------------------- ---------------------------------------*/void stand(void){ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_default); } wait_all_reach();}/* - tempat belok ke kiri - fungsi pemblokiran - langkah langkah parameter ingin berbelok --------------------------- --------------------------------------------------*/ void turn_left(langkah int tidak ditandatangani){ move_speed =spot_turn_speed; while (langkah--> 0) { if (site_now[3][1] ==y_start) { //leg 3&1 pindah set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_default); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start, z_up); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&2 pindahkan set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_default); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_up); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - spot belok kanan - fungsi blocking - langkah parameter langkah ingin berbelok ------------------------------ ---------------------------------------------*/void turn_right( unsigned int step){ move_speed =spot_turn_speed; while (langkah--> 0) { if (site_now[2][1] ==y_start) { //leg 2&0 pindah set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_up); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&3 pindah set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_up); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - maju - fungsi pemblokiran - langkah langkah parameter yang ingin dilakukan -------------------------------- -------------------------------------------------------*/void step_forward (tidak ditandatangani ke langkah){ move_speed =leg_move_speed; while (langkah--> 0) { if (site_now[2][1] ==y_start) { //leg 2&1 pindah set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); bergerak_kecepatan =body_move_speed; set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); kecepatan_gerakan =kecepatan_kaki_gerakan; set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&3 pindah set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); bergerak_kecepatan =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); kecepatan_gerakan =kecepatan_kaki_gerakan; set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - kembali - fungsi pemblokiran - langkah langkah parameter ingin pergi -------------------------------- -------------------------------------------------------*/void step_back (tidak ditandatangani ke langkah){ move_speed =leg_move_speed; while (langkah--> 0) { if (site_now[3][1] ==y_start) { //leg 3&0 pindah set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); bergerak_kecepatan =body_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); kecepatan_gerakan =kecepatan_kaki_gerakan; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&2 pindah set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); bergerak_kecepatan =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); kecepatan_gerakan =kecepatan_kaki_gerakan; set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}// tambahkan oleh RegisHsuvoid body_left(int i){ set_site(0, site_now[0][0] + i, KEEP, KEEP); set_site(1, site_now[1][0] + i, KEEP, KEEP); set_site(2, site_now[2][0] - saya, KEEP, KEEP); set_site(3, site_now[3][0] - i, KEEP, KEEP); wait_all_reach();}void body_right(int i){ set_site(0, site_now[0][0] - i, KEEP, KEEP); set_site(1, site_now[1][0] - i, KEEP, KEEP); set_site(2, site_now[2][0] + i, KEEP, KEEP); set_site(3, site_now[3][0] + i, KEEP, KEEP); wait_all_reach();}void hand_wave(int i){ float x_tmp; mengapung y_tmp; mengapung z_tmp; bergerak_kecepatan =1; if (site_now[3][1] ==y_start) { body_right(15); x_tmp =site_now[2][0]; y_tmp =site_now[2][1]; z_tmp =site_now[2][2]; bergerak_kecepatan =body_move_speed; for (int j =0; j i / 4)// move_speed =body_dance_speed * 2;// if (j> i / 2)// move_speed =body_dance_speed * 3;/ / set_site(0, KEEP, y_default - 20, KEEP);// set_site(1, KEEP, y_default + 20, KEEP);// set_site(2, KEEP, y_default - 20, KEEP);// set_site(3, KEEP, y_default + 20, KEEP);// wait_all_reach();// set_site(0, KEEP, y_default + 2 0, KEEP);// set_site(1, KEEP, y_default - 20, KEEP);// set_site(2, KEEP, y_default + 20, KEEP);// set_site(3, KEEP, y_default - 20, KEEP); // wait_all_reach();// }// move_speed =body_dance_speed;// head_down(30);//}/* - layanan microservos /fungsi interupsi timer/50Hz - ketika set situs diharapkan, fungsi ini memindahkan titik akhir ke sana dalam garis lurus - temp_speed[4][3] harus diatur sebelum mengatur situs yang diharapkan, itu memastikan titik akhir bergerak dalam garis lurus, dan memutuskan kecepatan bergerak. -------------------------------------------------- -------------------------*/void servo_service(void){ sei(); alfa float statis, beta, gamma; for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { if (abs(site_now[i][j] - site_expect[i][j])>=abs(temp_speed[i][j])) site_now[i][j] +=temp_speed[i][j]; lain site_now[i][j] =site_expect[i][j]; } cartesian_to_polar(alfa, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]); polar_to_servo(i, alfa, beta, gamma); } rest_counter++;}/* - atur salah satu situs ekspektasi titik akhir - fungsi ini akan mengatur temp_speed[4][3] secara bersamaan - fungsi non-blocking --------------- -------------------------------------------------- ----------*/void set_site(int leg, float x, float y, float z){ float length_x =0, length_y =0, length_z =0; if (x !=KEEP) length_x =x - site_now[leg][0]; if (y !=KEEP) length_y =y - site_now[leg][1]; if (z !=KEEP) length_z =z - site_now[leg][2]; panjang float =sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2)); temp_speed[leg][0] =length_x / length * move_speed * speed_multiple; temp_speed[leg][1] =length_y / length * move_speed * speed_multiple; temp_speed[leg][2] =length_z / length * move_speed * speed_multiple; if (x !=KEEP) site_expect[leg][0] =x; if (y !=KEEP) site_expect[leg][1] =y; if (z !=KEEP) site_expect[leg][2] =z;}/* - wait one of end points move to expect site - blocking function ---------------------------------------------------------------------------*/void wait_reach(int leg){ while (1) if (site_now[leg][0] ==site_expect[leg][0]) if (site_now[leg][1] ==site_expect[leg][1]) if (site_now[leg][2] ==site_expect[leg][2]) break;}/* - wait all of end points move to expect site - blocking function ---------------------------------------------------------------------------*/void wait_all_reach(void){ for (int i =0; i <4; i++) wait_reach(i);}/* - trans site from cartesian to polar - mathematical model 2/2 ---------------------------------------------------------------------------*/void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z){ //calculate w-z degree float v, w; w =(x>=0 ? 1 :-1) * (sqrt(pow(x, 2) + pow(y, 2))); v =w - length_c; alpha =atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2))); beta =acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b); //calculate x-y-z degree gamma =(w>=0) ? atan2(y, x) :atan2(-y, -x); //trans degree pi->180 alpha =alpha / pi * 180; beta =beta / pi * 180; gamma =gamma / pi * 180;}/* - trans site from polar to microservos - mathematical model map to fact - the errors saved in eeprom will be add ---------------------------------------------------------------------------*/void polar_to_servo(int leg, float alpha, float beta, float gamma){ if (leg ==0) { alpha =90 - alpha; beta =beta; gamma +=90; } else if (leg ==1) { alpha +=90; beta =180 - beta; gamma =90 - gamma; } else if (leg ==2) { alpha +=90; beta =180 - beta; gamma =90 - gamma; } else if (leg ==3) { alpha =90 - alpha; beta =beta; gamma +=90; } servo[leg][0].write(alpha); servo[leg][1].write(beta); servo[leg][2].write(gamma);}void abre() { display.clearDisplay(); display.fillCircle (50, 15, 12, WHITE); //ojo izquierdo grande display.fillCircle (82, 20, 7, WHITE); //ojo derecho pequeo display.display();}void cierra() { display.clearDisplay(); display.drawFastHLine(40, 15, 20, WHITE); display.drawFastHLine(72, 20, 15, WHITE); display.display();}void entorna() { display.clearDisplay(); display.fillCircle (42, 10, 20, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 15, WHITE); //ojo derecho pequeo display.fillRect (0, 0, 128, 15, BLACK); //ceja superior display.fillRect (0, 40, 128, 15, BLACK); //ceja inferior display.display();}void triste() { display.clearDisplay(); display.fillCircle (42, 10, 17, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 17, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 0, 35, 78, 0, BLACK); //ceja superior display.fillTriangle (50, 0, 128, 35, 128, 0, BLACK); //ceja superior display.display();}void happy() { display.clearDisplay(); display.fillCircle (42, 25, 15, WHITE); //ojo izquierdo grande display.fillCircle (82, 25, 15, WHITE); //ojo derecho pequeo display.fillCircle (42, 33, 20, BLACK); //ojo izquierdo grande display.fillCircle (82, 33, 20, BLACK); //ojo derecho pequeo display.display();}void enfado() { display.clearDisplay(); display.fillCircle (42, 10, 18, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 12, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 54, 26, 118, 0, BLACK); //ceja superior display.display();}void enfado1() { display.clearDisplay(); display.fillCircle (42, 10, 18, WHITE); //ojo izquierdo grande display.fillCircle (82, 10, 12, WHITE); //ojo derecho pequeo display.fillTriangle (0, 0, 65, 15, 120, 0, BLACK); //ceja superior display.display();} Suku cadang dan penutup khusus
