728x90
728x90
1. 매니퓰레이터와 모바일베이스를 사용하여 카메라로 공 추적하기
2. 모바일베이스를 사용해 공 추적하기 코딩 실습
////////////// Pixy2 카메라
#include <Pixy2SPI_SS.h> // SPI 통신하는 Pixy2 라이브러리를 포함시킴
Pixy2SPI_SS pixy; // SPI 통신하는 Pixy2 객체 생성
////////////// 모터 구동 관련 선언
#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 ARM_DXL_ID_1 0x05 // 매니퓰레이터 1번 모터 아이디 (가장 아래)
#define ARM_DXL_ID_2 0x06 // 매니퓰레이터 2번 모터 아이디
#define ARM_DXL_ID_3 0x07 // 매니퓰레이터 3번 모터 아이디
#define ARM_DXL_ID_4 0x08 // 매니퓰레이터 4번 모터 아이디 (가장 위)
#define NORMAL_MODE 0x00 // drive mode의 방향 모드
#define REVERSE_MODE 0x01
#define VELOCITY_BASED_PROFILE 0x00 // profile configuration
#define TIME_BASED_PROFILE 0x04
#define ARM_DXL_1_POSITION_MIN 0
#define ARM_DXL_1_POSITION_MAX 4095
#define ARM_DXL_2_POSITION_MIN 1024
#define ARM_DXL_2_POSITION_MAX 3474
#define ARM_DXL_3_POSITION_MIN 211
#define ARM_DXL_3_POSITION_MAX 2640
#define ARM_DXL_4_POSITION_MIN 722
#define ARM_DXL_4_POSITION_MAX 3161
#define ARM_DXL_1_OFFSET 0
#define ARM_DXL_2_OFFSET 238
#define ARM_DXL_3_OFFSET 799
#define ARM_DXL_4_OFFSET 0
// FinManipulatorServos 함수에서 찾을 모터 아이디들, 값이 중복 없이 정렬되어있어야 함
const uint8_t ARM_DXL_ID_CNT = 4;
const uint8_t ARM_DXL_IDS[ARM_DXL_ID_CNT] = {ARM_DXL_ID_1,
ARM_DXL_ID_2,
ARM_DXL_ID_3,
ARM_DXL_ID_4};
// 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};
// arm sync write 용 상수, 구조체 정의 및 인스턴스화
const uint16_t SW_JOINT_START_ADDR = 112; // sync write start 주소
const uint16_t SW_JOINT_DATA_SIZE = 8; // sync write data 길이
typedef struct sw_joint_data{ // 모터에 데이터를 쓰기 위한 구조체 정의
int32_t profile_velocity;
int32_t goal_position;
} __attribute__((packed)) sw_joint_data_t;
sw_joint_data_t sw_joint_data[ARM_DXL_ID_CNT]; // 모터 개수만큼 인스턴스 생성
DYNAMIXEL::InfoSyncWriteInst_t sw_arm_infos; // syncwrite 정보 인스턴스 생성
// syncWrite할 모터들 정보 인스턴스를 모터 개수만큼 생성
DYNAMIXEL::XELInfoSyncWrite_t info_arm_xels_sw[ARM_DXL_ID_CNT];
// mobile sync write 용 상수, 구조체 정의 및 인스턴스화
const int32_t GOAL_VELOCITY_VALUE = 88; // 모터에 적용할 속도 값
const uint16_t SW_WHEEL_START_ADDR = 104; // sync write start 주소
const uint16_t SW_WHEEL_DATA_SIZE = 4; // sync write data 길이
typedef struct sw_wheel_data{ // 모터에 데이터를 쓰기 위한 구조체 정의
int32_t goal_velocity;
} __attribute__((packed)) sw_wheel_data_t;
sw_wheel_data_t sw_wheel_data[MOBILE_DXL_ID_CNT]; // 모터 개수만큼 인스턴스 생성
DYNAMIXEL::InfoSyncWriteInst_t sw_mobile_infos; // syncwrite 정보 인스턴스 생성
// syncWrite할 모터들 정보 인스턴스를 모터 개수만큼 생성
DYNAMIXEL::XELInfoSyncWrite_t info_mobile_xels_sw[MOBILE_DXL_ID_CNT];
////////////// 매니퓰레이터 계산용 변수/상수
#define pi 3.141592
#define DTR(x) (x)*(pi/180) // degree to radian
#define RTD(x) (x)*(180/pi) // radian to degree
const float d = 79.75; // 바닥에서 매니퓰레이터 2번 모터 회전축까지의 거리
const float L1 = 109.21; // 2번과 3번 모터의 회전축간 거리
const float L2 = 86.4; // 3번과 4번 모터의 회전축간 거리
const float L3 = 97.2; // 4번 모터 회전축과 그리퍼 끝부분 사이의 거리
#define MANIPULATOR_M1_UPPER_LIMIT 20 // 숙였을 때 카메라에 바퀴가 안보이는 정도
#define MANIPULATOR_M1_DEFAULT 0
#define MANIPULATOR_M1_LOWER_LIMIT -20
#define MANIPULATOR_M2_DEFAULT -15
#define MANIPULATOR_M3_DEFAULT -130
#define MANIPULATOR_M4_UPPER_LIMIT 50 // 몸체에 그리퍼가 닿지 않을 정도
#define MANIPULATOR_M4_DEFAULT -15
#define MANIPULATOR_M4_LOWER_LIMIT -40
#define MOBILEBASE_DRIVING_SPEED_LIMIT 100
#define MOBILEBASE_ROTATION_LIMIT 200
#define PIXY2_X_SETPOINT 157 // 카메라 x위치 목표값
#define PIXY2_Y_SETPOINT 103 // 카메라 y위치 목표값
float manipulatorM1Angle = MANIPULATOR_M1_DEFAULT;
float manipulatorM2Angle = MANIPULATOR_M2_DEFAULT;
float manipulatorM3Angle = MANIPULATOR_M3_DEFAULT;
float manipulatorM4Angle = MANIPULATOR_M4_DEFAULT;
int16_t mobilebaseRotation = 0; // 좌/우회전 결정, 양쪽 바퀴 속도차
int16_t mobilebaseDrivingSpeed = 0; // 주행 속도
////////////// 메인 프로그램
void setup()
{
Serial.begin(115200);
pixy.init(); // pixy객체 초기화
dxl.begin(57600);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// 매니퓰레이터 모터가 모두 있는지 확인될 때 까지 진행하지 않음
while(!FindManipulatorServos()) {}
// 각 모터에 설정
for (int i = 0 ; i < ARM_DXL_ID_CNT ; i++) {
// 토크 끄기
dxl.writeControlTableItem(TORQUE_ENABLE, ARM_DXL_IDS[i], 0);
// 드라이브 모드 설정
// 방향은 Reverse Mode, Profile Configuration은 Time-based Profile
dxl.writeControlTableItem(DRIVE_MODE, ARM_DXL_IDS[i],
REVERSE_MODE + TIME_BASED_PROFILE);
// 오퍼레이팅 모드를 위치 제어 모드로 설정
dxl.writeControlTableItem(OPERATING_MODE, ARM_DXL_IDS[i], 3);
switch (ARM_DXL_IDS[i]) { // 모터에 따라 position limit, homing offset 설정
case ARM_DXL_ID_1:
dxl.writeControlTableItem(MIN_POSITION_LIMIT,
ARM_DXL_IDS[i], ARM_DXL_1_POSITION_MIN);
dxl.writeControlTableItem(MAX_POSITION_LIMIT,
ARM_DXL_IDS[i], ARM_DXL_1_POSITION_MAX);
dxl.writeControlTableItem(HOMING_OFFSET,
ARM_DXL_IDS[i], ARM_DXL_1_OFFSET);
break;
case ARM_DXL_ID_2:
dxl.writeControlTableItem(MIN_POSITION_LIMIT,
ARM_DXL_IDS[i], ARM_DXL_2_POSITION_MIN);
dxl.writeControlTableItem(MAX_POSITION_LIMIT,
ARM_DXL_IDS[i], ARM_DXL_2_POSITION_MAX);
dxl.writeControlTableItem(HOMING_OFFSET,
ARM_DXL_IDS[i], ARM_DXL_2_OFFSET);
break;
case ARM_DXL_ID_3:
dxl.writeControlTableItem(MIN_POSITION_LIMIT,
ARM_DXL_IDS[i], ARM_DXL_3_POSITION_MIN);
dxl.writeControlTableItem(MAX_POSITION_LIMIT,
ARM_DXL_IDS[i], ARM_DXL_3_POSITION_MAX);
dxl.writeControlTableItem(HOMING_OFFSET,
ARM_DXL_IDS[i], ARM_DXL_3_OFFSET);
break;
case ARM_DXL_ID_4:
dxl.writeControlTableItem(MIN_POSITION_LIMIT,
ARM_DXL_IDS[i], ARM_DXL_4_POSITION_MIN);
dxl.writeControlTableItem(MAX_POSITION_LIMIT,
ARM_DXL_IDS[i], ARM_DXL_4_POSITION_MAX);
dxl.writeControlTableItem(HOMING_OFFSET,
ARM_DXL_IDS[i], ARM_DXL_4_OFFSET);
break;
}
// 토크 켜기
dxl.writeControlTableItem(TORQUE_ENABLE, ARM_DXL_IDS[i], 1);
}
// manipulator sync write 준비
sw_arm_infos.packet.p_buf = nullptr; // nullptr을 전달하면 내부 버퍼를 사용
sw_arm_infos.packet.is_completed = false; // false로 초기화
sw_arm_infos.addr = SW_JOINT_START_ADDR; // 컨트롤 테이블에 sync write를 시작주소
sw_arm_infos.addr_length = SW_JOINT_DATA_SIZE; // sync write하는 데이터 길이
sw_arm_infos.p_xels = info_arm_xels_sw; // sync write 할 모터 정보
sw_arm_infos.xel_count = 0; // sync write 할 모터 개수
sw_joint_data[0].profile_velocity = 1000; // 모터에 write 할 데이터 초기화
sw_joint_data[0].goal_position = 2048;
sw_joint_data[1].profile_velocity = 1000;
sw_joint_data[1].goal_position = 2048;
sw_joint_data[2].profile_velocity = 1000;
sw_joint_data[2].goal_position = 2048;
sw_joint_data[3].profile_velocity = 1000;
sw_joint_data[3].goal_position = 2048;
// 모터 정보 리스트에 모터 아이디 설정, 데이터 포인터 지정
for(int i = 0; i < ARM_DXL_ID_CNT; i++) {
info_arm_xels_sw[i].id = ARM_DXL_IDS[i];
info_arm_xels_sw[i].p_data = (uint8_t*)&sw_joint_data[i];
sw_arm_infos.xel_count++;
}
sw_arm_infos.is_info_changed = true; // sync write 정보가 변경됨을 설정
SetManipulatorForwardMoveForSyncWrite( manipulatorM1Angle,
manipulatorM2Angle,
manipulatorM3Angle,
manipulatorM4Angle, 2000 );
while(!dxl.syncWrite(&sw_arm_infos)) {}
// 모바일베이스 모터가 모두 있는지 확인될 때 까지 진행하지 않음
while(!FindMobileBaseServos()) {}
// 각 모터에 설정
for (int i = 0 ; i < MOBILE_DXL_ID_CNT ; 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);
}
// mobilebase sync write 준비
sw_mobile_infos.packet.p_buf = nullptr;
sw_mobile_infos.packet.is_completed = false;
sw_mobile_infos.addr = SW_WHEEL_START_ADDR;
sw_mobile_infos.addr_length = SW_WHEEL_DATA_SIZE;
sw_mobile_infos.p_xels = info_mobile_xels_sw;
sw_mobile_infos.xel_count = 0;
sw_wheel_data[0].goal_velocity = 0; // 모터에 write 할 데이터 초기화
sw_wheel_data[1].goal_velocity = 0;
sw_wheel_data[2].goal_velocity = 0;
sw_wheel_data[3].goal_velocity = 0;
// 모터 정보 리스트에 모터 아이디 설정, 데이터 포인터 지정
for(int i = 0; i < MOBILE_DXL_ID_CNT; i++) {
info_mobile_xels_sw[i].id = MOBILE_DXL_IDS[i];
info_mobile_xels_sw[i].p_data = (uint8_t*)&sw_wheel_data[i].goal_velocity;
sw_mobile_infos.xel_count++;
}
sw_mobile_infos.is_info_changed = true; // sync write 정보가 변경됨을 설정
SetMobileGoalVelocityForSyncWrite( 0, 0, 0, 0 ); // 정지
while(!dxl.syncWrite(&sw_mobile_infos)) {}
delay(3000);
}
void loop()
{
pixy.ccc.getBlocks();
if( pixy.ccc.numBlocks )
{
if( pixy.ccc.blocks[0].m_signature == 1 ) // 시그니처 1번으로 인식되면
{
// 인식된 물체의 요소별 에러 계산
int16_t xError = PIXY2_X_SETPOINT - pixy.ccc.blocks[0].m_x;
int16_t yError = PIXY2_Y_SETPOINT - pixy.ccc.blocks[0].m_y;
// 카메라의 xError로 매니퓰레이터의 수평회전 결정
int16_t nextManM1Angle = manipulatorM1Angle - xError/25;
manipulatorM1Angle = constrain( nextManM1Angle,
MANIPULATOR_M1_LOWER_LIMIT,
MANIPULATOR_M1_UPPER_LIMIT );
// 카메라의 yError로 매니퓰레이터의 높이 결정
int16_t nextManM4Angle = manipulatorM4Angle + yError/25;
manipulatorM4Angle = constrain( nextManM4Angle,
MANIPULATOR_M4_LOWER_LIMIT,
MANIPULATOR_M4_UPPER_LIMIT );
// 매니퓰레이터 제어
SetManipulatorForwardMoveForSyncWrite( manipulatorM1Angle,
manipulatorM2Angle,
manipulatorM3Angle,
manipulatorM4Angle, 200);
while(dxl.syncWrite(&sw_arm_infos) != true){}
// follow error 계산
int16_t rotationError = MANIPULATOR_M1_DEFAULT - manipulatorM1Angle;
int16_t distanceError = MANIPULATOR_M4_DEFAULT - manipulatorM4Angle;
// rotationError로 모바일베이스의 회전속도 결정. 양쪽 바퀴의 속도차를 만듦
int16_t nextMobRotation = rotationError * 7;
mobilebaseRotation = constrain( nextMobRotation,
-MOBILEBASE_ROTATION_LIMIT,
MOBILEBASE_ROTATION_LIMIT );
// distanceError로 모바일베이스의 주행속도 결정. 따라가는 속도
int16_t nextMobDrivingSpeed = -distanceError * 3;
mobilebaseDrivingSpeed = constrain( nextMobDrivingSpeed,
-MOBILEBASE_DRIVING_SPEED_LIMIT,
MOBILEBASE_DRIVING_SPEED_LIMIT );
// 모바일베이스 제어
SetMobileGoalVelocityForSyncWrite( mobilebaseDrivingSpeed
- mobilebaseRotation/2,
mobilebaseDrivingSpeed
+ mobilebaseRotation/2,
mobilebaseDrivingSpeed
- mobilebaseRotation/2,
mobilebaseDrivingSpeed
+ mobilebaseRotation/2 );
while(dxl.syncWrite(&sw_mobile_infos) != true){}
} else { // 가장 크게 인식된 물체가 시그니처 1번이 아닌 경우: 정지
// 모바일베이스 제어
SetMobileGoalVelocityForSyncWrite( 0, 0, 0, 0 );
while(dxl.syncWrite(&sw_mobile_infos) != true){}
}
} else { // 카메라 뷰에 검출된 객체가 없는 경우: 정지
// 모바일베이스 제어
SetMobileGoalVelocityForSyncWrite( 0, 0, 0, 0 );
while(dxl.syncWrite(&sw_mobile_infos) != true){}
}
}
/*
* ARM_DXL_IDS 배열에 있는 아이디의 모터들이 모두 통신 가능한지
* 확인하는 함수. 모두 통신 가능하면 true, 아니면 false를 반환
*/
bool FindManipulatorServos() {
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(ARM_DXL_IDS)/sizeof(uint8_t)) {
uint8_t arm_dxl_ids_idx = 0;
uint8_t ids_pinged_idx = 0;
while(1) {
if (ARM_DXL_IDS[arm_dxl_ids_idx]
== ids_pinged[ids_pinged_idx++]) {
arm_dxl_ids_idx ++;
if (arm_dxl_ids_idx
== sizeof(ARM_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;
}
/*
* 각도를 입력받아 모터의 위치 값을 반환하는 함수
* argument :
* angle : 모터의 목표 각도
* return :
* 대응하는 모터의 목표 위치 값. -1은 변환 실패
*/
int32_t GetArmServoGoalPositionWithAngle(float angle) {
if (!isnan(angle)) {
// 입력받은 angle 값을 -180~180 범위 내의 값으로 제한하고
// 0~4095 범위로 매핑
return (int32_t)map(constrain((int32_t)angle, -180, 180),
-180, 180, 0, 4095);
} else {
return (int32_t)-1;
}
}
/*
* 매니퓰레이터를 네 모터의 각도 값과 시간으로 제어를 준비하는 함수
* argument :
* a1 : 5번 모터 각도
* a2 : 6번 모터 각도
* a3 : 7번 모터 각도
* a4 : 8번 모터 각도
* operatingTime : 매니퓰레이터 동작이 완료되기까지 소요될 시간
*/
void SetManipulatorForwardMoveForSyncWrite( float a1,
float a2,
float a3,
float a4,
int32_t operatingTime) {
int32_t motor1GoalPosition = GetArmServoGoalPositionWithAngle( a1 );
int32_t motor2GoalPosition = GetArmServoGoalPositionWithAngle( a2 );
int32_t motor3GoalPosition = GetArmServoGoalPositionWithAngle( a3 );
int32_t motor4GoalPosition = GetArmServoGoalPositionWithAngle( a4 );
if (motor1GoalPosition != -1) {
sw_joint_data[0].goal_position = motor1GoalPosition;
sw_joint_data[0].profile_velocity = operatingTime;
}
if (motor2GoalPosition != -1) {
sw_joint_data[1].goal_position = motor2GoalPosition;
sw_joint_data[1].profile_velocity = operatingTime;
}
if (motor3GoalPosition != -1) {
sw_joint_data[2].goal_position = motor3GoalPosition;
sw_joint_data[2].profile_velocity = operatingTime;
}
if (motor4GoalPosition != -1) {
sw_joint_data[3].goal_position = motor4GoalPosition;
sw_joint_data[3].profile_velocity = operatingTime;
}
sw_arm_infos.is_info_changed = true;
}
/*
* 그리퍼의 위치와 그리퍼의 피치 각도로 4-DOF 매니퓰레이터를 제어하는 함수
* params :
* x : 그리퍼 좌우 위치(mm)
* y : 그리퍼 전후 위치(mm)
* z : 그리퍼 높이(mm)
* angle : 그리퍼 피치 각도(degree)
* operatingTime : 매니퓰레이터 동작이 완료되기까지 소요될 시간
*/
void SetManipulatorInverseMoveForSyncWrite( int16_t x, int16_t y, int16_t z,
int16_t angle,
int32_t operatingTime ) {
int16_t zFromMotor2 = z - d; // 계산의 편의를 위해 2번 모터 관절과 지면 사이의 거리를
// 무시한 z축(위/아래) 거리
// 매니퓰레이터를 측면에서 봤을 때 전진 거리 yd
float yd = sqrt((int32_t)x*x + (int32_t)y*y)*(y/abs(y));
// 매니퓰레이터를 측면에서 봤을 때 2번째 링크가 끝나는 점의 위치 (y2, z2)
float y2 = yd - L3*cos(DTR(angle));
float z2 = zFromMotor2 - L3*sin(DTR(angle));
// 세타1을 구하기 위해 사용되는 변수
float k1 = L1 + L2*(y2*y2 + z2*z2 - L1*L1 - L2*L2)/(2*L1*L2);
float k2 = L2*-1*sqrt(1 - pow((y2*y2 + z2*z2 - L1*L1 - L2*L2)/(2*L1*L2), 2));
float theta1 = 0, theta2 = 0, theta3 = 0, theta4 = 0; // 모터별 세타 변수
theta1 = RTD(atan2(y, x)); // atan2는 -pi~pi 범위로 결과를 반환 함
theta2 = RTD(atan2(z2, y2)) - RTD(atan2(k2, k1));
theta3 = RTD(atan2(-1*sqrt(1 - pow((y2*y2 + z2*z2 - L1*L1 - L2*L2)/(2*L1*L2), 2)),
(y2*y2 + z2*z2 - L1*L1 - L2*L2)/(2*L1*L2)));
theta4 = angle - theta2 - theta3;
float a1 = 90 - theta1; // 세타 값을 모터 제어를 위한 각도로 변환
if (a1 > 180) // 범위를 -180~180으로 변환
a1 -= 360;
float a2 = -90 + theta2;
if (a2 < -180)
a2 += 360;
float a3 = theta3;
float a4 = theta4;
// 계산된 모터 각도로 매니퓰레이터 동작 준비
SetManipulatorForwardMoveForSyncWrite( a1, a2, a3, a4, operatingTime);
Serial.println("Result");
Serial.print(" theta1 = ");
Serial.println(theta1);
Serial.print(" theta2 = ");
Serial.println(theta2);
Serial.print(" theta3 = ");
Serial.println(theta3);
Serial.print(" theta4 = ");
Serial.println(theta4);
Serial.println();
Serial.println("Result motor angle");
Serial.print(" m1angle = ");
Serial.println(a1);
Serial.print(" m2angle = ");
Serial.println(a2);
Serial.print(" m3angle = ");
Serial.println(a3);
Serial.print(" m4angle = ");
Serial.println(a4);
}
/*
* 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_wheel_data[0].goal_velocity = fl_goal_velocity;
sw_wheel_data[1].goal_velocity = fr_goal_velocity;
sw_wheel_data[2].goal_velocity = bl_goal_velocity;
sw_wheel_data[3].goal_velocity = br_goal_velocity;
sw_mobile_infos.is_info_changed = true;
}
3. 물체 추적하기 미션(실습)
Pixy2 카메라는 pixy.ccc.getBlocks 함수를 실행하게 되면 영상 이미지 속 식별 물체의 폭(pixy.ccc.blocks[0].m_width)과 높이(pixy.ccc.blocks[0].m_height)를 알아낼 수 있습니다.
이를 이용하여 기준 거리를 정하고, 기준 거리에 물체를 놓았을 때, 영상 이미지 속 식별 물체의 폭과 높이를 알아내어 기준이 될 물체의 면적(objectArea = (pixy.ccc.blocks[0].m_width * pixy.ccc.blocks[0].m_height)을 계산합니다.
영상 이미지 속 식별 물체의 면적 변화량(dD)을 (현재 면적 - 기준 면적)으로 정의할 경우, 모바일 베이스의 주행 속도를 면적 변화량(dD)에 반비례하도록 지정(nextMobDrivingSpeed = dD * -weightFactor)하면 모든 조건에서 전진/후진을 손쉽게 처리할 수 있습니다.
위의 조건으로 지정할 경우, 영상 이미지 속 식별 물체의 거리가 멀어지게 되면, 물체의 면적이 기준 거리에서의 면적보다 작아지게 되고 dD는 작아지게 되므로, 모바일 베이스의 주행 속도는 증가하여 물체를 추격합니다. 그 결과로 영상 이미지 속 식별 물체의 거리가 기준 거리에 가까워지게 되면, dD는 여전히 음수이지만 증가하게 되므로 모바일 베이스의 주행 속도도 양수이지만 감소하게 되며 기준 거리가 되면 거의 정지하게 됩니다.
영상 이미지 속 식별 물체가 기준 거리보다 가까이 위치하게 되면, dD는 양수가 되므로 모바일 베이스의 주행 속도는 음수가 되어 후진하게 되며, 거리가 매우 가까울수록 dD는 더 큰 양수가 되므로 모바일 베이스의 주행 속도는 더 작은 음수가 되어 빠르게 후진하게 됩니다. 후진으로 인해 기준 거리에 가까워질수록 dD는 여전히 양수이지만 0에 가까워져 후진 속도가 감소하여 결국 거의 정지하게 됩니다.
프로그램을 실행하기 전에 일정한 거리를 유지할 객체를 인식시키고 해당 객체의 signature 값을 이용해 해당 객체가 검출될 경우 일정한 거리를 유지하는 행동을 수행하도록 합니다.
같은 signature를 가진 여러 개의 물체가 인식되는 경우, 어떻게 처리할 지도 생각해야 합니다. 가장 쉬운 방법은 가장 인식 면적이 넓은 물체를 추적 대상으로 하는 것입니다. 처음 인식할 물체의 인덱스(m_index)를 이용해 같은 물체를 계속 추적 대상으로 설정하는 것도 가능합니다.
코드를 완성하기 전 실험을 통해 모바일 베이스의 주행 속도를 결정하기 위해 거리 조절 기준 면적에 대한 면적 변화량(dD)에 곱해줄 가중치 값을 적절하게 선택해야 합니다.
////////////// [실습] 원근법을 깨우쳐 밀당하는 블랙 베리
// 기준 물체의 크기 변화를 이용해 모바일 베이스가 전진과 후진을 실행
// 기준 거리(30cm)에서의 물체의 면적을 측정하여,
// 면적 변화량(기준면적(PIXY2_DEFAULT_AREA=800)-측정면적)에 비례하여 주행 속도를 변화시킴
////////////// Pixy2 카메라
#include <Pixy2SPI_SS.h> // SPI 통신하는 Pixy2 라이브러리를 포함시킴
Pixy2SPI_SS pixy; // SPI 통신하는 Pixy2 객체 생성
////////////// 모터 구동 관련 선언
#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 ARM_DXL_ID_1 0x05 // 매니퓰레이터 1번 모터 아이디 (가장 아래)
#define ARM_DXL_ID_2 0x06 // 매니퓰레이터 2번 모터 아이디
#define ARM_DXL_ID_3 0x07 // 매니퓰레이터 3번 모터 아이디
#define ARM_DXL_ID_4 0x08 // 매니퓰레이터 4번 모터 아이디 (가장 위)
#define NORMAL_MODE 0x00 // drive mode의 방향 모드
#define REVERSE_MODE 0x01
#define VELOCITY_BASED_PROFILE 0x00 // profile configuration
#define TIME_BASED_PROFILE 0x04
// FinManipulatorServos 함수에서 찾을 모터 아이디들, 값이 중복 없이 정렬되어있어야 함
const uint8_t ARM_DXL_ID_CNT = 4;
const uint8_t ARM_DXL_IDS[ARM_DXL_ID_CNT] = {ARM_DXL_ID_1,
ARM_DXL_ID_2,
ARM_DXL_ID_3,
ARM_DXL_ID_4};
const uint32_t ARM_DXL_POSITION_MIN[ARM_DXL_ID_CNT] = {0, 1024, 211, 722};
const uint32_t ARM_DXL_POSITION_MAX[ARM_DXL_ID_CNT] = {4095, 3474, 2640, 3161};
const uint32_t ARM_DXL_OFFSET[ARM_DXL_ID_CNT] = {0, 238, 799, 0};
// 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};
// arm sync write 용 상수, 구조체 정의 및 인스턴스화
const uint16_t SW_JOINT_START_ADDR = 112; // sync write start 주소
const uint16_t SW_JOINT_DATA_SIZE = 8; // sync write data 길이
typedef struct sw_joint_data{ // 모터에 데이터를 쓰기 위한 구조체 정의
int32_t profile_velocity;
int32_t goal_position;
} __attribute__((packed)) sw_joint_data_t;
sw_joint_data_t sw_joint_data[ARM_DXL_ID_CNT]; // 모터 개수만큼 인스턴스 생성
DYNAMIXEL::InfoSyncWriteInst_t sw_arm_infos; // syncwrite 정보 인스턴스 생성
// syncWrite할 모터들 정보 인스턴스를 모터 개수만큼 생성
DYNAMIXEL::XELInfoSyncWrite_t info_arm_xels_sw[ARM_DXL_ID_CNT];
// mobile sync write 용 상수, 구조체 정의 및 인스턴스화
const int32_t GOAL_VELOCITY_VALUE = 88; // 모터에 적용할 속도 값
const uint16_t SW_WHEEL_START_ADDR = 104; // sync write start 주소
const uint16_t SW_WHEEL_DATA_SIZE = 4; // sync write data 길이
typedef struct sw_wheel_data{ // 모터에 데이터를 쓰기 위한 구조체 정의
int32_t goal_velocity;
} __attribute__((packed)) sw_wheel_data_t;
sw_wheel_data_t sw_wheel_data[MOBILE_DXL_ID_CNT]; // 모터 개수만큼 인스턴스 생성
DYNAMIXEL::InfoSyncWriteInst_t sw_mobile_infos; // syncwrite 정보 인스턴스 생성
// syncWrite할 모터들 정보 인스턴스를 모터 개수만큼 생성
DYNAMIXEL::XELInfoSyncWrite_t info_mobile_xels_sw[MOBILE_DXL_ID_CNT];
////////////// 매니퓰레이터 계산용 변수/상수
#define pi 3.141592
#define DTR(x) (x)*(pi/180) // degree to radian
#define RTD(x) (x)*(180/pi) // radian to degree
const float d = 79.75; // 바닥에서 매니퓰레이터 2번 모터 회전축까지의 거리
const float L1 = 109.21; // 2번과 3번 모터의 회전축간 거리
const float L2 = 86.4; // 3번과 4번 모터의 회전축간 거리
const float L3 = 97.2; // 4번 모터 회전축과 그리퍼 끝부분 사이의 거리
// 숙였을 때 카메라에 바퀴가 안보이는 정도
#define MANIPULATOR_M1_UPPER_LIMIT 20 // 우측 극대값
#define MANIPULATOR_M1_DEFAULT 0
#define MANIPULATOR_M1_LOWER_LIMIT -20 // 좌측 극대값
#define MANIPULATOR_M2_DEFAULT -15 // 곧바로 서는 각
#define MANIPULATOR_M3_DEFAULT -130
#define MANIPULATOR_M4_UPPER_LIMIT 50 // 몸체에 그리퍼가 닿지 않을 정도
#define MANIPULATOR_M4_DEFAULT 0
#define MANIPULATOR_M4_LOWER_LIMIT -40
#define MOBILEBASE_DRIVING_SPEED_LIMIT 100
#define MOBILEBASE_ROTATION_LIMIT 200
#define PIXY2_X_SETPOINT 157 // 카메라 x위치 목표값
#define PIXY2_Y_SETPOINT 103 // 카메라 y위치 목표값
#define DAMPING_FACTOR 15 // 카메라 인식 물체 추적시
// 댐핑 팩터(클수록 조금씩 이동)
#define PIXY2_DEFAULT_AREA 800 // 카메라 인식 물체의 기본 설정 면적
float manipulatorM1Angle = MANIPULATOR_M1_DEFAULT;
float manipulatorM2Angle = MANIPULATOR_M2_DEFAULT;
float manipulatorM3Angle = MANIPULATOR_M3_DEFAULT;
float manipulatorM4Angle = MANIPULATOR_M4_DEFAULT;
int16_t mobilebaseRotation = 0; // 좌/우회전 결정, 양쪽 바퀴 속도차
int16_t mobilebaseDrivingSpeed = 0; // 주행 속도
////////////// 메인 프로그램
void setup()
{
Serial.begin(115200);
pixy.init(); // pixy객체 초기화
dxl.begin(57600);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
motorInitialize();
}
void loop()
{
pixy.ccc.getBlocks();
if( pixy.ccc.numBlocks )
{
if( pixy.ccc.blocks[0].m_signature == 1 ) // 시그니처 1번으로 인식되면
{
// 인식된 물체의 요소별 기준으로부터의 변화량 계산
int16_t dX = pixy.ccc.blocks[0].m_x - PIXY2_X_SETPOINT;
int16_t dY = pixy.ccc.blocks[0].m_y - PIXY2_Y_SETPOINT;
// 카메라의 dX로 매니퓰레이터의 수평회전 결정
int16_t nextManM1Angle = manipulatorM1Angle + dX/DAMPING_FACTOR;
manipulatorM1Angle = constrain( nextManM1Angle,
MANIPULATOR_M1_LOWER_LIMIT,
MANIPULATOR_M1_UPPER_LIMIT );
// 카메라의 dY로 매니퓰레이터의 높이 결정
int16_t nextManM4Angle = manipulatorM4Angle - dY/DAMPING_FACTOR;
manipulatorM4Angle = constrain( nextManM4Angle,
MANIPULATOR_M4_LOWER_LIMIT,
MANIPULATOR_M4_UPPER_LIMIT );
// 매니퓰레이터 제어
SetManipulatorForwardMoveForSyncWrite( manipulatorM1Angle,
manipulatorM2Angle,
manipulatorM3Angle,
manipulatorM4Angle, 200);
while(dxl.syncWrite(&sw_arm_infos) != true){}
// 매니퓰레이터 수평 회전 변화량, 물체 크기 변화량 계산
int16_t dR = manipulatorM1Angle - MANIPULATOR_M1_DEFAULT;
uint16_t objectArea = (pixy.ccc.blocks[0].m_width
* pixy.ccc.blocks[0].m_height); // 물체 인식 면적
int32_t dD = (int32_t)objectArea - PIXY2_DEFAULT_AREA; // 면적 변화량
// 매니퓰레이터 회전 변화량으로 모바일베이스의 회전속도 결정. 양쪽 바퀴의 속도차를 만듦
int16_t nextMobRotation = dR * 7;
mobilebaseRotation = constrain( nextMobRotation,
-MOBILEBASE_ROTATION_LIMIT,
MOBILEBASE_ROTATION_LIMIT );
// dD가 음수일 때는 물체가 멀리 있기 때문에 로봇이 전진하도록 적당한 값의 음수를 곱함
// 양수일 때는 물체가 가까이 있기 때문에 로봇이 후진하도록 적당한 값의 양수를 곱함
int16_t nextMobDrivingSpeed = dD * -0.1;
mobilebaseDrivingSpeed = constrain( nextMobDrivingSpeed,
-MOBILEBASE_DRIVING_SPEED_LIMIT,
MOBILEBASE_DRIVING_SPEED_LIMIT );
// 모바일베이스 제어
SetMobileGoalVelocityForSyncWrite( mobilebaseDrivingSpeed
+ mobilebaseRotation/2,
mobilebaseDrivingSpeed
- mobilebaseRotation/2,
mobilebaseDrivingSpeed
+ mobilebaseRotation/2,
mobilebaseDrivingSpeed
- mobilebaseRotation/2 );
while(dxl.syncWrite(&sw_mobile_infos) != true){}
delay(200);
} else { // 카메라 뷰에 검출된 객체가 없는 경우: 정지
// 모바일베이스 제어
SetMobileGoalVelocityForSyncWrite( 0, 0, 0, 0 );
while(dxl.syncWrite(&sw_mobile_infos) != true){}
}
} else { // 카메라 뷰에 검출된 객체가 없는 경우: 정지
// 모바일베이스 제어
SetMobileGoalVelocityForSyncWrite( 0, 0, 0, 0 );
while(dxl.syncWrite(&sw_mobile_infos) != true){}
}
}
// 모터 초기화
void motorInitialize() {
// 매니퓰레이터 모터가 모두 있는지 확인될 때 까지 진행하지 않음
while(!FindManipulatorServos()) {}
// 각 모터에 설정
for (int i = 0 ; i < ARM_DXL_ID_CNT ; i++) {
// 토크 끄기
dxl.writeControlTableItem(TORQUE_ENABLE, ARM_DXL_IDS[i], 0);
// 드라이브 모드 설정
// 방향은 Reverse Mode, Profile Configuration은 Time-based Profile
dxl.writeControlTableItem(DRIVE_MODE, ARM_DXL_IDS[i],
REVERSE_MODE + TIME_BASED_PROFILE);
// 오퍼레이팅 모드를 위치 제어 모드로 설정
dxl.writeControlTableItem(OPERATING_MODE, ARM_DXL_IDS[i], 3);
// 각 모터의 최소/최대/offset 값 설정
dxl.writeControlTableItem(MIN_POSITION_LIMIT, ARM_DXL_IDS[i],
ARM_DXL_POSITION_MIN[i]);
dxl.writeControlTableItem(MAX_POSITION_LIMIT, ARM_DXL_IDS[i],
ARM_DXL_POSITION_MAX[i]);
dxl.writeControlTableItem(HOMING_OFFSET, ARM_DXL_IDS[i],
ARM_DXL_OFFSET[i]);
// 토크 켜기
dxl.writeControlTableItem(TORQUE_ENABLE, ARM_DXL_IDS[i], 1);
}
// manipulator sync write 준비
sw_arm_infos.packet.p_buf = nullptr; // nullptr을 전달하면 내부 버퍼를 사용
sw_arm_infos.packet.is_completed = false; // false로 초기화
sw_arm_infos.addr = SW_JOINT_START_ADDR; // 컨트롤 테이블에 sync write 시작주소
sw_arm_infos.addr_length = SW_JOINT_DATA_SIZE; // sync write하는 데이터길이
sw_arm_infos.p_xels = info_arm_xels_sw; // sync write 할 모터 정보
sw_arm_infos.xel_count = 0; // sync write 할 모터 개수
sw_joint_data[0].profile_velocity = 1000; // 모터에 write 할 데이터 초기화
sw_joint_data[0].goal_position = 2048;
sw_joint_data[1].profile_velocity = 1000;
sw_joint_data[1].goal_position = 2048;
sw_joint_data[2].profile_velocity = 1000;
sw_joint_data[2].goal_position = 2048;
sw_joint_data[3].profile_velocity = 1000;
sw_joint_data[3].goal_position = 2048;
// 모터 정보 리스트에 모터 아이디 설정, 데이터 포인터 지정
for(int i = 0; i < ARM_DXL_ID_CNT; i++) {
info_arm_xels_sw[i].id = ARM_DXL_IDS[i];
info_arm_xels_sw[i].p_data = (uint8_t*)&sw_joint_data[i];
sw_arm_infos.xel_count++;
}
sw_arm_infos.is_info_changed = true; // sync write 정보가 변경됨을 설정
SetManipulatorForwardMoveForSyncWrite( manipulatorM1Angle,
manipulatorM2Angle,
manipulatorM3Angle,
manipulatorM4Angle, 2000 );
while(!dxl.syncWrite(&sw_arm_infos)) {}
// 모바일베이스 모터가 모두 있는지 확인될 때 까지 진행하지 않음
while(!FindMobileBaseServos()) {}
// 각 모터에 설정
for (int i = 0 ; i < MOBILE_DXL_ID_CNT ; 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);
}
// mobilebase sync write 준비
sw_mobile_infos.packet.p_buf = nullptr;
sw_mobile_infos.packet.is_completed = false;
sw_mobile_infos.addr = SW_WHEEL_START_ADDR;
sw_mobile_infos.addr_length = SW_WHEEL_DATA_SIZE;
sw_mobile_infos.p_xels = info_mobile_xels_sw;
sw_mobile_infos.xel_count = 0;
sw_wheel_data[0].goal_velocity = 0; // 모터에 write 할 데이터 초기화
sw_wheel_data[1].goal_velocity = 0;
sw_wheel_data[2].goal_velocity = 0;
sw_wheel_data[3].goal_velocity = 0;
// 모터 정보 리스트에 모터 아이디 설정, 데이터 포인터 지정
for(int i = 0; i < MOBILE_DXL_ID_CNT; i++) {
info_mobile_xels_sw[i].id = MOBILE_DXL_IDS[i];
info_mobile_xels_sw[i].p_data = (uint8_t*)&sw_wheel_data[i].goal_velocity;
sw_mobile_infos.xel_count++;
}
sw_mobile_infos.is_info_changed = true; // sync write 정보가 변경됨을 설정
SetMobileGoalVelocityForSyncWrite( 0, 0, 0, 0 ); // 정지
while(!dxl.syncWrite(&sw_mobile_infos)) {}
delay(3000);
}
/*
* ARM_DXL_IDS 배열에 있는 아이디의 모터들이 모두 통신 가능한지
* 확인하는 함수. 모두 통신 가능하면 true, 아니면 false를 반환
*/
bool FindManipulatorServos() {
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(ARM_DXL_IDS)/sizeof(uint8_t)) {
uint8_t arm_dxl_ids_idx = 0;
uint8_t ids_pinged_idx = 0;
while(1) {
if (ARM_DXL_IDS[arm_dxl_ids_idx]
== ids_pinged[ids_pinged_idx++]) {
arm_dxl_ids_idx ++;
if (arm_dxl_ids_idx
== sizeof(ARM_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;
}
/*
* 각도를 입력받아 모터의 위치 값을 반환하는 함수
* argument :
* angle : 모터의 목표 각도
* return :
* 대응하는 모터의 목표 위치 값. -1은 변환 실패
*/
int32_t GetArmServoGoalPositionWithAngle(float angle) {
if (!isnan(angle)) {
// 입력받은 angle 값을 -180~180 범위 내의 값으로 제한하고
// 0~4095 범위로 매핑
return (int32_t)map(constrain((int32_t)angle, -180, 180),
-180, 180, 0, 4095);
} else {
return (int32_t)-1;
}
}
/*
* 매니퓰레이터를 네 모터의 각도 값과 시간으로 제어를 준비하는 함수
* argument :
* a1 : 5번 모터 각도
* a2 : 6번 모터 각도
* a3 : 7번 모터 각도
* a4 : 8번 모터 각도
* operatingTime : 매니퓰레이터 동작이 완료되기까지 소요될 시간
*/
void SetManipulatorForwardMoveForSyncWrite( float a1,
float a2,
float a3,
float a4,
int32_t operatingTime) {
int32_t motor1GoalPosition = GetArmServoGoalPositionWithAngle( a1 );
int32_t motor2GoalPosition = GetArmServoGoalPositionWithAngle( a2 );
int32_t motor3GoalPosition = GetArmServoGoalPositionWithAngle( a3 );
int32_t motor4GoalPosition = GetArmServoGoalPositionWithAngle( a4 );
if (motor1GoalPosition != -1) {
sw_joint_data[0].goal_position = motor1GoalPosition;
sw_joint_data[0].profile_velocity = operatingTime;
}
if (motor2GoalPosition != -1) {
sw_joint_data[1].goal_position = motor2GoalPosition;
sw_joint_data[1].profile_velocity = operatingTime;
}
if (motor3GoalPosition != -1) {
sw_joint_data[2].goal_position = motor3GoalPosition;
sw_joint_data[2].profile_velocity = operatingTime;
}
if (motor4GoalPosition != -1) {
sw_joint_data[3].goal_position = motor4GoalPosition;
sw_joint_data[3].profile_velocity = operatingTime;
}
sw_arm_infos.is_info_changed = true;
}
/*
* 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_wheel_data[0].goal_velocity = fl_goal_velocity;
sw_wheel_data[1].goal_velocity = fr_goal_velocity;
sw_wheel_data[2].goal_velocity = bl_goal_velocity;
sw_wheel_data[3].goal_velocity = br_goal_velocity;
sw_mobile_infos.is_info_changed = true;
}
위의 소스와 아래 파일들의 소스를 이용하여 아래의 그림과 같이 주행하도록 프로그램 하세요.
동작 순서
1. 전원 ON
2. 로봇 정지 상태로 대기
3. 왼쪽 버튼 SW1 누름
4. 미션 시작
5. 전진
6. 정면 좌/우 PSD가 10cm 이하이면 정지
7. 좌측 매커넘 이동
8. 좌측 PSD가 9cm 이하이면 정지
9. 다시 전진
10. 정면 우측 PSD가 25cm 이하이면 최종 정지
728x90
728x90
'소프트웨어 > Arduino' 카테고리의 다른 글
| 미션 위치로 이동해 지정된 블록 옮기기 (0) | 2026.05.20 |
|---|---|
| AGV2 이동 알고리즘 (0) | 2026.05.07 |
| 비전인식 물체 추적 (매니퓰레이터 활용) (0) | 2026.05.05 |
| 카메라 비전과 인식률 개선 (0) | 2026.05.05 |
| 관성 측정 장치 (IMU, Inertial Measurement Unit)를 사용한 각도 측정 실습 (0) | 2026.04.24 |
