Membangun Robot Labah-Labah Arduino – Tutorial Langkah demi Langkah

Komponen dan Perlengkapan

Berikut adalah komponen yang diperlukan untuk membangun robot labah-labah Arduino ini:

Arduino Nano R3	1
Modul Bluetooth Low Energy (BLE) (Generik)	1
Onion Corporation OLED Expansion	1
RGB Diffused Common Cathode	1
PCB Kustom JLCPCB	1

Tentang Proyek

Halo para pembaca! Berikut adalah tutorial baru untuk memandu Anda langkah demi langkah dalam membuat proyek elektronik super luar biasa ini, yang dikenal sebagai "robot merayap" atau juga disebut "robot labah-labah" atau "robot kaki empat."

Karena semua orang menyadari evolusi kecepatan tinggi teknologi robotika, kami memutuskan untuk membawa Anda 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 terbiasa dengan hal elektronik dan mampu menemukan proyek Anda sendiri.

Beralih ke tingkat yang lebih tinggi, kami memulai dengan robot ini yang konsepnya dasar namun akan menjadi sedikit rumit jika Anda mempelajari programnya lebih dalam. Dan karena perangkat ini sangat mahal di toko online, kami menyediakan panduan langkah demi langkah ini untuk memandu Anda membuat Spiderbot Anda sendiri.

Proyek ini sangat praktis untuk dibuat, terutama setelah mendapatkan PCB kustom yang kami pesan dari JLCPCB untuk meningkatkan penampilan robot kami dan juga ada cukup dokumen dan kode dalam panduan ini untuk memungkinkan Anda membuat robot merayap dengan mudah.

Kami telah membuat proyek ini dalam waktu hanya 7 hari, hanya dua hari untuk menyelesaikan pembuatan hardware dan rangkaian, lalu lima hari untuk menyiapkan kode dan aplikasi Android untuk mengontrol robot melalui aplikasi itu. Sebelum memulai, mari kita lihat terlebih dahulu

Apa yang Akan Anda Pelajari:

• Memilih komponen yang tepat berdasarkan fungsionalitas proyek Anda

• Membuat rangkaian untuk menghubungkan semua komponen yang dipilih

• Merakit semua bagian proyek

• Penyesuaian keseimbangan robot

• Menggunakan aplikasi Android untuk terhubung melalui Bluetooth dan mulai memanipulasi sistem

Langkah 1: Apa Itu "Robot Labah-Labah?"

Seperti namanya yang menentukan, robot kami adalah representasi dasar gerakan labah-labah namun tidak akan melakukan gerakan tubuh yang sama karena kami hanya menggunakan empat kaki alih-alih delapan kaki.

Disebut juga robot kaki empat karena memiliki empat kaki dan bergerak menggunakan kaki ini, gerakan setiap kaki terkait dengan kaki lain untuk mengidentifikasi posisi tubuh robot dan juga untuk mengontrol keseimbangan tubuh robot.

Robot kaki menangani medan dengan lebih baik daripada sejenis roda mereka dan bergerak dengan cara yang beragam dan mirip hewan. Namun, ini membuat robot kaki lebih rumit dan kurang dapat diakses oleh banyak pembuat. dan juga biaya pembuatan dan biaya tinggi yang harus dikeluarkan oleh pembuat untuk membuat tubuh kaki empat lengkap karena didasarkan pada servo motor atau stepper motor dan keduanya lebih mahal dari motor DC yang dapat digunakan dalam robot roda.

Keuntungan

Anda akan menemukan kaki empat banyak di alam, karena empat kaki memungkinkan stabilitas pasif, atau kemampuan untuk tetap berdiri tanpa menyesuaikan posisi secara aktif. Hal yang sama berlaku untuk robot. Robot kaki empat lebih murah dan lebih sederhana daripada robot dengan lebih banyak kaki, namun masih dapat mencapai stabilitas.

Langkah 2: Servo Motor adalah Aktor Utama

Servomotor sebagaimana didefinisikan di Wikipedia, adalah aktuator rotary atau linear yang memungkinkan kontrol presisi posisi sudut atau linear, kecepatan dan percepatan.[1] Ini terdiri dari motor yang sesuai yang dikaitkan dengan sensor untuk umpan balik posisi. Ini juga membutuhkan kontroler yang cukup canggih, seringkali modul khusus yang dirancang khusus untuk digunakan dengan servomotor.

Servomotor bukan kelas spesifik motor meskipun istilah servomotor sering digunakan untuk merujuk pada motor yang cocok untuk digunakan dalam sistem kontrol closed-loop.

Secara umum, sinyal kontrol adalah rangkaian pulsa gelombang persegi. Frekuensi umum untuk sinyal kontrol adalah 44Hz, 50Hz, dan 400Hz. Lebar pulsa positif yang menentukan posisi servo. Lebar pulsa positif sekitar 0.5ms akan menyebabkan horn servo melengkung sejauh mungkin ke kiri (secara umum sekitar 45 hingga 90 derajat tergantung pada servo yang bersangkutan). Lebar pulsa positif sekitar 2.5ms hingga 3.0ms akan menyebabkan servo melengkung ke kanan sejauh mungkin. Lebar pulsa sekitar 1.5ms akan menyebabkan servo memegang posisi netral pada 0 derajat. Tegangan output tinggi biasanya di antara 2,5 volt dan 10 volt (3V biasanya). Tegangan output 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 spesialis dalam prototipe PCB cepat dan produksi PCB jumlah kecil.

Dengan pengalaman lebih dari 10 tahun dalam pembuatan PCB, JLCPCB memiliki lebih dari 200.000 pelanggan di dalam dan luar negeri, dengan lebih dari 8.000 pesanan online prototipe PCB dan produksi PCB jumlah kecil per hari. Kapasitas produksi tahunan adalah 200.000 sq.m. untuk berbagai PCB lapisan 1, 2 lapisan atau multi-lapisan. JLC adalah produsen PCB profesional dengan skala besar, peralatan yang baik, manajemen yang ketat dan kualitas superior.

Kembali ke Proyek Kami

Untuk memproduksi PCB, saya telah membandingkan harga dari banyak produsen PCB dan saya memilih JLCPCB sebagai penyedia PCB terbaik dan penyedia PCB termurah untuk memesan rangkaian ini. Yang harus saya lakukan hanyalah beberapa klik sederhana untuk mengunggah file gerber dan menetapkan beberapa parameter seperti ketebalan, warna dan jumlah PCB, lalu saya telah membayar hanya 2 Dollar untuk mendapatkan PCB saya setelah hanya lima hari.

Seperti yang ditunjukkan pada gambar skematik terkait, saya menggunakan Arduino Nano untuk mengontrol seluruh sistem dan saya juga merancang bentuk robot labah-labah untuk membuat proyek ini jauh lebih baik.

Anda dapat mendapatkan file rangkaian (PDF) dari sini. Seperti yang Anda lihat pada gambar di atas, PCB sangat baik diproduksi dan saya telah mendapatkan bentuk PCB labah-labah yang sama yang telah kita desain dan semua label dan logo ada untuk memandu saya selama langkah pengelasan.

Anda juga dapat mengunduh file Gerber untuk rangkaian ini dari sini jika Anda ingin memesan desain rangkaian yang sama.

Langkah 4: Bahan-bahan

Sekarang mari kita tinjau komponen yang diperlukan untuk proyek ini, jadi seperti yang telah saya katakan, saya menggunakan Arduino Nano untuk menjalankan semua 12 servo motor dari kaki robot empat kaki. Proyek ini juga mencakup layar OLED untuk menampilkan wajah Cozmo dan modul bluetooth untuk mengontrol robot melalui aplikasi android.

Untuk membuat jenis proyek ini kita akan membutuhkan:

• - PCB yang kami pesan dari JLCPCB

• - 12 Servo motor seperti yang Anda ingat 3 servo untuk setiap kaki: https://amzn.to/2B25XbG

• - Satu Arduino Nano: https://amzn.to/2MmZsVg

• - Modul Bluetooth HC-06: https://amzn.to/2B1Z3CY

• - Satu layar OLED: https://amzn.to/2OySnyn

• - LED RGB 5mm: https://amzn.to/2B56hq3

• - Beberapa konektor header: https://amzn.to/2nyZg7i

• - Dan bagian tubuh robot yang perlu Anda cetak menggunakan printer 3D

Langkah 5: Rangkaian Robot

Sekarang kami memiliki PCB yang siap dan semua komponen telah dipasang dengan baik, setelah itu kami perlu merakit tubuh robot, prosedurnya sangat mudah jadi ikuti langkah-langkah yang saya tunjukkan, kami perlu menyiapkan setiap kaki satu sisi dan untuk membuat satu LED kami perlu dua servo motor untuk sendi dan bagian Coxa, Femur dan Tibia yang dicetak dengan bagian pemasang kecil ini.

Tentang bagian tubuh robot Anda dapat mengunduh file STL-nya dari sini.

Memulai dengan servo pertama, letakkan di soketnya dan pegang dengan bautnya, setelah itu putar sumbu servo ke 180° tanpa meletakkan baut untuk pemasang dan pindah ke bagian selanjutnya yaitu Femur untuk menghubungkannya ke Tibia menggunakan sumbu sendi servo pertama dan bagian pemasang. Langkah terakhir untuk menyelesaikan kaki adalah meletakkan sendi kedua yang berarti servo kedua untuk memegang bagian ketiga dari kaki yaitu bagian Coxa.

Sekarang ulangi hal yang sama untuk semua kaki untuk mendapatkan empat kaki yang siap. Setelah itu ambil chassis atas dan letakkan sisa servo di soketnya lalu hubungkan setiap kaki ke servo yang sesuai. Hanya ada satu bagian cetak terakhir yaitu chassis bawah robot di mana kita akan menempatkan papan sirkuit kita

Langkah 6: Aplikasi Android

Mengenai aplikasi android ini memungkinkan Anda untuk

terhubung ke robot Anda melalui Bluetooth dan membuat gerakan maju dan mundur serta belok kiri dan kanan, ini juga memungkinkan Anda untuk mengontrol warna lampu 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 Pengujian

Sekarang kami hampir siap untuk menjalankan robot namun kami perlu mengatur sudut sendi terlebih dahulu, jadi unggah kode pengaturan yang memungkinkan Anda untuk menempatkan setiap servo di posisi yang benar dengan melekatkan servo di 90 derajat jangan lupa untuk menghubungkan baterai DC 7V untuk menjalankan robot.

Selanjutnya kami perlu mengunggah program utama untuk mengontrol robot menggunakan aplikasi android. Keduanya dapat Anda unduh dari tautan ini:

- Kode penyesuaian servo: tautan unduhan - Program utama robot labah-labah: tautan unduhan

Setelah mengunggah kode saya telah menghubungkan layar OLED untuk menampilkan senyuman robot Cozmo yang telah saya buat dalam kode utama.

Seperti yang Anda lihat para sahabat pada gambar di atas, robot mengikuti semua instruksi yang dikirim dari ponsel saya dan masih ada beberapa perbaikan lain untuk dilakukan agar jauh lebih baik.

Kode

• Kode utama Robot Labah-Labah

Kode utama Robot Labah-LabahArduino

/************************************************************************************************************************************************************************ * - Author : BELKHIR Mohamed * * - Profession : (Electrical Ingineer) MEGA DAS owner * * - Main purpose : Industrial Application * * - Copyright (c) holder : All rights reserved * * - License : BSD 2-Clause License * * - Date : 20/04/2017 * * ***********************************************************************************************************************************************************************/ /*********************************** NOTE **************************************/ // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: // * Redistributions of source code must retain the above copyright notice, this // list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED /* */ #include <Servo.h> //to define and control servos #include <FlexiTimer2.h>//to set a timer to manage all servos #include <Wire.h> #include <Adafruit_SSD1306.h> #include <Adafruit_GFX.h> // OLED display TWI address #define OLED_ADDR 0x3C Adafruit_SSD1306 display(-1); /* Servos --------------------------------------------------------------------*/ //define 12 servos for 4 legs Servo servo[4][3]; //define servos' ports const int servo_pin[4][3] = { {11, 12, 13}, {2, 4, 7}, {14, 15, 16},{8, 9, 10} };/* Size of the robot ---------------------------------------------------------*/ const float length_a = 50; const float length_b = 77.1; const float length_c = 27.5; const float length_side = 71; const float z_absolute = -28; /* Constants for movement ----------------------------------------------------*/ 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; /* variables for movement ----------------------------------------------------*/ volatile float site_now[4][3]; //real-time coordinates of the end of each leg volatile float site_expect[4][3]; //expected coordinates of the end of each leg float temp_speed[4][3]; //each axis' speed, needs to be recalculated before each movement float move_speed; //movement speed float speed_multiple = 1; //movement speed multiple const 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, for automatic rest //functions' parameter const float KEEP = 255; //define PI for calculation const float pi = 3.1415926; /* Constants for turn --------------------------------------------------------*/ //temp length const 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); //site for turn const 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; // Use this variable to read each caractere received through serial port String data=""; void setup() { Serial.begin(9600); display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR); display.clearDisplay(); display.display(); delay(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]; } } //start servo service FlexiTimer2::set(20, servo_service); FlexiTimer2::start(); //initialize servos servo_attach(); stand(); // delay(3000); // sit(); // delay(3000); // stand(); // delay(3000); happy(); delay (random (500, 1000)); cierra(); delay (150); enfado(); delay (random (1000, 3000)); cierra(); delay (150); entorna(); delay (random (1000, 3000)); cierra(); delay (150); enfado1(); delay (random (1000, 3000)); cierra(); delay (150); triste(); delay (random (1000, 3000)); cierra(); delay (150); abre(); delay (random (500, 3000)); cierra(); delay (150); happy(); delay (random (500, 1000)); } void loop() { while(Serial.available()) // While serial data are available we store it { 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(); else data+=SerialData; } if(data=="f") // If the stored data is forward movement { cierra(); delay (150); happy(); step_forward(1); } if(data=="p") // If the stored data is backward movement { cierra(); delay (150); triste(); step_back(1); } if(data=="l") // If the stored data is to turn left the car { cierra(); delay (150); enfado1(); turn_left(1); } if(data=="m") // If the stored data is to turn right the car { cierra(); delay (150); enfado(); turn_right(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].attach(servo_pin[i][j]); delay(100); } } } void servo_detach(void) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 3; j++) { servo[i][j].detach(); delay(100); } } } void sit(void) { move_speed = stand_seat_speed; for (int leg = 0; leg < 4; leg++) { set_site(leg, KEEP, KEEP, z_boot); } wait_all_reach(); } /* - stand - blocking function ---------------------------------------------------------------------------*/ 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(); } /* - spot turn to left - blocking function - parameter step steps wanted to turn ---------------------------------------------------------------------------*/ void turn_left(unsigned int step) { move_speed = spot_turn_speed; while (step-- > 0) { if (site_now[3][1] == y_start) { //leg 3&1 move 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 move 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 turn to right - blocking function - parameter step steps wanted to turn ---------------------------------------------------------------------------*/ void turn_right(unsigned int step) { move_speed = spot_turn_speed; while (step-- > 0) { if (site_now[2][1] == y_start) { //leg 2&0 move 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 move 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(); } } } /* - go forward - blocking function - parameter step steps wanted to go ---------------------------------------------------------------------------*/ void step_forward(unsigned int step) { move_speed = leg_move_speed; while (step-- > 0) { if (site_now[2][1] == y_start) { //leg 2&1 move 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(); move_speed = 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(); move_speed = leg_move_speed; 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 move 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(); move_speed = 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(); move_speed = leg_move_speed; 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(); } } } /* - go back - blocking function - parameter step steps wanted to go ---------------------------------------------------------------------------*/ void step_back(unsigned int step) { move_speed = leg_move_speed; while (step-- > 0) { if (site_now[3][1] == y_start) { //leg 3&0 move 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(); move_speed = 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(); move_speed = leg_move_speed; 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 move 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(); move_speed = 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(); move_speed = leg_move_speed; 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(); } } } // add by RegisHsu void 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] - i, 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; float y_tmp; float z_tmp; move_speed = 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]; move_speed = body_move_speed; for (int j = 0; j < i; j++) { set_site(2, turn_x1, turn_y1, 50); wait_all_reach(); set_site(2, turn_x0, turn_y0, 50); wait_all_reach(); } set_site(2, x_tmp, y_tmp, z_tmp); wait_all_reach(); move_speed = 1; body_left(15); } else { body_left(15); x_tmp = site_now[0][0]; y_tmp = site_now[0][1]; z_tmp = site_now[0][2]; move_speed = body_move_speed; for (int j = 0; j < i; j++) { set_site(0, turn_x1, turn_y1, 50); wait_all_reach(); set_site(0, turn_x0, turn_y0, 50); wait_all_reach(); } set_site(0, x_tmp, y_tmp, z_tmp); wait_all_reach(); move_speed = 1; body_right(15); } } void hand_shake(int i) { float x_tmp; float y_tmp; float z_tmp; move_speed = 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]; move_speed = body_move_speed; for (int j = 0; j < i; j++) { set_site(2, x_default - 30, y_start + 2 * y_step, 55); wait_all_reach(); set_site(2, x_default - 30, y_start + 2 * y_step, 10); wait_all_reach(); } set_site(2, x_tmp, y_tmp, z_tmp); wait_all_reach(); move_speed = 1; body_left(15); } else { body_left(15); x_tmp = site_now[0][0]; y_tmp = site_now[0][1]; z_tmp = site_now[0][2]; move_speed = body_move_speed; for (int j = 0; j < i; j++) { set_site(0, x_default - 30, y_start + 2 * y_step, 55); wait_all_reach(); set_site(0, x_default - 30, y_start + 2 * y_step, 10); wait_all_reach(); } set_site(0, x_tmp, y_tmp, z_tmp); wait_all_reach(); move_speed = 1; body_right(15); } } void head_up(int i) { set_site(0, KEEP, KEEP, site_now[0][2] - i); set_site(1, KEEP, KEEP, site_now[1][2] + i); set_site(2, KEEP, KEEP, site_now[2][2] - i); set_site(3, KEEP, KEEP, site_now[3][2] + i); wait_all_reach(); } void head_down(int i) { set_site(0, KEEP, KEEP, site_now[0][2] + i); set_site(1, KEEP, KEEP, site_now[1][2] - i); set_site(2, KEEP, KEEP, site_now[2][2] + i); set_site(3, KEEP, KEEP, site_now[3][2] - i); wait_all_reach(); } /* - microservos service /timer interrupt function/50Hz - when set site expected,this function move the end point to it in a straight line - temp_speed[4][3] should be set before set expect site,it make sure the end point move in a straight line,and decide move speed. ---------------------------------------------------------------------------*/ void servo_service(void) { sei(); static float alpha, 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]; else site_now[i][j] = site_expect[i][j]; } cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]); polar_to_servo(i, alpha, beta, gamma); } rest_counter++; } /* - set one of end points' expect site - this founction will set temp_speed[4][3] at same time - non - blocking function ---------------------------------------------------------------------------*/ 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]; float length = 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(); } 

Bagian Kustom dan Enclosure

Skematik