모터 제어 part.3

오늘은 집게 부분을 제어해 볼 예정이다.

오늘의 목표는 집게 부분의 범위를 찾고, 키보드로 제어하는 것이다.

현재 집게 부분이 실험하다 부셔져서 부셔진 채로 진행하고 있다. 이점 양해 바란다

 

먼저 집게 부분의 모터는 몸통 부분과 달리 XH430-V350-R를 사용한다.

https://emanual.robotis.com/docs/kr/dxl/x/xh430-v350/

ROBOTIS e-Manual

메뉴얼은 다음 링크를 사용하자

 

하면서 알아낸 정보를 정리해 두었다. ( 우리 모터에만 해당 )

- 1800을 최소로 잡을 경우 ( 맨 끝이라고 판단) -> 방향은 기본 값 ( 양수 시계 반대방향 )
- 오차 최대 4 ( 임계값때매 값이 늘어는 거임 한마디로 1800이 최소면 임계 30 주면 1826까지 + 최대값에선 -30 이니 2000주면 1966까진 내려감)
- 임계 20에서 1800 ~ 2550 375당 한칸 -> 한칸 더 주면 뿌셔짐 최대 돌리는 각도는 2890정도인데 이건 최대 일뿐 뿌셔짐 최저도 1700은 안 1750가 최저일뿐 최저로 가면 뿌셔짐

 

총 3가지로 테스트를 하고 진행해 보겠다.

테스트는 다음과 같다.

- 집게 부분의 모터 제어

- 모터의 범위

- 키보드로 제어

 

이제 시작해 보겠다

 

집게 부분의 모터 제어

이 부분은 딱히 어려울게 없는게 part 1, 2에서 모터를 제어해보아서 금방 하였다.

https://youtube.com/shorts/7VbyBBtwDBs

 

 

모터 범위

범위를 찾고 적용한 영상이다

https://youtu.be/QGa4YW6siDU

 

 

 

키보드로 제어

위에서 진행한 부분을 전부 키보드로 구현 및 제어해 보았다.

해당 코드는 아래에 첨부한다.

# -*- coding: utf-8 -*-

import os
import time

if os.name == 'nt':
    import msvcrt
    def getch():
        return msvcrt.getch().decode()
else:
    import sys, tty, termios
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    def getch():
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch

from dynamixel_sdk import *

# DYNAMIXEL 설정
ADDR_TORQUE_ENABLE          = 64
ADDR_GOAL_POSITION          = 116
ADDR_PRESENT_POSITION       = 132

ADDR_PROFILE_ACCELERATION = 108  # Profile Acceleration address
ADDR_PROFILE_VELOCITY = 112      # Profile Velocity address

ADDR_DRIVE_MODE = 10
ADDR_OPERATING_MODE = 11

DXL_MINIMUM_POSITION_VALUE  = 1000         # Refer to the Minimum Position Limit of product eManual 0
DXL_MAXIMUM_POSITION_VALUE  = 3000      # Refer to the Maximum Position Limit of product eManual 4095

DXL_REVERSE_MODE_VALUE = 0x01           # 시계방향(시계 방향)
DXL_NORMAL_MODE_VALUE = 0x00            # 시계방향(반시계 방향) - 디폴트

DXL_REVERSE_SLAVE_MODE_VALUE = 0x02     # slave 설정

BAUDRATE = 57600

PROTOCOL_VERSION = 2.0

DXL_ID  = 1
DXL2_ID = 2
DXL3_ID = 3

DEVICENAME = '/dev/ttyARM'

TORQUE_ENABLE = 1
TORQUE_DISABLE = 0
DXL_MOVING_STATUS_THRESHOLD = 20    # Dynamixel moving status threshold

index_1 = 2000                                    
index_2 = 230

portHandler = PortHandler(DEVICENAME)

packetHandler = PacketHandler(PROTOCOL_VERSION)

# Open port
if portHandler.openPort():
    print("Succeeded to open the port")
else:
    print("Failed to open the port")
    print("Press any key to terminate...")                                                                                                    
    getch()
    quit()


# Set port baudrate
if portHandler.setBaudRate(BAUDRATE):
    print("Succeeded to change the baudrate")
else:
    print("Failed to change the baudrate")
    print("Press any key to terminate...")
    getch()
    quit()

# Set Profile Velocity and Acceleration
def set_profile_velocity_acceleration(velocity, acceleration):
    # Set Profile Velocity
    dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_PROFILE_VELOCITY, velocity)
    if dxl_comm_result != COMM_SUCCESS:
        print(f"Error setting velocity: {packetHandler.getTxRxResult(dxl_comm_result)}")
    elif dxl_error != 0:
        print(f"Error: {packetHandler.getRxPacketError(dxl_error)}")
    
    # Set Profile Acceleration
    dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_PROFILE_ACCELERATION, acceleration)
    if dxl_comm_result != COMM_SUCCESS:
        print(f"Error setting acceleration: {packetHandler.getTxRxResult(dxl_comm_result)}")
    elif dxl_error != 0:
        print(f"Error: {packetHandler.getRxPacketError(dxl_error)}")
        
# 속도와 가속도 값 설정
PROFILE_VELOCITY = 60     # Adjust this value (rev/min)
PROFILE_ACCELERATION = 20  # Adjust this value (rev/min^2)

# Operating 모드 설정
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, 3)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_OPERATING_MODE, 3)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL3_ID, ADDR_OPERATING_MODE, 3)

# 드라이브 모드 (master-slave 설정)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL3_ID, ADDR_DRIVE_MODE, DXL_REVERSE_SLAVE_MODE_VALUE)
if dxl_comm_result != COMM_SUCCESS:
    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
    print("%s" % packetHandler.getRxPacketError(dxl_error))
    
# 속도 및 가속도 조절
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION)
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION)

# Enable Dynamixel#1 Torque    
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
if dxl_comm_result != COMM_SUCCESS:
    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
    print("%s" % packetHandler.getRxPacketError(dxl_error))
else:
    print("Dynamixel#%d has been successfully connected" % DXL_ID)
    
# Enable Dynamixel#2 Torque
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
if dxl_comm_result != COMM_SUCCESS:
    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
    print("%s" % packetHandler.getRxPacketError(dxl_error))
else:
    print("Dynamixel#%d has been successfully connected" % DXL2_ID)

# 초기 위치로 이동
# Dynamixel#1
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, index_1)
if dxl_comm_result != COMM_SUCCESS:
    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
    print("%s" % packetHandler.getRxPacketError(dxl_error))

# Dynamixel#2
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_ID, ADDR_GOAL_POSITION, index_2)
if dxl_comm_result != COMM_SUCCESS:
    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
    print("%s" % packetHandler.getRxPacketError(dxl_error))
    

# 움직임 변화 값
value = 50

while 1:        
    print("Press any key to continue! (or press ESC to quit!)")
    input_key = getch()
    print(f"{input_key}")

    # 1번 DXL이 왼쪽으로 회전    
    if input_key == '1':
        index_1 += value
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, 3)
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
        dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, index_1)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))

    # 1번 DXL이 오른쪽으로 회전            
    elif input_key == '2':
        index_1 -= value
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, 3)
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
        dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, index_1)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))
    
    # 2번 DXL이 앞으로 회전
    elif input_key == '3': 
        dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION)
        print(f"{dxl_present_position}")
        index_2 += value
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_OPERATING_MODE, 3)
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
        dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_ID, ADDR_GOAL_POSITION, index_2)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))
            
    # 2번 DXL이 뒤로 회전        
    elif input_key == '4':
        index_2 -= value
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_OPERATING_MODE, 3)
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
        dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_ID, ADDR_GOAL_POSITION, index_2)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))
            
    elif input_key == '`':
        index_1 = 2000
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, 3)
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
        dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, index_1)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))

    elif input_key == chr(0x1b):
        if index_1 == 2000 and index_2 == 230:
            break
        else:
            # 1번 DXL 초기 위치로 이동
            index_1 = 2000
            dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_OPERATING_MODE, 3)
            dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
            dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, index_1)
        
            if dxl_comm_result != COMM_SUCCESS:
                print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
            elif dxl_error != 0:
                print("%s" % packetHandler.getRxPacketError(dxl_error))

            # 2번 DXL 초기 위치로 이동                
            index_2 = 230
            dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_OPERATING_MODE, 3)
            dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
            dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_ID, ADDR_GOAL_POSITION, index_2)
        
            if dxl_comm_result != COMM_SUCCESS:
                print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
            elif dxl_error != 0:
                print("%s" % packetHandler.getRxPacketError(dxl_error))
            
            dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION)

            time.sleep(0.5)
            break
            
    else:
        print("키 입력을 확인해 주세요.")

    
    


# 토크 비활성화 및 포트 닫기
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_TORQUE_ENABLE, TORQUE_DISABLE)
if dxl_comm_result != COMM_SUCCESS:
    print(f"Failed to disable torque: {packetHandler.getTxRxResult(dxl_comm_result)}")
elif dxl_error != 0:
    print(f"Error: {packetHandler.getRxPacketError(dxl_error)}")

portHandler.closePort()

 

주석으로 최대한 설명해 두었으니 참고바란다.