아두이노로만드는 4 족보행로봇

Size: px

Start display at page:

Download "아두이노로만드는 4 족보행로봇"

동해 용
4 years ago
Views:

1 아두이노로만드는 4 족보행로봇

2 목 차 Ⅰ. 아두이노및 C프로그래밍기초 아두이노소개 아두이노개발환경구축 C 프로그래밍기초 실습 #1 디지털출력 (LED제어) 실습 #2 - 디지털입력 ( 스위치입력 ) 실습 #3 디스플레이 (7세그먼트) 실습 #4 아날로그입력 ( 조이스틱모듈 ) 실습 #5 센서 실습 #6 통신 / 블루투스 실습 #7 모터 실습 #8 LCD 모듈 Ⅱ. 보행로봇시스템구현 보행로봇의개요 로봇기구해석 보행궤적생성 로봇시스템구현 Trot 보행구현프로그래밍 Trot 보행구현프로그래밍 ( 블루투스원격조종 )

3 1. 아두이노 (Arduino) 소개 이탈리아에서디자인수업의목적으로손쉽게사용할수있도록개발된 오픈소스형소형컴퓨터 Atmel 社의마이크로컨트롤러를기반으로구성 아두이노 (Arduino) = 마이크로컨트롤러보드 + 프로그램개발환경 + 인터넷공개포럼 공식포럼 :

4 아두이노 Mega 2560 Overview - Atmel 社의 ATmega2560 마이크로컨트롤러기반의아두이노보드 - 주요사양 마이크로컨트롤러동작전압입력전압 ( 허용 ) 입력전압 ( 한계 ) ATmega2560 5V 7-12V 6-20V 디지털 I/O 핀 54 개 (15 개는 PWM 출력기능 ) 아날로그입력핀 핀당 DC 전류 16 개 20 ma 3.3V 핀전류 50 ma Flash Memory 256 KB (8KB 는부트로더 ) SRAM EEPROM 클럭속도크기무게 8 KB 4 KB 16 MHZ (L) * 53.3(W) 37g - 2 -

5 2. 아두이노개발환경구축 통합개발환경 (Integration Development Environment) 이란? 마이크로컨트롤러시스템개발에필요한하드웨어연결및설정, 프로그래밍작성, 다운로드및디버깅등의작업을손쉽게할수있도록, 관련기능이통합되어있는소프트웨어개발도구또는환경. 통합개발환경소프트웨어설치? - 공식포럼 ( 에서 IDE 소프트웨어를다운로드

6 - 다운로드받은파일을실행하고, 프로그램설치를진행

7 보드연결및설정 - 아두이노메가보드를 PC 에연결 - 아두이노 IDE 실행 - 5 -

8 - 보드종류및포트설정 아두이노보드를다른 PC 에서연결해제한뒤다시연결할때포트번호나보드정보가 바뀔수있으므로, 프로그램컴파일이나보드업로딩이안뒬경우, 반드시확인해야함

9 기본프로그램작성및컴파일, 업로드 - 아두이노 IDE 실행후, 아래예제를입력함. // 아두이노처음시작하기 : 시리얼통신으로메시지받기 void setup() Serial.begin(9600); // 아두이노와통신시작 void loop() Serial.println("Hello Everyone!!"); delay(1000); // 메시지출력 // 1 초기다리기 - 컴파일 버튼클릭 업로드 버튼클릭 - 시리얼모니터 버튼클릭 출력결과확인 - 7 -

10 3. C 프로그래밍기초 C 언어는? 1972 년, 벨연구소 (Bell Lab.) 데니스리치가개발한프로그래밍언어로서, UNIX OS 를 운용하기위해사용되었다가점차적으로널리사용되어현재까지사용됨 C 언어의특징 - 문법이간결하고, 명백함 - 구조화가잘되어있어, 광범위한확장성을지님 - 하드웨어밀착제어가가능하면서도, 사용자편리성이우수함 C 언어학습의필요성 현재 ICT 기술의발전에힘입어, 컴퓨터프로그래밍기술이하드웨어에성능에대한제약을 거의받지않게되면서, 사용자편리성이우수한고급형프로그래밍언어 (Java, C#, Python, VB 등 ) 의사용이증대되고있다. 프로그래밍언어는적용되는시스템환경과상황에맞도록선택되어야하며, 특정 프로그래밍언어가가장좋다. 라는기준은없다. 현재기계공학분야에서주로다루는하드웨어및장비시스템은아직까지주로 C 프로그래밍을기반으로개발되어진다. C 언어는하드웨어성능이뛰어나지않은시스템으로부터고성능시스템에이르기까지적용될 수있도록, 컴파일러환경이다양하게구축되어있으며, 시스템의성능을최대한활용할수 있도록직관적인문법구조로이루어져있다. C 프로그래밍은타프로그래밍언어가갖는기본특성을모두담고있어, 프로그래밍습득에 필요한선수학습으로서매우가치가있는프로그래밍언어이다. 프로그래밍문법이란? 사람의언어와동일하게, 기본적인표현방법과규칙을명시한것. 컴퓨터는문법이 약간이라도틀리면, 동작이아예수행되지않으므로, 기본적인프로그래밍문법을이해하는 것이가장중요하다

11 데이터 : 상수, 변수 - 상수 : 프로그램이실행되기전에미리선언되어, 프로그램실행중값이바뀌지않는데이터 - 변수 : 프로그램이실행되기전데이터형을미리선언한후, 프로그램실행중필요에따라변할수있는데이터 ( 또는메모리공간 ) - 데이터형 : 변수저장되는데이터의종류 ( 정수, 실수, 문자등 ) 유형 크기 ( 바이트 ) 범위 int ~ unsigned int 2 0 ~ long ~ unsigned long float E+38 ~ E+38 double E+38 ~ E+38 boolean 1 true(1) 또는 False(0) char ~ 127 byte 1 0 ~ 255 데이터형별크기는아두이노기준임 연산자 : 데이터의처리를위해사용되는산술, 논리연산을수행하는기능을가진기호 유형 기호 산술연산자 +, -, *, /, %, ++, -- 대입연산자 =, +=, -=, *=, /=, %= 관계연산자 >, >=, <, <=, ==,!= 논리연산자 &&,,! 비트연산자 &,, ^, ~, <<, >> - 9 -

12 제어문 : 프로그램의실행흐름을조건에따라여러가지로나누거나바꾸는기능 유형형식사용예 if 문 if( 조건식 1) 수행문 1; else if( 조건식 2) 수행문 2; else 수행문 3; int x = 5; if(x >10) printf( x 는 10 보다크다 ); else if(x >= 5) printf( x 는 5 보다크거나같다 ); else printf( x 는 5 보다작다 ); for 문 for( 초기값 ; 조건식 ; 증감치 ) 반복수행문 ; for(int n=1 ; n<=10 ; n++) // 1 부터 10 숫자출력 printf( %d, n); while 문 while( 반복조건식 ) 수행문 ; int n=1; while(n <= 10) // 1 부터 10 숫자출력 printf( %d, n); n++; 기타 break, continue, return, do ~ while switch ~ case goto - 함수 (Function) : 프로그램에서반복수행되는부분 ( 또는기능 ) 을별도의기능으로묶어둔것 포인터 (Pointer) : 데이터또는함수가실행되는메모리주소값 (Memory Address) 배열 (Array) : 동일한형태의변수또는데이터를한꺼번에지정한것 구조체 (Structure) : 서로다른데이터형을하나의개념체로묶어서다루는것

13 C 프로그래밍기본예제 - 홀 / 짝수판별 // 변수에저장된숫자가짝수인지홀수인지판단 ( 변수와상수, if 문 ) int number = 1; int result = 0; void setup() Serial.begin(9600); // 아두이노와통신시작 void loop() result = number % 2; // number 를 2 로나눈나머지를 result 에저장 (0 또는 1) if(result == 0) Serial.println("EVEN"); else Serial.println("ODD"); while(1); // 나머지가 0 이면짝수 // 나머지가 1 이면홀수 - 숫자카운트 // 숫자카운트하기 (for 문 ) void setup() Serial.begin(9600); void loop() for(int n=0 ; n<=10 ; n++) Serial.print("Number : "); Serial.println(n); delay(1000); while(1); // 메시지출력 // 숫자출력

14 4. 실습 #1 디지털출력 (LED 제어 ) LED 1 개 On/Off 제어 - LED 회로연결 - 아두이노프로그램작성 // LED Blink 기본예제 void setup() // 13번핀출력초기화 pinmode(13, OUTPUT); void loop() digitalwrite(13, HIGH); delay(1000); digitalwrite(13, LOW); delay(1000); // LED 켜기 // 1초기다리기 // LED 끄기 // 1초기다리기

15 LED 8 개 On/Off 제어 - LED 회로연결 - 아두이노프로그램작성 // LED 순차적으로켜고끄기 void setup() // 출력핀초기화 for(int pin=6 ; pin <= 13 ; pin++) pinmode(pin, OUTPUT); void loop() for(int pin=6 ; pin <= 13 ; pin++) digitalwrite(pin, HIGH); delay(500); digitalwrite(pin, LOW);

5. 실습 #2 디지털입력 ( 스위치입력 ) 스위치 1 개연결 - 스위치회로연결 ( 실습 #1 에추가연결 ) - 아두이노프로그램작성 // 버튼입력할때 LED 켜지기 int buttonpin = 5; int ledpin = 13; int buttonstate = 0; // 입력핀번호 // 출력핀번호 // 버튼상태저장 void setup()

16 5. 실습 #2 디지털입력 ( 스위치입력 ) 스위치 1 개연결 - 스위치회로연결 ( 실습 #1 에추가연결 ) - 아두이노프로그램작성 // 버튼입력할때 LED 켜지기 int buttonpin = 5; int ledpin = 13; int buttonstate = 0; // 입력핀번호 // 출력핀번호 // 버튼상태저장 void setup() pinmode(ledpin, OUTPUT); // 출력핀 pinmode(buttonpin, INPUT); // 입력핀모드 void loop() buttonstate = digitalread(buttonpin); // 버튼입력 // 버튼상태확인 if(buttonstate == LOW) digitalwrite(ledpin, HIGH); else digitalwrite(ledpin, LOW); // LED ON // LED OFF

17 QUEST 1. 실습 #2 의회로를이용해서, LED 동작을여러가지로바꿔보시오. 2. 실습 #2 의회로를이용해서, 스위치를누를때마다 LED 점등위치가바뀌도록 프로그램을작성해보시오

18 6. 실습 #3 디스플레이 (7 세그먼트 ) 7 세그먼트 (FND Flexible Numerical Display) 란? - LED 를점등을이용해서숫자 (0 ~ F) 및간단한기호를표현할수있는디스플레이장치 - 공통단자의극성에따라서캐소드 (K) 형또는에노드 (A) 형으로구분됨

7 세그먼트 1 개연결 - 7 세그먼트회로연결 7 세그먼트의 a~h 단자를디지털핀 6~13 에순서대로연결 - 아두이노프로그램작성 // for 문을이용한 FND 출력예제 int ledpin[8] = 6, 7, 8, 9, 10, 11, 12, 13; byte numtable[10] = B00111111, // 0

19 7 세그먼트 1 개연결 - 7 세그먼트회로연결 7 세그먼트의 a~h 단자를디지털핀 6~13 에순서대로연결 - 아두이노프로그램작성 // for 문을이용한 FND 출력예제 int ledpin[8] = 6, 7, 8, 9, 10, 11, 12, 13; byte numtable[10] = B , // 0 B , // 1 B , // 2 B , // 3 B , // 4 B , // 5 B , // 6 B , // 7 B , // 8 B // 9 ; boolean segment = false; 다음장에계속

20 void setup() for(int i=0 ; i < 8 ; i++) pinmode(ledpin[i], OUTPUT); void loop() // 숫자증가 : 0 ~ 9 for(int Num=0 ; Num<=9 ; Num++) // 해당숫자를 FND에표시 for(int i=0 ; i<8 ; i++) // 해당비트를추출 segment = ( ( B << i ) & numtable[num] ) >> i; digitalwrite(ledpin[i], segment); delay(1000); QUEST 1. 실습 #3 의회로를이용해서, 7 세그먼트의동작을바꿔보시오. 2. 실습 #3 을기초로, 여러개의 7 세그먼트를연결해서, 다양한숫자값을표현하시 오

21 7. 실습 #4 아날로그입력 ( 조이스틱모듈 ) 아두이노보드아날로그입력단자및사양 아두이노보드아날로그출력단자및사양 PWM(Pulse Width Modulation, 펄스폭변조 ) 이란? - 빠르게 ON/OFF 되는신호의 ON 시간의비율 (Duty Ratio) 를조정하여, 아날로그출력과유사한출력효과를만드는것

22 아날로그조이스틱입력 - 조이스틱모듈연결 - 아두이노프로그램작성 // 아날로그입력값을전압으로표현하기 void setup() Serial.begin(9600); // 시리얼모니터초기화 void loop() int sensorvalue = analogread(a0); long voltage = map(sensorvalue, 0, 1023, 0, 500); Serial.print("Digital :"); Serial.print(sensorValue); Serial.print(" Voltage :"); Serial.println((float)voltage/100); // 아날로그 A0 입력받기 // 디지털값출력 // 전압값출력 delay(500); // 시간딜레이 0.5 초 - 프로그램업로드 시리얼모니터실행 조이스틱모듈조작, 값변화확인

23 8. 실습 #5 센서 조도센서 - 센서결선 - 아두이노프로그램작성 // CDS 센서와 LED int ledpin = 13; int sensorpin = 0; void setup() pinmode(ledpin, OUTPUT); void loop() int sensor = analogread(sensorpin); digitalwrite(ledpin, HIGH); delay(sensor); digitalwrite(ledpin, LOW); delay(sensor);

readtemperature(); if( isnan(humivalue) isnan(tempvalue) ) Serial.println("Failed"); return; Serial.print("Humidity:"); Serial.

24 온습도센서모듈 - 센서결선 - 아두이노프로그램작성 #include <DHT.h> DHT dht(2, DHT11); // 온습도센서모듈설정 void setup() Serial.begin(9600); Serial.println("DHT11 measure start"); dht.begin(); // 온습도센서모듈시작 void loop() float humivalue = dht.readhumidity(); float tempvalue = dht.readtemperature(); if( isnan(humivalue) isnan(tempvalue) ) Serial.println("Failed"); return; Serial.print("Humidity:"); Serial.print(humiValue); Serial.println("%RH"); Serial.print("Temperature:"); Serial.print(tempValue); Serial.println("C"); Serial.println(); delay(2000);

25 9. 실습 #6 통신 / 블루투스 블루투스모듈 (HC-06) 결선 블루투스모듈 (HC-06) 설정 - 아두이노프로그램작성 #include <SoftwareSerial.h> SoftwareSerial BT(12, 13); void setup() Serial.begin(9600); BT.begin(9600); void loop() if (BT.available()) Serial.write(BT.read()); if (Serial.available()) BT.write(Serial.read());

26 - 블루투스이름, PIN 설정

27 - 25 -

28 - 26 -

29 - 27 -

30 블루투스로원격 LED 오 / 오프제어 - 아두이노프로그램작성 #include <SoftwareSerial.h> int ledpin = 13; SoftwareSerial BT(2, 3); void setup() BT.begin(9600); pinmode(ledpin, OUTPUT); void loop() if(bt.available()) char a = (char)bt.read(); if(a == '1') digitalwrite(ledpin, HIGH); else if(a == '0') digitalwrite(ledpin, LOW); - 원격조정테스트 ( 채팅앱이용 )

31 - 원격조정테스트 ( 컨트롤러앱이용 )

10. 실습 #7 모터 서보모터란? 모터와모터의위치측정센서, 주변제어부를포함하여모터의위치, 속도를제어할수있도록구 성된제어시스템을서보제어시스템이라고하며, 이때사용되는모터를통상, 서보모터라고칭한다. 서보모터구동 - 모터결선 ( 실습 #4 에추가연결 ) - 아두이노프로그램작성 #include <Servo.

32 10. 실습 #7 모터 서보모터란? 모터와모터의위치측정센서, 주변제어부를포함하여모터의위치, 속도를제어할수있도록구 성된제어시스템을서보제어시스템이라고하며, 이때사용되는모터를통상, 서보모터라고칭한다. 서보모터구동 - 모터결선 ( 실습 #4 에추가연결 ) - 아두이노프로그램작성 #include <Servo.h> Servo myservo; void setup() myservo.attach(12); myservo.write(0); delay(1000); void loop() int sensorvalue = analogread(a0); int angle = map(sensorvalue, 0, 1023, 0, 180); myservo.write(angle); delay(50);

11. 실습 #8 LCD 모듈 LCD ( Liquid Crystal Display) - 디지털기기의기본적인디스플레이장치로널리사용됨 - 지정된문자 ( 숫자 ) 만표시할수있는텍스트 LCD와문자및그림출력이가능한그래픽 LCD로구분됨. LCD 구동 - LCD 결선 (I2C 통신방식 ) - 아두이노프로그램작성 #include <Wire.

33 11. 실습 #8 LCD 모듈 LCD ( Liquid Crystal Display) - 디지털기기의기본적인디스플레이장치로널리사용됨 - 지정된문자 ( 숫자 ) 만표시할수있는텍스트 LCD와문자및그림출력이가능한그래픽 LCD로구분됨. LCD 구동 - LCD 결선 (I2C 통신방식 ) - 아두이노프로그램작성 #include <Wire.h> #include <LiquidCrystal_I2C.h> // I2C 통신용 // LCD 제어용 LiquidCrystal_I2C lcd(0x3f, 16, 2); // LCD 초기세팅 (0x3F) void setup() lcd.init(); lcd.backlight(); lcd.setcursor(0,1); Serial.begin(9600); // LCD 초기화 // LCD 백라이트 ON // LCD 커서위치 다음장에계속

34 void loop() // 시리얼통신으로전송된데이터가있으면데이터표시함. if (Serial.available()) delay(100); lcd.clear(); while (Serial.available() > 0) lcd.write(serial.read()); - 시리얼모니터입력창에메시지입력후 LCD 에표시되는지확인 Hello World! - LCD 글자명암조정 ( 글자가안보이거나, 화면이검게나오는경우 )

35 Ⅱ. 보행로봇시스템구현 1. 보행로봇의개요 보행로봇이란? - 인간이나동물의다리 ( 관절 ) 구조를지니면서이와유사한보행방법을모방한 이동방식을사용하는로봇시스템 보행로봇연구의배경및필요성 - 바퀴또는궤도형태의운송수단이접근할수없는지형에서의운송수단 - 다양한환경 ( 험지, 우주, 해저 ) 에서의활용

36 Ⅱ. 보행로봇시스템구현 4족보행로봇 - 4개의다리구조를지닌보행로봇 - 다양한형태의보행로봇중, 적절한동작안정성및시스템복잡도, 기술난이도임 - 실제동물의경우역시, 빠른속도, 안정성, 적응성을가진동물은 4족동물임 견마형 4족보행로봇 - 4족보행로봇은관절의형태에따라 Insect Type(Reptile Type : 파충류형 ) 과 Mammal Type( 포유류 ) 으로분류됨. - Insect Type은무릎관절이 Hip 관절의높이와같거나위에있음. 다리는몸을기준으로옆으로벌어지는형태를띄고실제곤충과같은방법으로보행을함. - Mammal Type은무릎관절이 Hip 관절의아래에위치하므로다리가앞뒤로움직임. - Insect Type은느리지만안정적인보행, Mammal Type은빠르고동적인보행을보임. Insect Type Mammal Type

Ⅱ. 보행로봇시스템구현 견마형 4 족의보행패턴 - Work : 네발이각각 1개씩앞으로내딛어져서이동하는보행패턴을말한다.

한개의다리가공중에서앞으로내딛어질때, 나머지 3다리는몸체를지지하며지면을앞으로밀어내어몸체를이동시킨다.

37 Ⅱ. 보행로봇시스템구현 견마형 4 족의보행패턴 - Work : 네발이각각 1개씩앞으로내딛어져서이동하는보행패턴을말한다. Walk는 4족동물의이동방법중가장안정적인보행방법이다. 한개의다리가공중에서앞으로내딛어질때, 나머지 3다리는몸체를지지하며지면을앞으로밀어내어몸체를이동시킨다. 이때로봇 ( 동물 ) 이넘어지지않기위해서는지지하는세발의지지점을이은삼각형내에로봇의무게중심이위치해야한다. Walk - Trot : 대각선에위치한발이한쌍이되어움직이는보행방법이다. 대각의한쌍의발이몸체 를지지하며앞으로이동시키는동안다른발들을앞으로내딛게된다. 따라서 Trot 은 Walk 에 비해이동속도는빠르나, 안정성은낮다. Trot - Gallop : 앞발과뒷발이각각쌍을이루어보행하는방법으로, 보행중 4발이전부공중에있는 Flight Phase가존재하게된다. 발이땅에닿을때에는쌍을이루는두발중왼쪽발과오른발이약간의시간차이를두고땅에닿게되며, 시간차이가없는것을 Bound라고한다. 또한발의지면접촉순서에따라 Transverse Gallop과 Rotary Gallop으로구분지을수있으며, 한보행주기내에 Flight Phase가 2회일경우인 Fast Gallop과 1회인경우인 Slow Gallop으로도구분지을수있다. Gallop Bound

38 Ⅱ. 보행로봇시스템구현 2. 로봇기구해석 기구부개요 좌표계설정

Ⅱ. 보행로봇시스템구현 DH 파라미터 (Craig) joint (i) α i-1 a i-1 d i θ i θ 0 1 0 H/2 0 90 0 2 90 W/2 0 -θ 1-90 3 0 0 -Bw 0 0 4 90 Bh -L1 θ 2 0 5 0 L2 0 θ

Rear Leg joint (i) α i-1 a i-1 d i θ i θ 0 1 0 H/2 0 90 0 2 90 -W/2 0 -θ 1-90 3 0 0 -Bw 0 0 4 90 Bh -L1 θ 2 0 5 0 L2 0 θ 3 0 joint (i)

39 Ⅱ. 보행로봇시스템구현 DH 파라미터 (Craig) joint (i) α i-1 a i-1 d i θ i θ H/ W/2 0 -θ Bw Bh -L1 θ L2 0 θ 3 0 joint (i) Left Front Leg α i-1 a i-1 d i θ i θ H/ W/2 0 -θ Bw Bh -L1 θ L2 0 θ 3 0 Left Rear Leg joint (i) α i-1 a i-1 d i θ i θ H/ W/2 0 -θ Bw Bh -L1 θ L2 0 θ 3 0 joint (i) Right Front Leg α i-1 a i-1 d i θ i θ H/ W/2 0 -θ Bw Bh -L1 θ L2 0 θ 3 0 Right Rear Leg

40 Ⅱ. 보행로봇시스템구현 3. 보행궤적생성 동역학모델링 -LIPM (Linear Inverted Pendulum Model) : 거동하는로봇을질점형태의역진자로단순화한모델 LIPM 에의한궤적생성 - LIPM 의뉴턴방정식은아래와같다. - Z 축방향의거동은일정하다고가정하고, 식을정리 (Z = Hz( 보행높이 )) - 미분방정식을계산하고, 걸음시간 (Ts, 한걸음을내딛는주기 ) 동안의초기위치, 속도값이 주어졌을때, 미분방정식의특수해를구할수있음. - 최종위치값및속도값이아래의조건을만족한다고가정하면, 초기속도값은아래와같음

41 Ⅱ. 보행로봇시스템구현 순기구학 / 역기구학 -LIPM 에의한이동궤적은실제지면과접촉된발끝의이동에의한상대적인움직임을나타내며, 발의움직임은로봇을구성하는각관절의운동으로이루이진다. 따라서위에서생성된보행궤적을발의상대좌표로변환하고, 상대좌표에대한각관절의각도를계산하는역기구학식이필요하다. - 순기구학 : 왼쪽, 오른쪽다리를간단한 Link 구조형태로간주하여순기구학식수립

42 - 역기구학 - θ 1 계산 : 어깨관절의오프셋 L1 을고려하여보행궤적에따른 θ 1 을계산함. Ⅱ. 보행로봇시스템구현 - θ 2, θ 3 계산 : DH 변환행렬을이용해, 목표위치를다리관절좌표로변환후, 다리관절을 2Link 로간 구하여역기구학식을계산함. DH 변환행렬은발끝좌표를몸통좌표로변환하므로, 몸통좌표에서발끝좌표로변환하기위해 DH 변환 행렬의역행렬을구한뒤, 몸통좌표 ( 목표위치 ) 를곱하여발끝좌표를계산한다

43 Ⅱ. 보행로봇시스템구현 - MATLAB 을이용한 DH 변환행렬계산 ( 발끝좌표계 -> 몸통좌표계 )

44 Ⅱ. 보행로봇시스템구현 - 좌표변환식 ( 몸통좌표 -> 발끝좌표 )

45 Ⅱ. 보행로봇시스템구현 - θ 2, θ 3 계산 ( 최종 ) : 2Link 관절의역기구학해석과동일 앞발관절각도 뒷발관절각도

46 Ⅱ. 보행로봇시스템구현 4. 로봇시스템구현 기구부조립 - 구성요소 Link1 ( 2 개 / 우 2 개 ) Link2 4 개 ) Link3 (4 개 ) Frame1 (2 개 ) Frame2 (1 개 ) Foot (4 개 ) - 서보모터조립 1. 서보모터를 0도또는 180도방향으로완전히돌린후, 반대방향으로 90도회전시켜중앙각도를맞춘다. 2. 서보모터의중앙위치가로봇관절 Link 의일직선상태와맞도록조립한다

47 Ⅱ. 보행로봇시스템구현 전기 / 제어시스템구성 - 구성요소 MEGA2560 HS-485HB 서보모터 (12 개 ) (4 개 ), 충전기 (2 개 ) HC-06 블루투스모듈 브레드보드, 점퍼선등 - 블루투스연결

48 Ⅱ. 보행로봇시스템구현 - 서보모터 / 배터리파워연결 - 서보모터위치별연결핀번호 다리 ( 링크 ) Left Front Right Front Left Rear Right Rear Link Link Link

49 Ⅱ. 보행로봇시스템구현 5. Trot 보행구현프로그래밍 #include <math.h> #include <Servo.h> #define SAMPLING_TIME_MSEC 10 #define _1MSEC_TO_1SEC #define _1SEC_TO_1MSEC enum _COORD COORD_X, COORD_Y, COORD_Z, MAX_COORD_NUM ; enum _LEG LF, RF, LR, RR, MAX_LEG_NUM ; enum _JOINT JOINT_1, JOINT_2, JOINT_3, MAX_JOINT_NUM ; Servo servo[max_leg_num][max_joint_num]; // 로봇파라미터 double h = 188; double w = 110; double l1 = 0; double l2 = 65;

50 Ⅱ. 보행로봇시스템구현 double l3 = 70; double bh = 23.5; double bw = 20; double pf[max_leg_num][3] = h / 2, w / 2, -(bh + l2 + l3), h / 2, -w / 2, -(bh + l2 + l3), -h / 2 - bw, w / 2, -(bh + l2 + l3), -h / 2 - bw, -w / 2, -(bh + l2 + l3) ; // 몸통중심으로부터각발끝의 위치값 // 보행파라미터 double s = 20; double stride = 40; // 보폭 double stride_h = 20; // 보폭 ( 수직 ) double velocity = 40; // 보행속도 40 mm/s unsigned long ulperiod_step = (unsigned long)(stride / velocity * _1SEC_TO_1MSEC); double g = 9.81; double hz = 160; double w1 = sqrt(g / hz); double ts = ulperiod_step * _1MSEC_TO_1SEC; double xb[max_leg_num] = 0, ; double yb[max_leg_num] = 0, ; double zb[max_leg_num] = 0, ; // 아두이노핀설정 int PInServo[MAX_LEG_NUM][MAX_JOINT_NUM] = 14, 12, 11, 7, 6, 5, 10, 9, 8, 4, 3, 2; // 모터제어설정 int itargetangle[max_leg_num][max_joint_num] = 0,, ; int ioffsetangle[max_leg_num][max_joint_num] = 90-10, 90-5, 90-5, , 90, , , 90, , , 90, 90-5; int dir[max_leg_num][max_joint_num] = 1, 1, 1, -1, -1, -1, 1, 1, 1, -1, -1, -1; // 제어관련변수 unsigned long ulpretime_step = 0; int iseq = 0; void setup() // 시리얼통신초기화 Serial.begin(9600); // 서보모터초기화

51 Ⅱ. 보행로봇시스템구현 for (int ileg = 0 ; ileg < MAX_LEG_NUM ; ileg++) for (int iaxis = 0 ; iaxis < MAX_JOINT_NUM ; iaxis++) int angle = 0 + ioffsetangle[ileg][iaxis]; servo[ileg][iaxis].attach(pinservo[ileg][iaxis]); servo[ileg][iaxis].write(angle); // 2 초 Delay delay(2000); // 현재시간저장 ulpretime_step = millis(); void loop() unsigned long ulcurtime = millis(); int ileg; // 동작스텝변경 if (ulcurtime - ulpretime_step >= ulperiod_step) ulpretime_step = ulcurtime; iseq++; if (iseq > 3) iseq = 2; // 보행시간 ( t = 0 ~ Step Period ) double t = (double)(((ulcurtime - ulpretime_step) % ulperiod_step) / SAMPLING_TIME_MSEC ) * (SAMPLING_TIME_MSEC * _1MSEC_TO_1SEC); if (iseq == 0) // 보행준비

52 Ⅱ. 보행로봇시스템구현 for (int ileg = 0 ; ileg < MAX_LEG_NUM ; ileg++) // 보행궤적생성 xb[ileg] = pf[ileg][coord_x]; yb[ileg] = pf[ileg][coord_y]; zb[ileg] = pf[ileg][coord_z] + s * sin( (PI / 2.) * ( (t * _1SEC_TO_1MSEC) / (double)ulperiod_step) ); // 보행주기설정 // hz = zb[ileg]; // w1 = sqrt(g / hz); else if (iseq == 1) // 보행반걸음 ts = (double)ulperiod_step * _1MSEC_TO_1SEC; double xo[max_leg_num] = stride / 2, -stride / 2, -stride / 2, stride / 2; for (int ileg = 0 ; ileg < MAX_LEG_NUM ; ileg++) double xo_dot = ( (1 + exp(w1 * ts)) / (1 - exp(w1 * ts)) ) * w1 * xo[ileg]; double c1 = 0.5 * (xo[ileg] + (1 / w1) * xo_dot); double c2 = 0.5 * (xo[ileg] - (1 / w1) * xo_dot); // 보행궤적생성 if ((t * _1SEC_TO_1MSEC) >= ulperiod_step / 2.) xb[ileg] = pf[ileg][coord_x] + ( c1 * exp(w1 * t) + c2 * exp(-w1 * t) ); if ( ileg == RF ileg == LR ) zb[ileg] = pf[ileg][coord_z] + s + stride_h * sin( (PI) * ( ((t * _1SEC_TO_1MSEC) - (double)(ulperiod_step / 2.)) / (double)(ulperiod_step / 2.)) ); yb[ileg] = pf[ileg][coord_y];

53 Ⅱ. 보행로봇시스템구현 else if (iseq == 2) // 보행완걸음 1 ts = (double)ulperiod_step * _1MSEC_TO_1SEC; double xo[max_leg_num] = -stride / 2, stride / 2, stride / 2, -stride / 2; for (int ileg = 0 ; ileg < MAX_LEG_NUM ; ileg++) double xo_dot = ( (1 + exp(w1 * ts)) / (1 - exp(w1 * ts)) ) * w1 * xo[ileg]; double c1 = 0.5 * (xo[ileg] + (1 / w1) * xo_dot); double c2 = 0.5 * (xo[ileg] - (1 / w1) * xo_dot); // 보행궤적생성 xb[ileg] = pf[ileg][coord_x] + ( c1 * exp(w1 * t) + c2 * exp(-w1 * t) ); yb[ileg] = pf[ileg][coord_y]; if ( ileg == LF ileg == RR ) zb[ileg] = pf[ileg][coord_z] + s + stride_h * sin( (PI) * ( (t * _1SEC_TO_1MSEC) / (double)(ulperiod_step)) ); else if (iseq == 3) // 보행완걸음2 ts = (double)ulperiod_step * _1MSEC_TO_1SEC; double xo[max_leg_num] = stride / 2, -stride / 2, -stride / 2, stride / 2; for (int ileg = 0 ; ileg < MAX_LEG_NUM ; ileg++) double xo_dot = ( (1 + exp(w1 * ts)) / (1 - exp(w1 * ts)) ) * w1 * xo[ileg]; double c1 = 0.5 * (xo[ileg] + (1 / w1) * xo_dot); double c2 = 0.5 * (xo[ileg] - (1 / w1) * xo_dot); // 보행궤적생성 xb[ileg] = pf[ileg][coord_x] + ( c1 * exp(w1 * t) + c2 * exp(-w1 * t) ); yb[ileg] = pf[ileg][coord_y]; if ( ileg == RF ileg == LR ) zb[ileg] = pf[ileg][coord_z] + s + stride_h * sin( (PI) * ( (t * _1SEC_TO_1MSEC) /

54 Ⅱ. 보행로봇시스템구현 (double)(ulperiod_step)) ); // PLOT 확인 Serial.println(xb[LF]); // 역기구학계산 : ileg = LF; // Left Front Leg double th1 = -atan((yb[ileg] - (w / 2)) / (zb[ileg])) - asin(l1 / (sqrt((yb[ileg] - (w / 2)) * (yb[ileg] - (w / 2)) + zb[ileg] * zb[ileg]))); double xf3 = (yb[ileg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2) ) - (2 * bh * (cos(th1 - PI / 2) * cos(th1 - PI / 2)) + 2 * bh * (sin(th1 - PI / 2) * sin(th1 - PI / 2)) + w * cos(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) + (zb[ileg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); double yf3 = bw - h / 2 + xb[ileg]; double zf3 = (2 * l1 * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * l1 * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * sin(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) - (zb[ileg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) + (yb[ileg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); double th3 = acos((xf3 * xf3 + yf3 * yf3 - l2 * l2 - l3 * l3) / (2 * l2 * l3)); double th2 = atan(yf3 / xf3) - atan((l3 * sin(th3)) / (l2 + l3 * cos(th3))); itargetangle[ileg][joint_1] = (int)(th1 * (180. / PI)); itargetangle[ileg][joint_2] = (int)(th2 * (180. / PI)); itargetangle[ileg][joint_3] = (int)(th3 * (180. / PI)); ileg = RF; // Right Front Leg th1 = -atan((yb[ileg] + (w / 2)) / (zb[ileg])) + asin(l1 / (sqrt((yb[ileg] + (w / 2)) * (yb[ileg] + (w / 2)) + zb[ileg] * zb[ileg]))); xf3 = (yb[ileg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * bh * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * bh * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * cos(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) + (zb[ileg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); yf3 = bw - h / 2 + xb[ileg]; zf3 = (yb[ileg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (zb[ileg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * l1 * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * l1 * sin(th1 - PI / 2) *

55 Ⅱ. 보행로봇시스템구현 sin(th1 - PI / 2) - w * sin(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))); th3 = acos((xf3 * xf3 + yf3 * yf3 - l2 * l2 - l3 * l3) / (2 * l2 * l3)); th2 = atan(yf3 / xf3) - atan((l3 * sin(th3)) / (l2 + l3 * cos(th3))); itargetangle[ileg][joint_1] = (int)(th1 * (180. / PI)); itargetangle[ileg][joint_2] = (int)(th2 * (180. / PI)); itargetangle[ileg][joint_3] = (int)(th3 * (180. / PI)); ileg = LR; // Left Rear Leg th1 = -atan((yb[ileg] - (w / 2)) / (zb[ileg])) - asin(l1 / (sqrt((yb[ileg] - (w / 2)) * (yb[ileg] - (w / 2)) + zb[ileg] * zb[ileg]))); xf3 = (yb[ileg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * bh * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * bh * sin(th1 - PI / 2) * sin(th1 - PI / 2) + w * cos(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) + (zb[ileg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); yf3 = bw + h / 2 + xb[ileg]; zf3 = (2 * l1 * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * l1 * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * sin(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) - (zb[ileg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) + (yb[ileg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); th3 = -acos((xf3 * xf3 + yf3 * yf3 - l2 * l2 - l3 * l3) / (2 * l2 * l3)); th2 = atan(yf3 / xf3) - atan((l3 * sin(th3)) / (l2 + l3 * cos(th3))); itargetangle[ileg][joint_1] = (int)(th1 * (180. / PI)); itargetangle[ileg][joint_2] = (int)(th2 * (180. / PI)); itargetangle[ileg][joint_3] = (int)(th3 * (180. / PI)); ileg = RR; // Right Front Leg th1 = -atan((yb[ileg] + (w / 2)) / (zb[ileg])) + asin(l1 / (sqrt((yb[ileg] + (w / 2)) * (yb[ileg] + (w / 2)) + zb[ileg] * zb[ileg]))); xf3 = (yb[ileg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * bh * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * bh * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * cos(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))) + (zb[ileg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)); yf3 = bw + h / 2 + xb[ileg]; zf3 = (yb[ileg] * sin(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) *

56 Ⅱ. 보행로봇시스템구현 sin(th1 - PI / 2)) - (zb[ileg] * cos(th1 - PI / 2)) / (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2)) - (2 * l1 * cos(th1 - PI / 2) * cos(th1 - PI / 2) + 2 * l1 * sin(th1 - PI / 2) * sin(th1 - PI / 2) - w * sin(th1 - PI / 2)) / (2 * (cos(th1 - PI / 2) * cos(th1 - PI / 2) + sin(th1 - PI / 2) * sin(th1 - PI / 2))); th3 = -acos((xf3 * xf3 + yf3 * yf3 - l2 * l2 - l3 * l3) / (2 * l2 * l3)); th2 = atan(yf3 / xf3) - atan((l3 * sin(th3)) / (l2 + l3 * cos(th3))); itargetangle[ileg][joint_1] = (int)(th1 * (180. / PI)); itargetangle[ileg][joint_2] = (int)(th2 * (180. / PI)); itargetangle[ileg][joint_3] = (int)(th3 * (180. / PI)); // 모터구동 ConvAngle((int**)iTargetAngle); void ConvAngle(int** ptarangle) for (int ileg = 0 ; ileg < MAX_LEG_NUM ; ileg++) for (int iaxis = 0 ; iaxis < MAX_JOINT_NUM ; iaxis++) int iangle = *( ptarangle + (ileg * MAX_JOINT_NUM) + iaxis ); int imotorangle = ioffsetangle[ileg][iaxis] + (dir[ileg][iaxis] * iangle); 향에맞도록변환 // 목표각도를모터구동방 // 모터구동 servo[ileg][iaxis].write(imotorangle); void ConvAngle(int ileg, int iaxis, double dangle) int imotorangle = ioffsetangle[ileg][iaxis] + (dir[ileg][iaxis] * (int)(dangle * (180. / PI))); servo[ileg][iaxis].write(imotorangle);

57 Ⅱ. 보행로봇시스템구현 6 Trot 보행구현프로그래밍 ( 블루투스원격조종 ) 아두이노프로그래밍 프로그래밍코드는별도자료첨부 컨트롤앱 (App) 설정

58 Ⅱ. 보행로봇시스템구현

아두이노로만드는인형뽑기장치

아두이노로만드는인형뽑기장치 목 차 Ⅰ. 아두이노및 C프로그래밍기초 ------------------------------------------------------- 1 1. 아두이노소개 ------------------------------------------------------------------------- 1 2. 아두이노개발환경구축 --------------------------------------------------------------