이 프로젝트 정보
얘들 아! 다음은 "거미 로봇" 또는 "네 발 달린 로봇"이라고도 하는 "크롤러 로봇"인 이러한 종류의 매우 놀라운 전자 프로젝트를 만드는 동안 단계별로 안내하는 새로운 자습서입니다.
모두가 로봇 기술의 고속 진화를 알아차렸기 때문에 로봇 공학 및 로봇 제작에 대한 더 높은 수준으로 여러분을 안내하기로 결정했습니다. 우리는 여러분이 전자 제품에 조금 익숙해지고 자신의 프로젝트를 발명할 수 있도록 하기 위해 몇 가지 기본적인 전자 프로젝트와 PICTO92 라인 팔로워 로봇과 같은 기본 로봇을 만드는 것으로 시작했습니다.
다른 수준으로 이동하여 개념의 기본 로봇인 이 로봇으로 시작했지만 프로그램에 대해 더 깊이 들어가면 조금 복잡해집니다. 그리고 이러한 가제트는 웹 스토어에서 너무 비싸기 때문에 우리는 이 단계별 지침을 제공합니다. 자신만의 Spiderbot을 만드는 방법을 안내해 드립니다. .
이 프로젝트는 로봇의 외관을 개선하기 위해 JLCPCB에서 주문한 맞춤형 PCB를 얻은 후 특별히 만들기에 매우 편리하며 이 가이드에는 크롤러를 쉽게 만들 수 있도록 충분한 문서와 코드가 있습니다.
우리는 이 프로젝트를 단 7일 만에 만들었습니다. 하드웨어 제작과 조립을 마치는 데 2일, 그리고 코드와 안드로이드 앱을 준비하는 데 5일 만에 말입니다. 그것을 통해 로봇을 제어하기 위해. 시작하기 전에 먼저 살펴보겠습니다.
배울 내용: <울> 프로젝트 기능에 따라 올바른 구성 요소 선택
<울> 선택한 모든 구성 요소를 연결하는 회로 만들기
<울> 모든 프로젝트 부분 조립
<울> 로봇 균형 조정
<울> Android 앱 사용. Bluetooth를 통해 연결하고 시스템 조작 시작
1단계:"거미로봇"이란 무엇입니까?
이름에서 알 수 있듯이 우리 로봇은 sipder 움직임의 기본 표현이지만 8개의 다리 대신 4개의 다리만 사용하기 때문에 정확히 동일한 신체 움직임을 수행하지는 않습니다.
네발이라고도 함 로봇은 4개의 다리가 있고 이 다리를 사용하여 움직이기 때문에 각 다리의 움직임은 다른 다리와 연관되어 로봇 몸체의 위치를 식별하고 로봇 몸체의 균형을 제어합니다.
다리가 달린 로봇은 바퀴 달린 로봇보다 지형을 더 잘 다루며 다양하고 동물적인 방식으로 움직입니다. 그러나 이는 다리가 있는 로봇을 더 복잡하게 만들고 많은 제조업체에서 접근하기 어렵게 만듭니다. 또한 서보 모터 또는 스테퍼 모터를 기반으로 하고 둘 다 바퀴 달린 로봇에 사용할 수 있는 DC 모터보다 비싸기 때문에 4족 전신을 만들기 위해 제작자가 지출해야 하는 제작 비용과 높은 비용도 있습니다.
장점
네 개의 다리가 수동적 안정성을 허용하거나 능동적으로 위치를 조정하지 않고 서 있을 수 있는 능력을 허용하기 때문에 네 발 달린 동물은 자연에서 많이 발견될 것입니다. 로봇도 마찬가지다. 다리가 4개인 로봇은 다리가 많은 로봇보다 저렴하고 단순하지만 여전히 안정성을 얻을 수 있습니다.
2단계:서보 모터가 주요 액츄에이터
Wikipedia에 정의된 서보 모터는 각도 또는 선형 위치, 속도 및 가속도를 정밀하게 제어할 수 있는 회전식 액츄에이터 또는 선형 액츄에이터입니다.[1]위치 피드백을 위해 센서에 결합된 적절한 모터로 구성됩니다. 또한 비교적 정교한 컨트롤러가 필요하며 종종 서보 모터용으로 특별히 설계된 전용 모듈이 필요합니다.
서보 모터라는 용어는 폐쇄 루프 제어 시스템에 사용하기에 적합한 모터를 지칭하는 데 자주 사용되지만 서보 모터는 특정 종류의 모터가 아닙니다.
일반적으로 제어 신호는 구형파 펄스 열입니다. 제어 신호의 공통 주파수는 44Hz, 50Hz 및 400Hz입니다. 양의 펄스 폭은 서보 위치를 결정하는 것입니다. 약 0.5ms의 양의 펄스 폭은 서보 혼이 왼쪽으로 최대한 편향되게 합니다(일반적으로 해당 서보에 따라 약 45~90도). 약 2.5ms ~ 3.0ms의 양의 펄스 폭은 서보가 가능한 한 오른쪽으로 편향되도록 합니다. 약 1.5ms의 펄스 폭은 서보가 0도에서 중립 위치를 유지하도록 합니다. 출력 고전압은 일반적으로 2.5V에서 10V 사이입니다(일반적으로 3V). 출력 저전압 범위는 -40mV ~ 0V입니다.
3단계:PCB 제작(JLCPCB 제작)
JLCPCB 소개
JLCPCB(Shenzhen JIALICHUANG Electronic Technology Development Co., Ltd.)는 중국 최대의 PCB 프로토타입 기업이자 빠른 PCB 프로토타입 및 소량 PCB 생산을 전문으로 하는 하이테크 제조업체입니다.
PCB 제조 분야에서 10년 이상의 경험을 가진 JLCPCB는 국내외에 200,000명 이상의 고객을 보유하고 있으며 하루에 8,000건 이상의 PCB 프로토타이핑 및 소량 PCB 생산을 온라인 주문합니다. 연간 생산 능력은 200,000 sq.m입니다. 다양한 1층, 2층 또는 다층 PCB용. JLC는 대규모, 유정 장비, 엄격한 관리 및 우수한 품질을 갖춘 전문 PCB 제조업체입니다.
우리 프로젝트로 돌아가기
PCB를 생산하기 위해 나는 많은 PCB 생산자들의 가격을 비교했고 이 회로를 주문하기 위해 JLCPCB 최고의 PCB 공급 업체와 가장 저렴한 PCB 공급 업체를 선택했습니다. 내가 해야 할 일은 거버 파일을 업로드하고 PCB 두께 색상 및 수량과 같은 일부 매개변수를 설정하기 위해 몇 번의 간단한 클릭만 하면 됩니다. 그런 다음 단 5일 만에 내 PCB를 얻기 위해 2달러를 지불했습니다.
관련 도식의 그림을 보여주듯이 나는 Arduino Nano를 사용하여 전체 시스템을 제어했으며 이 프로젝트를 훨씬 더 좋게 만들기 위해 로봇 거미 모양을 설계했습니다.
여기에서 회로(PDF) 파일을 얻을 수 있습니다. 위의 그림에서 볼 수 있듯이 PCB는 매우 잘 제조되었으며 우리가 디자인한 것과 동일한 PCB 스파이더 모양을 가지고 있으며 모든 레이블과 로고가 납땜 단계에서 저를 안내합니다.
동일한 회로 설계를 주문하려는 경우 여기에서 이 회로에 대한 Gerber 파일을 다운로드할 수도 있습니다.
4단계:재료
이제 이 프로젝트에 필요한 필수 구성 요소를 검토하겠습니다. 제가 말했듯이 저는 Arduino Nano를 사용하여 로봇 4개 다리의 12개 서보 모터를 모두 실행하고 있습니다. 이 프로젝트에는 Cozmo 얼굴을 표시하는 OLED 디스플레이와 안드로이드 앱을 통해 로봇을 제어하는 블루투스 모듈도 포함됩니다.
이러한 종류의 프로젝트를 만들려면 다음이 필요합니다.
<울> - JLCPCB에서 주문한 PCB
<울> - 각 다리에 3개의 서보를 기억하는 12개의 서보 모터 :https://amzn.to/2B25XbG
<울> - 아두이노 나노 하나 :https://amzn.to/2MmZsVg
<울> - HC-06 블루투스 모듈 :https://amzn.to/2B1Z3CY
<울> - OLED 디스플레이 화면 1개 :https://amzn.to/2OySnyn
<울> - 5mm RGB LED :https://amzn.to/2B56hq3
<울> - 일부 헤더 연결:https://amzn.to/2nyZg7i
<울> - 그리고 3D 프린터를 사용하여 인쇄하는 데 필요한 로봇 본체 피스
5단계:로봇 조립
이제 우리는 PCB를 준비하고 모든 구성 요소를 매우 잘 납땜했습니다. 그 후에 로봇 본체를 조립해야 합니다. 절차가 너무 쉽기 때문에 내가 보여주는 단계를 따르십시오. 먼저 각 다리 측면을 준비하고 만들 필요가 있습니다. 하나는 조인트용으로 두 개의 서보 모터가 필요하고 이 작은 부착 부품이 있는 Coxa, Femur 및 Tibia 인쇄 부품이 필요합니다.
여기에서 로봇의 STL 파일을 다운로드할 수 있습니다.
첫 번째 서보부터 시작하여 소켓에 넣고 나사로 고정한 후 부착용 나사를 배치하지 않고 서보 축을 180°로 돌리고 경골에 연결할 대퇴골이 있는 다음 부분으로 이동합니다. 첫 번째 서보 조인트 도끼와 어태치 피스를 사용합니다. 다리를 완성하는 마지막 단계는 두 번째 관절을 배치하는 것입니다. 두 번째 서보는 다리의 세 번째 부분인 Coxa 조각을 고정하는 두 번째 서보를 의미합니다.
이제 모든 다리에 대해 동일한 작업을 반복하여 4개의 다리를 준비합니다. 그런 다음 상단 섀시를 잡고 나머지 서보를 소켓에 넣은 다음 각 다리를 적절한 서보에 연결합니다. 우리가 회로 기판을 놓을 하단 로봇 섀시인 마지막으로 인쇄된 부품이 하나만 있습니다.
6단계:Android 앱
Android에 대해 이야기하면 다음을 수행할 수 있습니다.
블루투스를 통해 로봇에 연결하고 전후 이동 및 왼쪽 오른쪽 회전을 수행하면 이 색상환에서 원하는 색상을 선택하여 실시간으로 로봇 조명 색상을 제어할 수도 있습니다.
다음 링크에서 Android 앱을 무료로 다운로드할 수 있습니다. 여기
7단계:Arduino 코드 및 테스트 검증
이제 로봇을 실행할 준비가 거의 완료되었지만 먼저 관절 각도를 설정해야 하므로 서보를 90도에 부착하여 각 서보를 올바른 위치에 놓을 수 있는 설정 코드를 업로드하십시오. 7V 연결을 잊지 마십시오. 로봇을 구동하기 위한 DC 배터리입니다.
다음으로 안드로이드 앱을 사용하여 로봇을 제어하는 메인 프로그램을 업로드해야 합니다. 두 프로그램 모두 다음 링크에서 다운로드할 수 있습니다.
- 스케일링 서보 코드 :다운로드 링크 - 스파이더 로봇 메인 프로그램 :다운로드 링크
코드를 업로드한 후 메인 코드에서 만든 Cozmo 로봇 스마일을 표시하기 위해 OLED 디스플레이를 연결했습니다.
위의 사진에서 볼 수 있듯이 로봇은 내 스마트폰에서 보낸 모든 지침을 따르고 훨씬 더 나은 성능을 위해 수행해야 할 몇 가지 다른 개선 사항을 수행합니다.
<섹션 클래스="섹션 컨테이너 섹션 축소 가능" id="코드"> 코드
<울> 스파이더 로봇 메인 코드
Spider Robot 메인 코드Arduino
<사전>/************************************************ ************************************************** ************************************************** ********************** * - 저자 :BELKHIR Mohamed * * - 직업 :(전기 엔지니어) MEGA DAS 소유자 * * - 주요 목적 :산업용 응용 * * - Copyright (c) 보유자 :All rights reserved * * - 라이선스 :BSD 2조 라이선스 * * - 날짜 :2017-04-20 * * ****************** ************************************************** ************************************************** *****************************************************/ /*********************************** 노트 ************* *************************/// 수정 여부에 관계없이 소스 및 바이너리 형식의 재배포 및 사용은 다음 조건을 충족하는 경우 허용됩니다. 조건이 충족됨:// * 소스 코드의 재배포는 위의 저작권 표시, this// 조건 목록 및 다음 면책조항을 유지해야 합니다.// * 바이너리 형식의 재배포는 복제해야 합니다. 위의 저작권 고지,// 이 조건 목록 및 문서// 및/또는 배포와 함께 제공되는 기타 자료의 다음 부인.// 이 소프트웨어는 저작권 보유자와 기여자가 "있는 그대로" 제공합니다// 그리고 상업성 및 특정 목적에의 적합성에 대한 묵시적 보증을 포함하되 이에 국한되지 않는 모든 명시적 또는 묵시적 보증은 부인됩니다./* */#include //서보를 정의하고 제어하기 위해#include //모든 서보를 관리하기 위한 타이머 설정#include #include #include // OLED 디스플레이 TWI 주소#define OLED_ADDR 0x3CAdafruit_SSD1306 display(-1); /* 서보 ----------------------------------------------- ---------------------*///4개의 다리에 대해 12개의 서보를 정의합니다.Servo 서보[4][3];//서보의 포트 정의 const int servo_pin[4] [3] ={ {11, 12, 13}, {2, 4, 7}, {14, 15, 16},{8, 9, 10} };/* 로봇 크기 ------ -------------------------------------------------- -*/const float l ength_a =50;const float length_b =77.1;const float length_c =27.5;const float length_side =71;const float z_absolute =-28;/* 이동 상수 ---------------- ------------------------------------*/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;/* 이동 변수 --------------- -------------------------------------*/volatile float site_now[4][3]; //각 legvolatile 끝의 실시간 좌표 float site_expect[4][3]; //각 legfloat 끝의 예상 좌표 temp_speed[4][3]; //각 축의 속도, 각 이동 전에 다시 계산해야 합니다.float move_speed; //이동 speedfloat speed_multiple =1; //이동 속도 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, 자동 휴식용//함수의 매개변수 const float KEEP =255;//계산을 위한 PI 정의 const float pi =3.1415926;/* 회전 상수 ------------- -------------------------------------------*///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);//turnconst float turn_x1 =(temp_a - length_side) / 2;const float turn_y1 =y_start + y_step / 2;const float turn_x0 =turn_x1 - temp_b * cos(temp_alpha);const float turn_y0 =temp_b * sin (temp_alpha) - turn_y1 - length_side; const int lightR=3; const int lightG=5; const int lightB=6;int LedR=0;int LedG=0;int LedB=0;char SerialData; // 이 변수를 사용하여 직렬로 수신된 각 문자를 읽습니다. portString data="";void setup() { Serial.begin(9600); display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR); display.clearDisplay(); 디스플레이.디스플레이(); 지연(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]; } } //서보 서비스 시작 FlexiTimer2::set(20,servo_service); FlexiTimer2::start(); //서보 초기화 서보_attach(); 스탠드();// 지연(3000);// 앉아();// 지연(3000);// 스탠드();// 지연(3000); 행복하다(); 지연(임의(500, 1000)); 시에라(); 지연(150); 엔파도(); 지연(랜덤(1000, 3000)); 시에라(); 지연(150); 엔토르나(); 지연(랜덤(1000, 3000)); 시에라(); 지연(150); 엔파도1(); 지연(랜덤(1000, 3000)); 시에라(); 지연(150); 트리스테(); 지연(랜덤(1000, 3000)); 시에라(); 지연(150); 아버(); 지연(임의(500, 3000)); 시에라(); 지연(150); 행복하다(); delay (random (500, 1000));}void loop() { while(Serial.available()) // 직렬 데이터를 사용할 수 있는 동안 저장합니다. { 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(); 그렇지 않으면 데이터 + =직렬 데이터; } if(data=="f") // 저장된 데이터가 정방향 이동인 경우 { cierra(); 지연(150); 행복하다(); step_forward(1); } if(data=="p") // 저장된 데이터가 뒤로 이동하는 경우 { cierra(); 지연(150); 트리스테(); 스텝백(1); } if(data=="l") // 저장된 데이터가 왼쪽으로 도는 경우 car { cierra(); 지연(150); 엔파도1(); turn_left(1); } if(data=="m") // 저장된 데이터가 우회전이라면 car { cierra(); 지연(150); 엔파도(); 우회전(5); } 데이터=""; 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++) { 서보[i][j]. 부착(서보핀[i][j]); 지연(100); } }}보이드 서보 분리(무효){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { 서보[i][j].detach(); 지연(100); } }}좌석 무효(무효){ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_boot); } wait_all_reach();}/* - 스탠드 - 차단 기능 ------------------------------------- --------------------------------------*/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();}/* - 좌회전 지점 - 차단 기능 - 원하는 매개변수 단계 단계 --------------------------- ------------------------------------------------*/ void turn_left(unsigned int step){ move_speed =spot_turn_speed; while (step--> 0) { if (site_now[3][1] ==y_start) { //레그 3&1 이동 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 { //레그 0&2 이동 set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, 턴_x0 + x_offset, 턴_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(); } }}/* - 스팟 우회전 - 차단 기능 - 전환하려는 매개변수 단계 단계 ------------------------------ ---------------------------------------------*/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, 턴_x0 + x_offset, 턴_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 { //레그 1&3 이동 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(); } }}/* - 앞으로 이동 - 차단 기능 - 가고자 하는 매개변수 단계 단계 -------------------------------- -------------------------------------------*/void step_forward(unsigned int 단계){ 이동 속도 =다리 이동 속도; 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(); 이동 속도 =다리 이동 속도; 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 { //레그 0&3 이동 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(); 이동 속도 =다리 이동 속도; 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(); } }}/* - 돌아가기 - 차단 기능 - 가고자 하는 매개변수 단계 단계 -------------------------------- -------------------------------------------*/void step_back(unsigned int 단계){ 이동 속도 =다리 이동 속도; 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(); 이동 속도 =다리 이동 속도; 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 { //레그 1&2 이동 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(); 이동 속도 =다리 이동 속도; 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(); } }}// 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();}무효 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();}무효 hand_wave(int i){ float x_tmp; float y_tmp; float z_tmp; 이동 속도 =1; if (site_now[3][1] ==y_start) { body_right(15); x_tmp =사이트_지금[2][0]; y_tmp =사이트_지금[2][1]; z_tmp =사이트_지금[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 + 2 0, KEEP);// set_site(1, KEEP, y_default - 20, KEEP);// set_site(2, KEEP, y_default + 20, KEEP);// set_site(3, KEEP, y_default - 20, KEEP); // wait_all_reach();// }// move_speed =body_dance_speed;// head_down(30);//}/* - 마이크로서보 서비스 /타이머 인터럽트 기능/50Hz - 사이트 설정이 예상되면 이 함수는 끝점을 그곳으로 이동합니다. 직선으로 - temp_speed[4][3]는 예상 위치를 설정하기 전에 설정해야 하며 끝점이 직선으로 이동하는지 확인하고 이동 속도를 결정합니다. -------------------------------------------------- -------------------------*/무효 서보 서비스(무효){ sei(); 정적 부동 알파, 베타, 감마; 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]; 그렇지 않으면 site_now[i][j] =site_expect[i][j]; } cartesian_to_polar(알파, 베타, 감마, site_now[i][0], site_now[i][1], site_now[i][2]); polar_to_servo(i, 알파, 베타, 감마); } rest_counter++;}/* - 끝점 중 하나를 예상 사이트로 설정 - 이 함수는 temp_speed[4][3]를 동시에 설정합니다. - 비차단 기능 --------------- -------------------------------------------------- ----------*/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 - 현재 사이트[leg][0]; if (y !=KEEP) length_y =y - 현재 사이트[레그][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();} 
