본문으로 바로가기

4자유도 매니퓰레이터의 위치 제어 #2

category 소프트웨어/Arduino 2026. 3. 25. 23:52

1. 4자유도 매니퓰레이터의 위치 제어 - 역기구학

 

각 모터의 각도를 알고있을 때 매니퓰레이터 끝 부분의 위치를 계산할 수 있는 정기구학과는 반대로, 목표하는 위치가 정해져 있을 때 이 지점에 도달하기 위해 필요한 각 관절 모터의 각도를 구하는 것을 역기구학이라고 합니다.

매니퓰레이터의 정기구학에서는 모터 각도에 따른 위치 결과 값이 하나입니다. 하지만 역기구학에서는 아래 그림과 같이 목표 위치가 같아도 매니퓰레이터의 자세는 다를 수 있기 때문에 결과 값이 여러 개 일수 있습니다. 그래서 목표 위치와 함께 적정한 자세를 선택하는 것이 중요합니다.

자유도 6개로 3차원 좌표계에서 말단장치의 모든 위치(x, y, z) 와 자세(roll, pitch, yaw)를 표현할 수 있습니다. 자유도가 그 이상이 되면 장애물을 회피하는 등에 사용할 수 있습니다.

 

 

 

 

2. 매니퓰레이터의 역기구학 - 2자유도

 

 

 

매니퓰레이터의 각 링크 길이 l_1 과 l_2를 알고 있고, 목표 위치(x, y)가 주어졌을 때

(이 후로, 계산의 편의를 위해 지면과의 거리 d는 무시) 코사인 제2 법칙을 사용해 cos⁡〖θ_2 〗 를 구할 수 있습니다.

 

 

 

 

3. 매니퓰레이터의 역기구학 - 3자유도

 

 

 

 

 

 

4. 매니퓰레이터의 역기구학 - 4자유도

 

블랙베리의 4자유도 매니퓰레이터는 3자유도 매니퓰레이터의 베이스 부분에 수평 회전을 추가한 모습입니다. 이로 인해 2차원으로만 이동하던 매니퓰레이터의 이동 범위는 3차원으로 늘어납니다. 모터 개수가 늘어남에 따라 모터 번호와 좌표계를 우측 사진과 같이 정의합니다.

 

 

 

 

 

 

 

5. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (1)

 

 

 

(1). radian에서 degree로 변환하기 위한 매크로와 degree에서 radian으로 변환하기 위한 매크로를 정의합니다.

라디안을 각도로 바꾸는 식은 다음과 같습니다.

        (라디안) * (180 /pi)

RTD(x) : radian to degree
입력된 x를 각도로 변환하는 매크로입니다.

 

 

 

(2). 매니퓰레이터의 링크의 길이 값들을 상수로 선언합니다. 각 상수들은 아래와 같습니다.
 
 

6. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (2)

 

 

(3). SetManipulatorInverseMoveForSyncWrite 함수는 그리퍼 끝 부분의 위치를 의미하는 x, y, z와 지면과의 그리퍼 각도 angle, 매니퓰레이터 동작 시간 operatingTime을 인자로 받습니다.

인자로 받는 x, y, z, angle 값은 아래 표시된 것과 같습니다.

 

 

 

7. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (3)

 

 

 

(4). SetManipulatorInverseMoveForSyncWrite 함수는 아래와 같이 정의됩니다.
 
1) 각 모터의 θ값을 구하기 위해 필요한 값들을 구합니다.  매니퓰레이터가 아래 그림의 빨간색 선과 같은 자세가 되도록 하기 위해, θ_3를 -180~0 범위로 제한하면 sin⁡〖θ_3 〗는 아래와 같아집니다.
 
2) 각 모터의 θ 변수들을 선언하고 값을 구합니다.
 
 

 

8. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (4)

 

 

 

(4). SetManipulatorInverseMoveForSyncWrite 함수는 아래와 같이 정의됩니다.
 
3) 세타 값을 모터 각도로 변환합니다. SetManipulatorForwardMoveForSyncWrite 함수는
-180~180도 범위의 각도 값을 인자로 받기 때문에 a1과 a2 값의 범위를 그에 맞게 변환합니다.
 
4) 계산된 모터 각도로 매니퓰레이터 동작을 준비합니다.
 
5) 계산된 세타 값을 출력합니다.
 
6) 계산된 모터 각도를 출력합니다.

 

 

 

9. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (5)

 

 

 

(5). 매니퓰레이터 제어에 사용 될 그리퍼의 최종 위치 X, Y, Z와 그리퍼의 각도 Angle 변수를 선언하고, 이동 가능한 범위 안에서 적당한 값을 지정합니다. (X, Y, Z는 mm 단위)
 
(6). 프로그램에서 사용할 변수를 선언 후 초기화 합니다.

unit : X, Y, Z, Angle 변수에 더하거나 뺄 값의 단위입니다

positionIsChanged : 그리퍼의 위치나 각도가 변경될 때 참이 되는 플래그입니다. 기본 값을 참으로 합니다.
 

setup 에서는 아래와 같은 동작을 합니다.

(7). 모터 설정이 끝난 후에 그리퍼의 목표 위치 값과 각도 값을 출력합니다.
 
(8). 초기 목표 위치로 이동하도록 매니퓰레이터를 제어합니다.
 
(9). “Start”를 출력합니다.

 

 

 

10. 4자유도 매니퓰레이터의 역기구학 코딩 실습 – 그리퍼 위치 제어 KEY

 

 

 

 

 

 

 

 

11. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (6)

 

 

loop에서는 아래 10번 부터 14번 과정을 반복합니다.

(10). 시리얼 모니터로 수신된 값이 있는지 확인하는 코드입니다. 이 예제 소스코드에서 loop문 안의 모든 코드는 Serial에 수신된 값이 있을 때만 실행됩니다. 시리얼 모니터를 연 후, 사용자가 시리얼 모니터를 통해 값을 전송하면 아두이노 보드에서는 Serial.available()함수를 통해 읽을 수 있는 값이 몇 바이트인지 확인할 수 있습니다.

Serial.available() : Serial에 수신된 BYTE 수 반환
(11). 구분선을 출력합니다.
 
(12). switch문에서는 Serial.read()함수에서 반환된 값에 따라 분기됩니다.

Serial.read() : HardwareSerial객체인 Serial에 수신된 데이터 중 가장 오래된 데이터 1바이트 읽어서 반환하는 함수입니다. 읽어진 값은 수신 버퍼에서 삭제됩니다.

1)  받아진 값에 따라 unit값을 변경하는 부분입니다. 이 경우에는 그리퍼의 위치나 각도 값이 변경된 것이 아니기 때문에 isPositionChanged를 0으로 변경합니다.

- 받아진 값이 ‘1’이면 unit을 1로 변경합니다.
- 받아진 값이 ‘2’이면 unit을 10으로 변경합니다.
- 받아진 값이 ‘3’이면 unit을 20으로 변경합니다.

2)  받아진 값에 따라 X값을 변경하는 부분입니다.
- 받아진 값이 ‘d’이면 X에 unit을 더합니다
- 받아진 값이 ‘a’이면 X에서 unit을 뺍니다.

 

 

12. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (7)

 

 

 

loop에서는 아래 10번 부터 14번 과정을 반복합니다.

(12). switch문에서는 Serial.read()함수에서 반환된 값에 따라 분기됩니다.

3)  받아진 값에 따라 Y값을 변경하는 부분입니다.
- 받아진 값이 ‘w’이면 Y에 unit을 더합니다.
- 받아진 값이 ‘x’이면 (Y – unit)이 0 이상일 때만
   Y에서 unit을 뺍니다.

4)  받아진 값에 따라 Z값을 변경하는 부분입니다.
- 받아진 값이 ‘q’이면 Z에 unit을 더합니다
- 받아진 값이 ‘z’이면 (Z – unit)이 0이상일 때만
   Z에서 unit을 뺍니다.

5)  받아진 값에 따라 Angle값을 변경하는 부분입니다.
- 받아진 값이 ‘e’이면 (Angle + unit)이 90이하일 때만
   Angle에 unit을 더합니다
- 받아진 값이 ‘c’이면 (Angle – unit)이 -90이상일 때만
   Angle에서 unit을 뺍니다.

6)  받아진 값이 위에 나열된 케이스 중에 없다면 잘못된 입력이라고 출력합니다. 이 경우에는 그리퍼의 위치나 각도 값이 변경된 것이 아니기 때문에 isPositionChanged를 0으로 변경합니다.
 

 

13. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (8)

 

 

loop에서는 아래 10번 부터 14번 과정을 반복합니다.

 

(13). 그리퍼의 위치나 각도가 변경되었다면 X, Y, Z, Angle값을 출력하고, 매니퓰레이터를 해당 위치로 이동시킵니다. 그리고 isPositionChanged를 초기 값인 1로 변경합니다.
 
(14). 구분선을 출력합니다.
 
 

 

14. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (9)

 

 

 

(1). 프로그램을 모바일매니퓰레이터 블랙베리에 업로드합니다.
 
(2). 우측 상단의 시리얼 모니터 버튼을 클릭합니다.
 
(3). 시리얼모니터에서 통신속도는 소스코드에 사용한 통신 속도에 맞춰 115200으로 변경하고 line ending 없음을 선택합니다.
 

프로그램이 실행되면 매니퓰레이터가 초기 자세로 변경되고, 좌측 사진과 같이 초기 자세에 대한 내용이 출력됩니다.

 

 

 

15. 4자유도 매니퓰레이터의 역기구학 코딩 실습 (10)

 

 

 

왼쪽의 시리얼 모니터 상단에 텍스트를 입력하고, 전송 버튼을 클릭하거나 엔터를 누르면 입력된 내용이 블랙베리에 전송됩니다.

- 문자 ‘1’, ‘2’ 또는 ‘3’을 전송하면 unit이 변경됩니다.

    unit이 클 수록 한 번에 이동하는 거리가 커집니다.

- 문자 ‘a’, ‘d’, ‘w’, ‘x’, ‘q’, ‘z’, ‘e’, ‘c’를 전송하면 그리퍼의 위치와 각도를 이동시킬 수 있습니다.

그리퍼의 위치가 변경되면 좌측 사진과 같이 이동한 위치에 대한 내용이 출력됩니다.

 

 

 

 

//////////////  모터 구동 관련 선언
#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 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};

// sync write 용 상수, 구조체 정의 및 인스턴스화
const uint16_t SW_START_ADDR = 112; // sync write start 주소
const uint16_t SW_DATA_SIZE = 8; // sync write data 길이
typedef struct sw_data{ // 모터에 데이터를 쓰기 위한 구조체 정의
  int32_t profile_velocity;
  int32_t goal_position;
} __attribute__((packed)) sw_data_t;

sw_data_t sw_data[ARM_DXL_ID_CNT]; // 모터 개수만큼 인스턴스 생성
DYNAMIXEL::InfoSyncWriteInst_t sw_infos; // syncwrite 정보 인스턴스 생성
// syncWrite할 모터들 정보 인스턴스를 모터 개수만큼 생성
DYNAMIXEL::XELInfoSyncWrite_t info_xels_sw[ARM_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번 모터 회전축과 그리퍼 끝부분 사이의 거리

int16_t X = 0;
int16_t Y = 110;
int16_t Z = 190;
int16_t Angle = 0;

uint8_t unit = 1;
bool isPositionChanged = 1;

//////////////  메인 프로그램

void setup()
{
  Serial.begin(115200);

  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);
  }

  // sync write 준비
  sw_infos.packet.p_buf = nullptr; // nullptr을 전달하면 내부 버퍼를 사용
  sw_infos.packet.is_completed = false; // false로 초기화
  sw_infos.addr = SW_START_ADDR; // 컨트롤 테이블에 sync write를 시작하는 주소
  sw_infos.addr_length = SW_DATA_SIZE; // sync write하는 데이터 길이
  sw_infos.p_xels = info_xels_sw; // sync write 할 모터 정보
  sw_infos.xel_count = 0; // sync write 할 모터 개수

  sw_data[0].profile_velocity = 1000; // 모터에 write 할 데이터 초기화
  sw_data[0].goal_position = 2048;
  sw_data[1].profile_velocity = 1000;
  sw_data[1].goal_position = 2048;
  sw_data[2].profile_velocity = 1000;
  sw_data[2].goal_position = 2048;
  sw_data[3].profile_velocity = 1000;
  sw_data[3].goal_position = 2048;

  // 모터 정보 리스트에 모터 아이디 설정, 데이터 포인터 지정
  for(int i = 0; i < ARM_DXL_ID_CNT; i++) {
    info_xels_sw[i].id = ARM_DXL_IDS[i];
    info_xels_sw[i].p_data = (uint8_t*)&sw_data[i];
    sw_infos.xel_count++;
  }
  sw_infos.is_info_changed = true; // sync write 정보가 변경됨을 설정

  Serial.println("Target position");
  Serial.print("    X : ");
  Serial.println(X);
  Serial.print("    Y : ");
  Serial.println(Y);
  Serial.print("    Z : ");
  Serial.println(Z);
  Serial.print("    Angle : ");
  Serial.println(Angle);
  Serial.println();
  SetManipulatorInverseMoveForSyncWrite(X, Y, Z, Angle, 2000);
  while(!dxl.syncWrite(&sw_infos)) {}
  
  Serial.println("------------------------");
  Serial.println();
  Serial.println("    Start");
  Serial.println();
  Serial.println("------------------------");
}

void loop()
{
  if( Serial.available() )
  {
    Serial.println();
    Serial.println("------------------------");
    Serial.println();
    
    switch( Serial.read() )
    {
      case '1': // 위치를 변경할 때 한 번에 1씩(조금씩) 변화시킴
        Serial.println("unit : 1");
        unit = 1;
        isPositionChanged = 0;
        break;
      case '2': // 한 번에 10씩 중간 크기로 변화시킴
        Serial.println("unit : 10");
        unit = 10;
        isPositionChanged = 0;
        break;
      case '3': // 한 번에 20씩 많이 변화시킴
        Serial.println("unit : 20");
        unit = 20;
        isPositionChanged = 0;
        break;
        
      case 'd': // x축 증가
        X += unit;
        break;
      case 'a': // x축 감소
        X -= unit;
        break;
        
      case 'w': // y축 증가
        Y += unit;
        break;
      case 'x': // y축 감소
        if((Y-unit) >= 0)
          Y -= unit;
        break;
        
      case 'q': // z축 증가
        Z += unit;
        break;
      case 'z': // z축 감소
        if((Z-unit) >= 0)
          Z -= unit;
        break;
        
      case 'e': // 그리퍼 각도 증가
        if((Angle+unit) <= 90)
          Angle += unit;
        break;
      case 'c': // 그리퍼 각도 감소
        if((Angle-unit) >= -90)
          Angle -= unit;
        break;
        
      default:
        Serial.println("Wrong input");
        isPositionChanged = 0;
    }

    if( isPositionChanged )
    {
      Serial.println("Position has changed");
      Serial.print("    X : ");
      Serial.println(X);
      Serial.print("    Y : ");
      Serial.println(Y);
      Serial.print("    Z : ");
      Serial.println(Z);
      Serial.print("    Angle : ");
      Serial.println(Angle);
      Serial.println();
      
      SetManipulatorInverseMoveForSyncWrite(X, Y, Z, Angle, 2000);
      while(!dxl.syncWrite(&sw_infos)) {}
    }
    isPositionChanged = 1;

    Serial.println();
    Serial.println("------------------------");
    Serial.println();
  } // if( Serial.available() )의 끝
}

/*
 * 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_data[0].goal_position = motor1GoalPosition;
    sw_data[0].profile_velocity = operatingTime;
  }
  if (motor2GoalPosition != -1) {
    sw_data[1].goal_position = motor2GoalPosition;
    sw_data[1].profile_velocity = operatingTime;
  }
  if (motor3GoalPosition != -1) {
    sw_data[2].goal_position = motor3GoalPosition;
    sw_data[2].profile_velocity = operatingTime;
  }
  if (motor4GoalPosition != -1) {
    sw_data[3].goal_position = motor4GoalPosition;
    sw_data[3].profile_velocity = operatingTime;
  }
  
  sw_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);
}

 

 

728x90
728x90

'소프트웨어 > Arduino' 카테고리의 다른 글

돌아다니는 자동차 로봇 - 피드백(Feedback) 제어 실습  (0) 2026.04.12
모터 제어와 4방향 주행 - PWM / DC모터 제어  (0) 2026.04.12
4자유도 매니퓰레이터의 위치 제어 #1  (0) 2026.03.25
스마트 액츄에이터의 각도 제어  (0) 2026.03.25
메카넘휠을 사용한 모바일베이스의 주행 원리  (0) 2026.03.18
댓글 , 엮인글
MakingRobot
블로그 이미지 MakingRobotWizard 님의 블로그
MENU
CATEGORY
VISITOR 오늘 / 전체

티스토리툴바