Tentang proyek ini
Quadruped berbasis Arduino! Quadruped adalah singkatan dari four-legged bot, yang pada dasarnya terlihat seperti laba-laba berkaki empat, jadi mari kita pelajari cara laba-laba berjalan dan mencoba menirunya dengan Arduino.
Persediaan:
Langkah 1:Komponen Diperlukan - 1 X Arduino Mega atau Arduino Uno
- 1 X PCB yang Dibor
- 12 X Motor Servo (9g)
- 1 X Sensor Ultrasonik HC-SR04
- LED 4 X RGB
- Karton
Langkah 2:Mempertahankan CG
pusat gravitasi (CG) adalah faktor utama saat berjalan. Pusat gravitasi tetap berada di pusat tubuh untuk menjaga keseimbangan jika CG bergerak keluar dari pusat pada batas-batas tertentu maka keseimbangan akan terpengaruh dan menyebabkan jatuh
Jadi mari kita lihat tentang mempertahankan CG sambil berjalan.
Jika setiap kaki berada di 45 derajat maka CG akan berada di tengah dengan sempurna, Tetapi jika kita menggerakkan kaki mana pun, cg akan bergeser ke sisi itu sehingga menyebabkan jatuh di sisi itu.
Jadi untuk menghindari hal ini kedua ujung kaki dipertahankan pada sudut yang lebih besar dari 45 derajat berdasarkan ukuran bot, sehingga ketiga kaki akan membentuk segitiga, jika CG akan berada di dalamnya dan kaki keempat akan bebas bergerak dan CG akan tetap berada di dalam segitiga.
Langkah 3:Prosedur Berjalan
- Ini adalah posisi awal, dengan dua kaki (C, D) diluruskan di satu sisi, dan dua kaki lainnya (A, B) ditarik ke dalam.
- Kaki kanan atas(B) terangkat dan menjangkau, jauh di depan robot.
- Semua kaki bergeser ke belakang, menggerakkan tubuh ke depan.
- Kaki kiri-belakang (D) terangkat dan melangkah ke depan di samping tubuh. Posisi ini adalah bayangan cermin dari posisi awal.
- Kaki kiri atas(B) mengangkat dan menjangkau, jauh di depan robot.
- Sekali lagi, semua kaki bergeser ke belakang, menggerakkan tubuh ke depan.
- Kaki kanan belakang terangkat(B) dan melangkah kembali ke tubuh, membawa kita kembali ke posisi awal.
Langkah 4:Rencana untuk Quadruped LEGS.pdf TUBUH.pdf
Langkah 5:Konstruksi Tubuh
Bangun tubuh sesuai dengan PDF.
Langkah 6:Koneksi Sirkuit
Buat perisai sendiri sesuai kebutuhan Anda Arduino mega memiliki 15 pin pwm, gunakan 12 di antaranya untuk koneksi servo dan 3 untuk led RBG dan dua pin untuk sensor ultrasonik
Langkah 7:Inisialisasi Servo - Upload program ke arduino mega dan mulai merakit kaki sesuai gambar
#include Servo servo[4][3];//menentukan port servosconst int servo_pin[4][3] ={ {10,11,2}, {3,4 ,5}, {6,7,8}, {9, 12, 13} };void setup(){ //inisialisasi semua servos untuk (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].attach(servo_pin[i][j]); penundaan (20); } }}void loop(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].write(90); penundaan (20); } }}
Langkah 8:Langkah Terakhir
/* Termasuk ----------------------------------------- -------------------------*/#include //untuk mendefinisikan dan mengontrol servos#include // untuk mengatur timer untuk mengatur semua servos#menentukan ledred 46#menentukan ledblue 44#menentukan ledgreen 45/* Servos --------------------------- -----------------------------------------*///menentukan 12 servos untuk 4 kakiServo servo[4][3];//menentukan port servosconst int servo_pin[4][3] ={ {2, 3, 4}, {20, 6, 7}, {8, 9, 17}, { 16, 12, 13} };/* Ukuran robot ------------------------------------ ---------------------*/const float length_a =55;const float length_b =77.5;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;/* ---------------------------------------- -----------------------------------*//* - fungsi pengaturan -------- -------------------------------------------------- -----------------*/pengaturan batal(){ pi nMode(ledred,OUTPUT);pinMode(ledblue,OUTPUT);pinMode(ledgreen,OUTPUT); //mulai serial untuk debug Serial.begin(115200); Serial.println("Robot memulai inisialisasi"); //inisialisasi parameter default 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(); Serial.println("Layanan servo dimulai"); //inisialisasi servos servo_attach(); Serial.println("Servos diinisialisasi"); Serial.println("Inisialisasi Robot Selesai");}void servo_attach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i] [j].attach(pin servo_[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); } }}/* - fungsi loop ------------------------------------------ ----------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Berdiri"); berdiri(); penundaan(2000); analogWrite(dipimpin,0); analogWrite(ledblue,255); Serial.println("Langkah maju"); langkah_maju(5); penundaan(2000); analogWrite(ledblue,0); analogWrite(ledgreen,255); Serial.println("Mundur"); langkah_belakang(5); penundaan(2000); analogWrite(ledgreen,0); analogWrite(dipimpin,255); analogWrite(ledblue,255); Serial.println("Belok kiri"); belok_kiri(5); penundaan(2000); analogWrite(ledgreen,255); analogWrite(dipimpin,0); analogWrite(ledblue,255); Serial.println("Belok kanan"); belok_kanan(5); penundaan(2000); analogWrite(ledgreen,255); analogWrite(dipimpin,255); analogWrite(ledblue,0); Serial.println("Gelombang tangan"); gelombang_tangan(3); penundaan(2000); Serial.println("Gelombang tangan"); jabat tangan(3); penundaan(2000); int x=100; for(int i=0;i<5;i++) { analogWrite(ledgreen,255); analogWrite(ledred,255);//white analogWrite(ledblue,255); penundaan (x); analogWrite(ledgreen,255);//kuning analogWrite(ledred,255); analogWrite(ledblue,0); penundaan (x); analogWrite(ledgreen,255);//cyan analogWrite(ledred,0); analogWrite(ledblue,255); penundaan (x); analogWrite(ledgreen,0); analogWrite(ledred,255);//purple analogWrite(ledblue,255); penundaan (x); analogWrite(ledgreen,0); analogWrite(ledred,255);//red analogWrite(ledblue,0); penundaan (x); analogWrite(ledgreen,0);//blue analogWrite(ledred,0); analogWrite(ledblue,255); penundaan (x); analogWrite(ledgreen,255); analogWrite(dipimpin,0); analogWrite(ledblue,0); //penundaan hijau(x); } analogWrite(hijau led,0); analogWrite(dipimpin,0); analogWrite(ledblue,0); //Serial.println("Menari tubuh"); //body_dance(10);// delay(2000); //Serial.println("Duduk");// duduk(); delay(1000);}/* - duduk - fungsi pemblokiran ------------------------------------- ---------------------------------------*/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();}/* - 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 + 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(); } kecepatan_gerakan =kecepatan_tubuh_tari; head_down(30);}/* - layanan microservos / fungsi interupsi timer/50Hz - ketika situs yang ditetapkan diharapkan, fungsi ini memindahkan titik akhir ke dalam garis lurus - temp_speed[4][3] harus ditetapkan sebelum menetapkan situs yang diharapkan , pastikan titik akhir bergerak dalam garis lurus, dan tentukan 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 harapan 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;}/* - tunggu salah satu titik akhir pindah ke situs yang diharapkan - fungsi pemblokiran ----------------- -------------------------------------------------- --------*/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;}/* - tunggu semua titik akhir pindah ke situs yang diharapkan - fungsi pemblokiran ---- -------------------------------------------------- ---------------------*/void wait_all_reach(void){ for (int i =0; i <4; i++) wait_reach(i);}/* - situs trans dari kartesian ke kutub - model matematika 2/2 ------------------------------------- --------------------------------------*/void cartesian_to_polar(volatil float &alfa, float &beta volatil , float &gamma yang mudah menguap, float yang mudah menguap x, float yang mudah menguap y, float yang mudah menguap z){ //menghitung derajat wz float v, w; w =(x>=0 ? 1 :-1) * (kuadrat(pow(x, 2) + pow(y, 2))); v =w - panjang_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); //hitung x-y-z derajat gamma =(w>=0) ? atan2(y, x) :atan2(-y, -x); // derajat trans pi->180 alfa =alfa / pi * 180; beta =beta / pi * 180; gamma =gamma / pi * 180;}/* - situs trans dari kutub ke mikroservo - peta model matematika ke fakta - kesalahan yang disimpan di eeprom akan ditambahkan ----------------- -------------------------------------------------- --------*/void polar_to_servo(kaki int, alfa mengambang, beta mengambang, gamma mengambang){ if (kaki ==0) { alfa =90 - alfa; beta =beta; gama +=90; } else if (kaki ==1) { alpha +=90; beta =180 - beta; gama =90 - gama; } else if (kaki ==2) { alpha +=90; beta =180 - beta; gama =90 - gama; } else if (kaki ==3) { alfa =90 - alfa; beta =beta; gama +=90; } servo[kaki][0].tulis(alfa); servo[kaki][1].tulis(beta); servo[leg][2].write(gamma);}
Hubungkan pin led
- Itu dia, hewan berkaki empatmu sudah siap!
- Unggah program.
- Hubungkan servo sesuai dengan pin yang ditentukan dalam program.
Kode
laba-labaArduino
/* Termasuk ------------------------------------------------------- ----------------------*/#include //untuk mendefinisikan dan mengontrol servos#include //untuk mengatur pengatur waktu untuk mengatur semua servos # tentukan ledred 46 # tentukan ledblue 44 # tentukan ledgreen 45/* Servos ------------------------------ ---------------------------------------*///menentukan 12 servos untuk 4 kakiServo servo[ 4][3];//menentukan port servosconst int servo_pin[4][3] ={ {2, 3, 4}, {20, 6, 7}, {8, 9, 17}, {16, 12 , 13} };/* Ukuran robot --------------------------------------- ------------------*/const float length_a =55;const float length_b =77.5;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;/* ---------------------------------------- -----------------------------------*//* - fungsi pengaturan -------- -------------------------------------------------- -----------------*/pengaturan batal(){ pi nMode(ledred,OUTPUT);pinMode(ledblue,OUTPUT);pinMode(ledgreen,OUTPUT); //start serial for debug Serial.begin(115200); Serial.println("Robot starts initialization"); //initialize default parameter 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(); Serial.println("Servo service started"); //initialize servos servo_attach(); Serial.println("Servos initialized"); Serial.println("Robot initialization Complete");}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); } }}/* - loop function ---------------------------------------------------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Stand"); stand(); delay(2000); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Step forward"); step_forward(5); delay(2000); analogWrite(ledblue,0); analogWrite(ledgreen,255); Serial.println("Step back"); step_back(5); delay(2000); analogWrite(ledgreen,0); analogWrite(ledred,255); analogWrite(ledblue,255); Serial.println("Turn left"); turn_left(5); delay(2000); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Turn right"); turn_right(5); delay(2000); analogWrite(ledgreen,255); analogWrite(ledred,255); analogWrite(ledblue,0); Serial.println("Hand wave"); hand_wave(3); delay(2000); Serial.println("Hand wave"); hand_shake(3); delay(2000); int x=100; for(int i=0;i<5;i++) { analogWrite(ledgreen,255); analogWrite(ledred,255);//white analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,255);//yellow analogWrite(ledred,255); analogWrite(ledblue,0); delay(x); analogWrite(ledgreen,255);//cyan analogWrite(ledred,0); analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,0); analogWrite(ledred,255);//purple analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,0); analogWrite(ledred,255);//red analogWrite(ledblue,0); delay(x); analogWrite(ledgreen,0);//blue analogWrite(ledred,0); analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,0); //green delay(x); } analogWrite(ledgreen,0); analogWrite(ledred,0); analogWrite(ledblue,0); //Serial.println("Body dance"); //body_dance(10); // delay(2000); //Serial.println("Sit"); // sit(); delay(1000);}/* - sit - blocking function ---------------------------------------------------------------------------*/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 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] - 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 / 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 + 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(); } move_speed =body_dance_speed; head_down(30);}/* - 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);}
spider_fix.inoArduino
// Locate the initial position of legs // RegisHsu 2015-09-09#include Servo servo[4][3];//define servos' portsconst int servo_pin[4][3] ={ {10,11,2}, {3,4,5}, {6,7,8}, {9, 12, 13} };void setup(){ //initialize all servos for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].attach(servo_pin[i][j]); delay(20); } }}void loop(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].write(90); delay(20); } }}
Suku cadang dan penutup khusus
Skema
