1. 메카넘휠을 사용한 모바일베이스의 주행 원리 (1)
메카넘휠은 아래 첫 번째 그림과 같이 림(rim)휠과 여러 개의 롤러(roller)로 구성됩니다.
모터가 회전할 때, 지면과의 마찰에서 롤러는 자유로이 회전하기 때문에 지면에 맞닿은 롤러의 축에 수직인 (주황색 화살표) 방향으로는 속도벡터가 발생하지 않습니다. 대신 지면에 맞닿은 롤러의 축(초록색 화살표) 방향으로만 마찰이 생겨서 속도벡터가 발생합니다. 모바일베이스에 있는 네 개의 메카넘 휠의 속도벡터를 합하면 모바일베이스를 어느 방향이든 원하는 방향으로 이동시킬 수 있습니다.
2. 메카넘휠을 사용한 모바일베이스의 주행 원리 (2)
메카넘휠 모바일베이스는 롤러 축의 각도가 다른 두 종류의 메카넘휠 네 개로 구성되어 있습니다. 이 두 종류의 메카넘 휠을 각각 A타입, B타입이라고 합니다. 메카넘휠이 대각선 방향으로 이동하는 특성을 이용하기 위해 아래 첫 번째 사진과 같이 휠이 설치됩니다. 그리고 아래 두 번째 그림과 같이 각 휠에서 생성되는 속도 벡터들의 합을 통해 모바일베이스의 이동 방향을 결정할 수 있습니다.
3. 메카넘휠을 사용한 모바일베이스의 주행 원리 (3)
메카넘휠 모바일베이스는 4개의 모터 속도를 다르게 동작 시키면서 여러가지 형태로 회전주행이 가능합니다.
4. 메카넘휠을 사용한 모바일베이스의 기본 주행 코딩 실습 (1)
블랙베리에 프로그램을 업로드 한 후 아래 사진을 참조하여 모바일베이스가 이동하는 것을 확인합니다.
아래 그림의 파란색 화살표는 각 휠의 회전 방향이고, 빨간색 화살표는 각 휠의 이동 방향입니다.
loop 에서는 아래와 같이 동작합니다.
(1) 전진하고, 2초간 유지합니다.
(2) 후진하고, 2초간 유지합니다.
(3) 우진하고, 2초간 유지합니다.
(4) 좌진하고, 2초간 유지합니다.
5. 메카넘휠을 사용한 모바일베이스의 기본 주행 코딩 실습 (2)
아래 사진을 참조하여 모바일베이스가 이동하는 것을 확인합니다.
아래 그림의 파란색 화살표는 각 휠의 회전 방향이고, 빨간색 화살표는 각 휠의 이동 방향입니다.
loop 에서는 아래와 같이 동작합니다.
(5) 45도 방향으로 전진하고, 2초간 유지합니다.
(6) 225도 방향으로 후진하고, 2초간 유지합니다.
(7) 315도 방향으로 전진하고, 2초간 유지합니다.
(8) 135도 방향으로 후진하고, 2초간 유지합니다.
(9) 정지하고 5초 대기 후에 다시 반복합니다.
////////////// 모터 구동 관련 선언
#include <Dynamixel2Arduino.h>
#define DXL_SERIAL Serial1
const int DXL_DIR_PIN = 2; // DIR PIN
const float DXL_PROTOCOL_VERSION = 2.0;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
// 컨트롤 테이블 아이템의 이름을 사용하기 위해 이 네임스페이스가 필요함
using namespace ControlTableItem;
////////////// 모터 구동용 변수/상수
#define MOBILE_DXL_ID_FL 0x01 // 좌측 전방 모터 아이디
#define MOBILE_DXL_ID_FR 0x02 // 우측 전방 모터 아이디
#define MOBILE_DXL_ID_BL 0x03 // 좌측 후방 모터 아이디
#define MOBILE_DXL_ID_BR 0x04 // 우측 후방 모터 아이디
#define NORMAL_MODE 0x00 // drive mode의 방향 모드
#define REVERSE_MODE 0x01
// FindMobileBaseServos 함수에서 찾을 모터 아이디들, 값이 중복 없이 정렬되어있어야 함
const uint8_t MOBILE_DXL_ID_CNT = 4;
const uint8_t MOBILE_DXL_IDS[MOBILE_DXL_ID_CNT] = {MOBILE_DXL_ID_FL,
MOBILE_DXL_ID_FR,
MOBILE_DXL_ID_BL,
MOBILE_DXL_ID_BR};
// sync write 용 상수, 구조체 정의 및 인스턴스화
const int32_t GOAL_VELOCITY_VALUE = 88; // 모터에 적용할 속도 값
const uint16_t GOAL_VELOCITY_ADDR = 104; // sync write start 주소
const uint16_t GOAL_VELOCITY_SIZE = 4; // sync write data 길이
typedef struct sw_data{ // 모터에 데이터를 쓰기 위한 구조체 정의
int32_t goal_velocity;
} __attribute__((packed)) sw_data_t;
sw_data_t sw_data[MOBILE_DXL_ID_CNT]; // 모터 개수만큼 인스턴스 생성
DYNAMIXEL::InfoSyncWriteInst_t sw_infos; // syncwrite 정보 인스턴스 생성
// syncWrite할 모터들 정보 인스턴스를 모터 개수만큼 생성
DYNAMIXEL::XELInfoSyncWrite_t info_xels_sw[MOBILE_DXL_ID_CNT];
////////////// 메인 프로그램
void setup()
{
Serial.begin(115200);
dxl.begin(57600);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// 모터가 모두 있는지 확인될 때 까지 진행하지 않음
while(!FindMobileBaseServos()) {}
// 각 모터에 설정
for (int i = 0 ; i < sizeof(MOBILE_DXL_IDS)/sizeof(uint8_t) ; i++) {
// 토크 끄기
dxl.writeControlTableItem(TORQUE_ENABLE, MOBILE_DXL_IDS[i], 0);
// 모드 설정
if (MOBILE_DXL_IDS[i] == MOBILE_DXL_ID_FL ||
MOBILE_DXL_IDS[i] == MOBILE_DXL_ID_BL) { // 좌측 바퀴
dxl.writeControlTableItem(DRIVE_MODE, MOBILE_DXL_IDS[i], NORMAL_MODE);
} else if (MOBILE_DXL_IDS[i] == MOBILE_DXL_ID_FR ||
MOBILE_DXL_IDS[i] == MOBILE_DXL_ID_BR) { // 우측 바퀴
dxl.writeControlTableItem(DRIVE_MODE, MOBILE_DXL_IDS[i], REVERSE_MODE);
}
dxl.writeControlTableItem(OPERATING_MODE, MOBILE_DXL_IDS[i], 1);
// 토크 켜기
dxl.writeControlTableItem(TORQUE_ENABLE, MOBILE_DXL_IDS[i], 1);
}
// sync write 준비
sw_infos.packet.p_buf = nullptr; // nullptr을 전달하면 내부 버퍼를 사용
sw_infos.packet.is_completed = false; // false로 초기화
sw_infos.addr = GOAL_VELOCITY_ADDR; // 컨트롤 테이블에 sync write를 시작하는 주소
sw_infos.addr_length = GOAL_VELOCITY_SIZE; // sync write하는 데이터 길이
sw_infos.p_xels = info_xels_sw; // sync write 할 모터 정보
sw_infos.xel_count = 0; // sync write 할 모터 개수
sw_data[0].goal_velocity = 0; // 모터에 write 할 데이터 초기화
sw_data[1].goal_velocity = 0;
sw_data[2].goal_velocity = 0;
sw_data[3].goal_velocity = 0;
// 모터 정보 리스트에 모터 아이디 설정, 데이터 포인터 지정
for(int i = 0; i < MOBILE_DXL_ID_CNT; i++) {
info_xels_sw[i].id = MOBILE_DXL_IDS[i];
info_xels_sw[i].p_data = (uint8_t*)&sw_data[i].goal_velocity;
sw_infos.xel_count++;
}
sw_infos.is_info_changed = true; // sync write 정보가 변경됨을 설정
}
void loop()
{
// 모든 모터 전진 : 전진
SetMobileGoalVelocityForSyncWrite( GOAL_VELOCITY_VALUE, GOAL_VELOCITY_VALUE,
GOAL_VELOCITY_VALUE, GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(2000);
// 모든 모터 후진 : 후진
SetMobileGoalVelocityForSyncWrite( -GOAL_VELOCITY_VALUE, -GOAL_VELOCITY_VALUE,
-GOAL_VELOCITY_VALUE, -GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(2000);
// 전방좌측과 후방우측 모터 전진, 전방우측과 후방좌측 모터 후진 : 우로 평행 이동 (우진)
SetMobileGoalVelocityForSyncWrite( GOAL_VELOCITY_VALUE, -GOAL_VELOCITY_VALUE,
-GOAL_VELOCITY_VALUE, GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(2000);
// 전방좌측과 후방우측 모터 후진, 전방우측과 후방좌측 모터 전진 : 좌로 평행 이동 (좌진)
SetMobileGoalVelocityForSyncWrite( -GOAL_VELOCITY_VALUE, GOAL_VELOCITY_VALUE,
GOAL_VELOCITY_VALUE, -GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(2000);
// 전방좌측과 후방우측 모터 전진, 전방우측과 후방좌측 모터 정지 : 45도 방향으로 전진
SetMobileGoalVelocityForSyncWrite( GOAL_VELOCITY_VALUE, 0,
0, GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(2000);
// 전방좌측과 후방우측 모터 후진, 전방우측과 후방좌측 모터 정지 : 225도 방향으로 후진
SetMobileGoalVelocityForSyncWrite( -GOAL_VELOCITY_VALUE, 0,
0, -GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(2000);
// 전방좌측과 후방우측 모터 정지, 전방우측과 후방좌측 모터 전진 : 315도 방향으로 전진
SetMobileGoalVelocityForSyncWrite( 0, GOAL_VELOCITY_VALUE,
GOAL_VELOCITY_VALUE, 0 );
while(dxl.syncWrite(&sw_infos) != true){}
delay(2000);
// 전방좌측과 후방우측 모터 정지, 전방우측과 후방좌측 모터 후진 : 135도 방향으로 후진
SetMobileGoalVelocityForSyncWrite( 0, -GOAL_VELOCITY_VALUE,
-GOAL_VELOCITY_VALUE, 0 );
while(dxl.syncWrite(&sw_infos) != true){}
delay(2000);
SetMobileGoalVelocityForSyncWrite( 0, 0, 0, 0 ); // 4바퀴 모두 정지
while(dxl.syncWrite(&sw_infos) != true){}
delay(5000);
}
/*
* MOBILE_DXL_IDS 배열에 있는 아이디의 모터들이 모두 통신 가능한지
* 확인하는 함수. 모두 통신 가능하면 true, 아니면 false를 반환
*/
bool FindMobileBaseServos() {
uint8_t ids_pinged[10] = {0,};
bool is_each_motor_found = true;
if (uint8_t count_pinged = dxl.ping(DXL_BROADCAST_ID, ids_pinged,
sizeof(ids_pinged)/sizeof(ids_pinged[0]), 100)) {
if (count_pinged >= sizeof(MOBILE_DXL_IDS)/sizeof(uint8_t)) {
uint8_t mobile_dxl_ids_idx = 0;
uint8_t ids_pinged_idx = 0;
while(1) {
if (MOBILE_DXL_IDS[mobile_dxl_ids_idx]
== ids_pinged[ids_pinged_idx++]) {
mobile_dxl_ids_idx ++;
if (mobile_dxl_ids_idx
== sizeof(MOBILE_DXL_IDS)/sizeof(uint8_t)) {
// 찾으려는 모터를 모두 찾은 경우
break;
}
} else {
if (ids_pinged_idx == count_pinged) {
// 통신가능한 모터가 더이상 없는 경우
is_each_motor_found = false;
break;
}
}
}
if (!is_each_motor_found) {
Serial.print("Motor IDs does not match : ");
Serial.println(dxl.getLastLibErrCode());
}
} else {
Serial.print("Motor count does not match : ");
Serial.println(dxl.getLastLibErrCode());
is_each_motor_found = false;
}
} else{
Serial.print("Broadcast returned no items : ");
Serial.println(dxl.getLastLibErrCode());
is_each_motor_found = false;
}
return is_each_motor_found;
}
/*
* sync write 하기 위한 모바일베이스의 goal velocity 값들을 설정하는 함수
*/
void SetMobileGoalVelocityForSyncWrite(int32_t fl_goal_velocity,
int32_t fr_goal_velocity,
int32_t bl_goal_velocity,
int32_t br_goal_velocity) {
sw_data[0].goal_velocity = fl_goal_velocity;
sw_data[1].goal_velocity = fr_goal_velocity;
sw_data[2].goal_velocity = bl_goal_velocity;
sw_data[3].goal_velocity = br_goal_velocity;
sw_infos.is_info_changed = true;
}6. 메카넘휠을 사용한 모바일베이스의 응용 주행 코딩 실습 (1)
loop 에서는 아래와 같이 동작합니다.
(1) 제자리에서 좌회전을 시작하고, 3초간 유지합니다.
(2) 제자리에서 우회전을 시작하고, 3초간 유지합니다.
7. 메카넘휠을 사용한 모바일베이스의 응용 주행 코딩 실습 (2)
loop 에서는 아래와 같이 동작합니다.
(3) 우측 두 개의 모터만 회전시켜서 좌회전을 시작하고, 3초간 유지합니다.
(4) 좌측 두 개의 모터만 회전시켜서 우회전을 시작하고, 3초간 유지합니다.
8. 메카넘휠을 사용한 모바일베이스의 응용 주행 코딩 실습 (3)
loop 에서는 아래와 같이 동작합니다.
(5) 좌측 두 개의 모터를 빠르게, 우측 두 개의 모터를 느리게 전진시켜 원을 그리며 이동하게 한 후, 3초간 유지합니다.
(6) 전방 두 개의 모터를 느리게, 후방 두 개의 모터를 빠르게 회전시켜 원을 그리며 이동하게 한 후, 3초간 유지합니다.
(7) 바퀴 모터 4개를 모두 정지시키고 5초간 대기합니다.
////////////// 모터 구동 관련 선언
#include <Dynamixel2Arduino.h>
#define DXL_SERIAL Serial1
const int DXL_DIR_PIN = 2; // DIR PIN
const float DXL_PROTOCOL_VERSION = 2.0;
Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
// 컨트롤 테이블 아이템의 이름을 사용하기 위해 이 네임스페이스가 필요함
using namespace ControlTableItem;
////////////// 모터 구동용 변수/상수
#define MOBILE_DXL_ID_FL 0x01 // 좌측 전방 모터 아이디
#define MOBILE_DXL_ID_FR 0x02 // 우측 전방 모터 아이디
#define MOBILE_DXL_ID_BL 0x03 // 좌측 후방 모터 아이디
#define MOBILE_DXL_ID_BR 0x04 // 우측 후방 모터 아이디
#define NORMAL_MODE 0x00 // drive mode의 방향 모드
#define REVERSE_MODE 0x01
// FindMobileBaseServos 함수에서 찾을 모터 아이디들, 값이 중복 없이 정렬되어있어야 함
const uint8_t MOBILE_DXL_ID_CNT = 4;
const uint8_t MOBILE_DXL_IDS[MOBILE_DXL_ID_CNT] = {MOBILE_DXL_ID_FL,
MOBILE_DXL_ID_FR,
MOBILE_DXL_ID_BL,
MOBILE_DXL_ID_BR};
// sync write 용 상수, 구조체 정의 및 인스턴스화
const int32_t GOAL_VELOCITY_VALUE = 88; // 모터에 적용할 속도 값
const uint16_t GOAL_VELOCITY_ADDR = 104; // sync write start 주소
const uint16_t GOAL_VELOCITY_SIZE = 4; // sync write data 길이
typedef struct sw_data{ // 모터에 데이터를 쓰기 위한 구조체 정의
int32_t goal_velocity;
} __attribute__((packed)) sw_data_t;
sw_data_t sw_data[MOBILE_DXL_ID_CNT]; // 모터 개수만큼 인스턴스 생성
DYNAMIXEL::InfoSyncWriteInst_t sw_infos; // syncwrite 정보 인스턴스 생성
// syncWrite할 모터들 정보 인스턴스를 모터 개수만큼 생성
DYNAMIXEL::XELInfoSyncWrite_t info_xels_sw[MOBILE_DXL_ID_CNT];
////////////// 메인 프로그램
void setup()
{
Serial.begin(115200);
dxl.begin(57600);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// 모터가 모두 있는지 확인될 때 까지 진행하지 않음
while(!FindMobileBaseServos()) {}
// 각 모터에 설정
for (int i = 0 ; i < sizeof(MOBILE_DXL_IDS)/sizeof(uint8_t) ; i++) {
// 토크 끄기
dxl.writeControlTableItem(TORQUE_ENABLE, MOBILE_DXL_IDS[i], 0);
// 모드 설정
if (MOBILE_DXL_IDS[i] == MOBILE_DXL_ID_FL ||
MOBILE_DXL_IDS[i] == MOBILE_DXL_ID_BL) { // 좌측 바퀴
dxl.writeControlTableItem(DRIVE_MODE, MOBILE_DXL_IDS[i], NORMAL_MODE);
} else if (MOBILE_DXL_IDS[i] == MOBILE_DXL_ID_FR ||
MOBILE_DXL_IDS[i] == MOBILE_DXL_ID_BR) { // 우측 바퀴
dxl.writeControlTableItem(DRIVE_MODE, MOBILE_DXL_IDS[i], REVERSE_MODE);
}
dxl.writeControlTableItem(OPERATING_MODE, MOBILE_DXL_IDS[i], 1);
// 토크 켜기
dxl.writeControlTableItem(TORQUE_ENABLE, MOBILE_DXL_IDS[i], 1);
}
// sync write 준비
sw_infos.packet.p_buf = nullptr; // nullptr을 전달하면 내부 버퍼를 사용
sw_infos.packet.is_completed = false; // false로 초기화
sw_infos.addr = GOAL_VELOCITY_ADDR; // 컨트롤 테이블에 sync write를 시작하는 주소
sw_infos.addr_length = GOAL_VELOCITY_SIZE; // sync write하는 데이터 길이
sw_infos.p_xels = info_xels_sw; // sync write 할 모터 정보
sw_infos.xel_count = 0; // sync write 할 모터 개수
sw_data[0].goal_velocity = 0; // 모터에 write 할 데이터 초기화
sw_data[1].goal_velocity = 0;
sw_data[2].goal_velocity = 0;
sw_data[3].goal_velocity = 0;
// 모터 정보 리스트에 모터 아이디 설정, 데이터 포인터 지정
for(int i = 0; i < MOBILE_DXL_ID_CNT; i++) {
info_xels_sw[i].id = MOBILE_DXL_IDS[i];
info_xels_sw[i].p_data = (uint8_t*)&sw_data[i].goal_velocity;
sw_infos.xel_count++;
}
sw_infos.is_info_changed = true; // sync write 정보가 변경됨을 설정
}
void loop()
{
// 제자리 좌회전
SetMobileGoalVelocityForSyncWrite( -GOAL_VELOCITY_VALUE, GOAL_VELOCITY_VALUE,
-GOAL_VELOCITY_VALUE, GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(3000);
// 제자리 우회전
SetMobileGoalVelocityForSyncWrite( GOAL_VELOCITY_VALUE, -GOAL_VELOCITY_VALUE,
GOAL_VELOCITY_VALUE, -GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(3000);
// 좌측 고정 후 좌회전
SetMobileGoalVelocityForSyncWrite( 0, GOAL_VELOCITY_VALUE,
0, GOAL_VELOCITY_VALUE );
while(dxl.syncWrite(&sw_infos) != true){}
delay(3000);
// 우측 고정 후 우회전
SetMobileGoalVelocityForSyncWrite( GOAL_VELOCITY_VALUE, 0,
GOAL_VELOCITY_VALUE, 0);
while(dxl.syncWrite(&sw_infos) != true){}
delay(3000);
// 원 그리기1
SetMobileGoalVelocityForSyncWrite( GOAL_VELOCITY_VALUE*2, GOAL_VELOCITY_VALUE/5,
GOAL_VELOCITY_VALUE*2, GOAL_VELOCITY_VALUE/5 );
while(dxl.syncWrite(&sw_infos) != true){}
delay(3000);
// 원 그리기2
SetMobileGoalVelocityForSyncWrite( GOAL_VELOCITY_VALUE/5, -GOAL_VELOCITY_VALUE/5,
-GOAL_VELOCITY_VALUE*2, GOAL_VELOCITY_VALUE*2 );
while(dxl.syncWrite(&sw_infos) != true){}
delay(3000);
SetMobileGoalVelocityForSyncWrite( 0, 0, 0, 0 ); // 4바퀴 모두 정지
while(dxl.syncWrite(&sw_infos) != true){}
delay(5000);
}
/*
* MOBILE_DXL_IDS 배열에 있는 아이디의 모터들이 모두 통신 가능한지
* 확인하는 함수. 모두 통신 가능하면 true, 아니면 false를 반환
*/
bool FindMobileBaseServos() {
uint8_t ids_pinged[10] = {0,};
bool is_each_motor_found = true;
if (uint8_t count_pinged = dxl.ping(DXL_BROADCAST_ID, ids_pinged,
sizeof(ids_pinged)/sizeof(ids_pinged[0]), 100)) {
if (count_pinged >= sizeof(MOBILE_DXL_IDS)/sizeof(uint8_t)) {
uint8_t mobile_dxl_ids_idx = 0;
uint8_t ids_pinged_idx = 0;
while(1) {
if (MOBILE_DXL_IDS[mobile_dxl_ids_idx]
== ids_pinged[ids_pinged_idx++]) {
mobile_dxl_ids_idx ++;
if (mobile_dxl_ids_idx
== sizeof(MOBILE_DXL_IDS)/sizeof(uint8_t)) {
// 찾으려는 모터를 모두 찾은 경우
break;
}
} else {
if (ids_pinged_idx == count_pinged) {
// 통신가능한 모터가 더이상 없는 경우
is_each_motor_found = false;
break;
}
}
}
if (!is_each_motor_found) {
Serial.print("Motor IDs does not match : ");
Serial.println(dxl.getLastLibErrCode());
}
} else {
Serial.print("Motor count does not match : ");
Serial.println(dxl.getLastLibErrCode());
is_each_motor_found = false;
}
} else{
Serial.print("Broadcast returned no items : ");
Serial.println(dxl.getLastLibErrCode());
is_each_motor_found = false;
}
return is_each_motor_found;
}
/*
* sync write 하기 위한 모바일베이스의 goal velocity 값들을 설정하는 함수
*/
void SetMobileGoalVelocityForSyncWrite(int32_t fl_goal_velocity,
int32_t fr_goal_velocity,
int32_t bl_goal_velocity,
int32_t br_goal_velocity) {
sw_data[0].goal_velocity = fl_goal_velocity;
sw_data[1].goal_velocity = fr_goal_velocity;
sw_data[2].goal_velocity = bl_goal_velocity;
sw_data[3].goal_velocity = br_goal_velocity;
sw_infos.is_info_changed = true;
}
728x90
728x90
'소프트웨어 > Arduino' 카테고리의 다른 글
| 4자유도 매니퓰레이터의 위치 제어 #1 (0) | 2026.03.25 |
|---|---|
| 스마트 액츄에이터의 각도 제어 (0) | 2026.03.25 |
| 여러 개의 모터를 동시에 제어하기 (0) | 2026.03.18 |
| 스마트 액츄에이터의 회전 방향과 속도 제어 코딩 실습 (0) | 2026.03.18 |
| 스마트 액츄에이터의 회전 방향과 속도 제어 (0) | 2026.03.18 |
