Arduino Bitirme Projeleri Arduino ile Örümcek Robot Yapımı By Muhammet Özcan Posted on 30 Nisan 2017 44 min read 2 5 16,549 Paylaş ! Facebook Paylaş ! Twitter Paylaş ! Google+ Paylaş ! Reddit Paylaş ! Pinterest Paylaş ! Linkedin Paylaş ! Tumblr Merhaba Arkadaşlar bu yazımda sizlere Sakarya Üniversitesi Mekatronik Mühendisliği Bölümü öğrencileri İsmet Ünallı ve Ferhat Özcan arkadaşlarımızın bitirme tezi olan örümcek robot yapımını anlatacağım. İlerleyen teknoloji ile birlikte insan hayatında robotların yeri ve önemi gün geçtikçe önem kazanmaktadır. Geçmişten günümüze baktığımızda robotların yaratılış amaçlarında farklılıklar olduğu görülebilmektedir. Gerek otonom olarak çalışabilme özelliği gerek bir operatör kontrolünde çalışabilme özelliğine sahip olmaları vazgeçilmez olabilmelerine olanak sağlamaktadır. Gün geçtikçe insanlığın yerini alan bu makinalar daha hızlı, uzun ömürlü ve yüksek çalışma kapasitelerine sahiptirler. Günümüz teknolojileri ile üretilen robotlar savunma sanayi, mayın tarama, casusluk vs. alanlarda görev yapabilmeye başlamışlardır. Bu çalışma alanlarında görev yapan robotlardan bir tanesi de örümcek robotlardır. Örümcek robotlar Hexapod(6 bacaklı), Quadruped(4 bacaklı) ve benzer türlere sahip robotlar olarak ayrılabilmektedir. Projenin Çalışması Esin kaynağımız olan 4 bacaklı örümcek robotumuz toplam 4 adet ayağı bulunan ve her bacak 3 serbestlik derecesine sahip şekilde tasarlanmıştır.Genel olarak bakıldığında 12 adet serbestliğe sahiptir. Robotumuz da eklemlerin hareketi servo motorlar ile sağlanmaktadır. Servo motorlarımızın kontrolü Arduino Mega ile sağlanmıştır. Robotumuz 3 boyutlu tasarım ortamında SolidWorks’ te çizilmiş, gerekli analizler ve gerekli bütün montajlar yine aynı ortam üzerinde yapılmıştır. Bunun yanında çizilen parçalarımız 3D yazıcıdan çıkartılmıştır ve malzemelerimiz PLA ile basılmıştır. Robotumuz için Konum analizi, Hız analizi ve İvme analizi hesaplanmış ve SolidWorks’ de yapılan analiz ve olası durumlarla karşılaştırılmıştır. Mekanik Tasarım ve Analiz Bu proje 3 boyutlu tasarım programı olan SolidWorks’ de yapılmış ve analiz bu ortamda gerçekleştirilmiştir. Tasarım çalışmamız toplam 226 adet montaj ilişkisinden oluşmaktadır. Bunun yanında 169 adet toplam bileşen sayısı bulunmaktadır ayrıca bütün montaj parçaları ile 261.11 gram dır.Mekanik analiz çalışmamızda tasarım aşaması sonrası parçalarımızı SolidWorks’de analiz yaparak 3D baskıdan çıkan parçalarımıza belirli kuvvetler uygulamak ve sabit, dönme vs. noktalarımızın belirlenmesi ve sonuçlara göre değerlendirme yaparak tasarım üzerinde oynamalar yapabilmeye olanak kılmaktır. Bunun için statik ve dinamik kuvvetler incelenmiştir. Elektronik Tasarım Fritzing programında hazırlanmış olan sistemimizin görsel şeması oluşturulmuştur. Bu görsel ile sistemimiz hakkında, kullanılan pinler hakkında, servo motor ve buton hakkında bilgi verilmiş olup Arduino’nun 12 motor için harici bir besleme ile desteklenmesi gerektiği gibi durumlar görülmektedir. Malzeme ve Fiyat Listesi Arduino Mega 12 x Servo Motor(ES08MAII) Lipo Pil Lion Pil Lion Şarj Ünitesi 9 V’luk Pil 9 V’luk Pil Yuvası Jumper Kablolar (Erkek – Erkek) Jumper Kablolar (Dişi – Erkek) Sonuçlar ve Öneriler Yapılan çalışmada özgün ve yeniliklere açık olmak öncülüğünde sistemimiz literatür ve gerekli çalışmalar ile tasarım aşamasında başlanmıştır. Çalışmamız diğer örümcek tiplerine göre daha yüksek boy, daha yüksek bir dayanım ve sağlık içeren malzemeden üretilecek olmasıdır. Bu şekilde gerek hobi, gerek askeri ya da istenilen çalışma alanına göre sağlık, görsellik, ucuzluk ile avantajlı olması beklenmektedir. Deneysel çalışmalarda Solidworks programında gerekli analizler yapılmış ve yükün olası gelebilecek noktaları analize dahil edilerek çalışmalar yürütülmüştür. Çalışmanın sonucu olarak istenildiği şekilde hareket sağlanmış ve kalite, sağlamlık ve görsel anlamda istenilen başarıyı yakalamıştır. Bunun yanında bir tez olarak teknik açıdan edindiğimiz bilgi ve tecrübü dahilinde grup çalışması, sistemin incelenmesi, zamanın verimli kullanılması gibi bir çok faktörün elde edilmesinde öncülük sağlanmıştır. Yapılan çalışmanın ileri bir safha durumlarında gerekli ekipmanlara montaj için uygun olması ayrıca robotumuz plastikten tasarlanmış olması böylece hem fiyat hem talep açısından dikkat çekiyor olması geleceğe yönelik bir talep artmasını sağlayabilmektedir. Bu çalışmanın ileri safhaları olarak değerlenmesi ile var olan sisteme talep durumuna göre kamera modülü takılması, uzaktan kontrol mekanizmaları ile kontrol edilebilmesi, durum neticesine göre yapısal tasarım değişikliğine gidilerek istenilen amaca yönelik bir robot yapılması olarak düşünülebilir. Öneriler olarak robotumuzun kullanım alanlarına göre tespiti önceden yapılıp o amaç doğrultusunda tasarım ve üretim faaliyetlerinin devam edilmesidir. Ayrıca pil çok önem arz etmekle birlikte tasarım aşamasında uygun pilin tespiti ve bu doğrultuda tasarımın şekillenmesi sağlanmalıdır. Bununla birlikte robotun çalışacağı ortam, maruz kalacağı koşullar göz önüne alınarak malzeme seçimi yapılmalı ve yoğunluk bu açıdan değerlendirilerek sistem için en uygun olan parçaların üretilebilmesidir. Projenin kodlarını aklınızda bir fikir olması açısından aşağıda paylaştım.Örümcek robotun STL dosyalarına ulaşmak için mesaj atabilirsiniz.Fikir ve sorularınızı çekinmeden sorabilirsiniz.Bir dahaki yazıda görüşmek üzere:) [php] #include <Servo.h> //Servo motor kütüphanesi #include <FlexiTimer2.h> //12 servo için tanımlamalar Servo servo[4][3]; const int servo_pin[4][3] = { {2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13} }; /*Robotun ölçüleri ———————————————————*/ const float length_a = 58; //ara parça const float length_b = 78.5; //bacak uzunluğu const float length_c = 27.5; //servo motor ölçüsü const float length_side = 131; //örümceğin boyu const float z_absolute = -36; //bacağın uç noktası ile gövde arası mesafe const float z_default = -70, z_up = -45, z_boot = z_absolute; //örümceğin normal yerden yüksekliği const float x_default = 50, x_offset =0; //bacağın o uç noktası ile o bacağa bağlı motorun kısmına olan x ekseni const float y_start = 0, y_step = 50; //ileriye atılan bacak mesafesi volatile float site_now[4][3]; //Gerçek zamanlı robotun o anki koordinatları volatile float site_expect[4][3]; //Her bacağın olması gereken yani beklenen koordinatları float temp_speed[4][3]; //her eksenin hızı, hesaplanmalıdır float move_speed; //hareket hızı float speed_multiple = 1; //çoklu hareket hızı 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//50 sn de bir otomatik resetleme //parametre fonksiyonu const float KEEP = 255; //PI tanımlama 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; /* —————————————————————————*/ /* – setup function —————————————————————————*/ void setup() { //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() { Serial.println("Stand"); stand(); delay(2000); Serial.println("Step forward"); step_forward(5); delay(2000); Serial.println("Step back"); step_back(5); delay(2000); Serial.println("Turn left"); turn_left(5); delay(2000); Serial.println("Turn right"); turn_right(5); delay(2000); Serial.println("Hand wave"); hand_wave(3); delay(2000); Serial.println("Hand wave"); hand_shake(3); delay(2000); Serial.println("Sit"); sit(); delay(5000); } /* – 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 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); } } /* – 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); } [/php]
