다양한 제품 리뷰/ROBOT_ARM
모터 제어 part.4
찬영_00
2024. 10. 28. 23:55
오늘은 전체적으로 모터 9개를 전부 키보드로 제어해 보려한다.
그중 마스터 슬레이브인 듀얼모드는 총 2개이다.
먼저 step by step으로 진행하겠다.
- 모터의 초기값 전부 구하기
- 모터 전부 제어 및 범위 구하기
- 모터 전부 키보드로 제어해보기
위 2개가 끝이다. 좀 더 세분화 할 순 있지만 이전 포스터를 보면 충분히 할 수 있기에 여기에 따로 포스팅 하진 않겠다.
모터의 초기값으로 돌리기
모터 전부 제어 및 범위 구하기
모터 1번은 0 ~ 180 까지
모터 2,3번은 0 ~ 120 까지
모터 4,5번은 0 ~ 180 까지
모터 6,7,8번은 0 ~ 360 까지
모터 9번은 대략 80까지
맞춰주면 될 것 같다.
모터 전부 키보드로 제어해보기
실행하다가 또 하나 부셔 먹었다..
정신 차리고 다시 코드를 짜겠다
일단 키보드 제어에는 1,2,4,6,7,8,9,0이 들어가고, esc가 들어간다.
1을 누르면 1번 모터만 제어하도록 하고,
2을 누르면 2번 모터만 제어하도록 한다.
마찬가지로 위 숫자는 모터 순서이고, 선택한 모터 제어에 들어가면 .과 /로 제어가 가능하다.
그리고 그 모터의 제어를 벗어나려면 0을 누르면 되고
esc는 모터 제어 모드를 벗어나는 키로, 모터를 전부 초기 위치로 되돌린다.
코드는 다음과 같다.
#!/usr/bin/env python
# -*- 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 * # Uses Dynamixel SDK library
ADDR_TORQUE_ENABLE = 64
ADDR_GOAL_POSITION = 116
ADDR_PRESENT_POSITION = 132
ADDR_DRIVE_MODE = 10
ADDR_OPERATING_MODE = 11
ADDR_PROFILE_ACCELERATION = 108 # Profile Acceleration address
ADDR_PROFILE_VELOCITY = 112 # Profile Velocity address
DXL_REVERSE_MODE_VALUE = 0x01 # 시계방향(시계 방향)
DXL_NORMAL_MODE_VALUE = 0x00 # 시계방향(반시계 방향) - 디폴트
DXL_NORMAL_SLAVE_MODE_VALUE = 0x02 # slave 설정
DXL_REVERSE_SLAVE_MODE_VALUE = 0x03
# 0 ~ 4095 범위 안에서 설정
DXL_MINIMUM_POSITION_VALUE_1 = 1000
DXL_MAXIMUM_POSITION_VALUE_1 = 3000
DXL_MINIMUM_POSITION_VALUE_2 = 230
DXL_MAXIMUM_POSITION_VALUE_2 = 2277
DXL_MINIMUM_POSITION_VALUE_4 = 0
DXL_MAXIMUM_POSITION_VALUE_4 = 2047
DXL_MINIMUM_POSITION_VALUE_6 = 0
DXL_MAXIMUM_POSITION_VALUE_6 = 4095
DXL_MINIMUM_POSITION_VALUE_7 = 0
DXL_MAXIMUM_POSITION_VALUE_7 = 4095
DXL_MINIMUM_POSITION_VALUE_8 = 0
DXL_MAXIMUM_POSITION_VALUE_8 = 4095
DXL_MINIMUM_POSITION_VALUE_9 = 1800
DXL_MAXIMUM_POSITION_VALUE_9 = 2550
BAUDRATE = 57600
# DYNAMIXEL Protocol Version (1.0 / 2.0)
PROTOCOL_VERSION = 2.0
DXL1_ID = 1
DXL2_ID = 2
DXL3_ID = 3
DXL4_ID = 4
DXL5_ID = 5
DXL6_ID = 6
DXL7_ID = 7
DXL8_ID = 8
DXL9_ID = 9
# Use the actual port assigned to the U2D2.
DEVICENAME = '/dev/ttyARM'
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold
# 키보드 누를 때마다 움직이는 범위
step_1 = 50
step_2 = 50
step_4 = 100
step_6 = 100
step_7 = 100
step_8 = 200
step_9 = 75
# Goal position
dxl_goal_position_1 = [DXL_MINIMUM_POSITION_VALUE_1 + DXL_MOVING_STATUS_THRESHOLD, DXL_MAXIMUM_POSITION_VALUE_1 - DXL_MOVING_STATUS_THRESHOLD]
dxl_goal_position_2 = [DXL_MINIMUM_POSITION_VALUE_2 + DXL_MOVING_STATUS_THRESHOLD, DXL_MAXIMUM_POSITION_VALUE_2 - DXL_MOVING_STATUS_THRESHOLD]
dxl_goal_position_4 = [DXL_MINIMUM_POSITION_VALUE_4 + DXL_MOVING_STATUS_THRESHOLD, DXL_MAXIMUM_POSITION_VALUE_4 - DXL_MOVING_STATUS_THRESHOLD]
dxl_goal_position_6 = [DXL_MINIMUM_POSITION_VALUE_6 + DXL_MOVING_STATUS_THRESHOLD, DXL_MAXIMUM_POSITION_VALUE_6 - DXL_MOVING_STATUS_THRESHOLD]
dxl_goal_position_7 = [DXL_MINIMUM_POSITION_VALUE_7 + DXL_MOVING_STATUS_THRESHOLD, DXL_MAXIMUM_POSITION_VALUE_7 - DXL_MOVING_STATUS_THRESHOLD]
dxl_goal_position_8 = [DXL_MINIMUM_POSITION_VALUE_8 + DXL_MOVING_STATUS_THRESHOLD, DXL_MAXIMUM_POSITION_VALUE_8 - DXL_MOVING_STATUS_THRESHOLD]
dxl_goal_position_9 = [DXL_MINIMUM_POSITION_VALUE_9 + DXL_MOVING_STATUS_THRESHOLD, DXL_MAXIMUM_POSITION_VALUE_9 - DXL_MOVING_STATUS_THRESHOLD]
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()
# 속도 및 가속도 세팅 함수
def set_profile_velocity_acceleration(velocity, acceleration, ID):
# Set Profile Velocity
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, 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, 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)}")
#초기 위치 함수
def set_init_position(ID, minPosition):
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, ID, ADDR_GOAL_POSITION, minPosition)
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
# 토크 설정 함수
def Enable_Torque(T_ENABLE, ID):
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, ID, ADDR_TORQUE_ENABLE, T_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 has been successfully connected")
# 토크 해제 함수
def Disable_Torque(T_ENABLE, ID):
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, ID, ADDR_TORQUE_ENABLE, T_ENABLE)
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
# 드라이브 모드 (master-slave 설정)
def durl_mode(ID):
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, 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))
# 속도와 가속도 값 설정
PROFILE_VELOCITY = 60 # Adjust this value (rev/min)
PROFILE_ACCELERATION = 20 # Adjust this value (rev/min^2)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_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, DXL4_ID, ADDR_OPERATING_MODE, 3)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL6_ID, ADDR_OPERATING_MODE, 3)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL7_ID, ADDR_OPERATING_MODE, 3)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL8_ID, ADDR_OPERATING_MODE, 3)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL9_ID, ADDR_OPERATING_MODE, 3)
# 방향 설정
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_DRIVE_MODE, DXL_NORMAL_MODE_VALUE)
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL4_ID, ADDR_DRIVE_MODE, DXL_REVERSE_MODE_VALUE)
#슬레이브 모드 ( 듀얼모드)
durl_mode(DXL3_ID)
durl_mode(DXL5_ID)
# 속도 및 가속도 조절
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION, DXL1_ID)
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION, DXL2_ID)
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION, DXL4_ID)
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION, DXL6_ID)
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION, DXL7_ID)
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION, DXL8_ID)
set_profile_velocity_acceleration(PROFILE_VELOCITY, PROFILE_ACCELERATION, DXL9_ID)
# 초기 위치로 이동
set_init_position(DXL1_ID, 2000)
set_init_position(DXL2_ID, dxl_goal_position_2[0])
set_init_position(DXL4_ID, dxl_goal_position_4[0])
set_init_position(DXL6_ID, dxl_goal_position_6[0])
set_init_position(DXL7_ID, dxl_goal_position_7[0])
set_init_position(DXL8_ID, dxl_goal_position_8[0])
set_init_position(DXL9_ID, dxl_goal_position_9[0])
# Enable Dynamixel Torque
Enable_Torque(TORQUE_ENABLE, DXL1_ID)
Enable_Torque(TORQUE_ENABLE, DXL2_ID)
Enable_Torque(TORQUE_ENABLE, DXL4_ID)
Enable_Torque(TORQUE_ENABLE, DXL6_ID)
Enable_Torque(TORQUE_ENABLE, DXL7_ID)
Enable_Torque(TORQUE_ENABLE, DXL8_ID)
Enable_Torque(TORQUE_ENABLE, DXL9_ID)
while 1:
print("Press any key to continue! (or press ESC to quit!)")
key_input = getch()
if key_input == chr(0x1b): # ESC 키를 누르면 종료
set_init_position(DXL1_ID, 2000)
set_init_position(DXL2_ID, dxl_goal_position_2[0])
set_init_position(DXL4_ID, dxl_goal_position_4[0])
set_init_position(DXL6_ID, dxl_goal_position_6[0])
set_init_position(DXL7_ID, dxl_goal_position_7[0])
set_init_position(DXL8_ID, dxl_goal_position_8[0])
set_init_position(DXL9_ID, dxl_goal_position_9[0])
break
# 1번 모터 제어
elif key_input == '1':
print("모터{key_input}에 들어왔습니다. 종료하려면 0을 누르세요. 움직이려면 .와 /으로 움직이세요")
while 1:
key_input_1 = getch()
if key_input_1 == '0':
break
elif key_input_1 == '.':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL1_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position + step_1 >= dxl_goal_position_1[1]:
print(f"max angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL1_ID, ADDR_GOAL_POSITION, present_position + step_1)
elif key_input_1 == '/':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL1_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position - step_1 <= dxl_goal_position_1[0]:
print(f"min angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL1_ID, ADDR_GOAL_POSITION, present_position - step_1)
# 2번 모터 제어
elif key_input == '2':
print("모터{key_input}에 들어왔습니다. 종료하려면 0을 누르세요. 움직이려면 .와 /으로 움직이세요")
while 1:
key_input_1 = getch()
if key_input_1 == '0':
break
elif key_input_1 == '.':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL2_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position + step_2 >= dxl_goal_position_2[1]:
print(f"max angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_ID, ADDR_GOAL_POSITION, present_position + step_2)
elif key_input_1 == '/':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL2_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position - step_2 <= dxl_goal_position_2[0]:
print(f"min angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL2_ID, ADDR_GOAL_POSITION, present_position - step_2)
# 4번 모터 제어
elif key_input == '4':
print("모터{key_input}에 들어왔습니다. 종료하려면 0을 누르세요. 움직이려면 .와 /으로 움직이세요")
while 1:
key_input_1 = getch()
if key_input_1 == '0':
break
elif key_input_1 == '.':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL4_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position + step_4 >= dxl_goal_position_4[1]:
print(f"max angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL4_ID, ADDR_GOAL_POSITION, present_position + step_4)
elif key_input_1 == '/':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL4_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position - step_4 <= dxl_goal_position_4[0]:
print(f"min angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL4_ID, ADDR_GOAL_POSITION, present_position - step_4)
# 6번 모터 제어
elif key_input == '6':
print("모터{key_input}에 들어왔습니다. 종료하려면 0을 누르세요. 움직이려면 .와 /으로 움직이세요")
while 1:
key_input_1 = getch()
if key_input_1 == '0':
break
elif key_input_1 == '.':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL6_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position + step_6 >= dxl_goal_position_6[1]:
print(f"max angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL6_ID, ADDR_GOAL_POSITION, present_position + step_6)
elif key_input_1 == '/':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL6_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position - step_6 <= dxl_goal_position_6[0]:
print(f"min angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL6_ID, ADDR_GOAL_POSITION, present_position - step_6)
# 7번 모터 제어
elif key_input == '7':
print("모터{key_input}에 들어왔습니다. 종료하려면 0을 누르세요. 움직이려면 .와 /으로 움직이세요")
while 1:
key_input_1 = getch()
if key_input_1 == '0':
break
elif key_input_1 == '.':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL7_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position + step_7 >= dxl_goal_position_7[1]:
print(f"max angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL7_ID, ADDR_GOAL_POSITION, present_position + step_7)
elif key_input_1 == '/':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL7_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position - step_7 <= dxl_goal_position_7[0]:
print(f"min angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL7_ID, ADDR_GOAL_POSITION, present_position - step_7)
# 8번 모터 제어
elif key_input == '8':
print("모터{key_input}에 들어왔습니다. 종료하려면 0을 누르세요. 움직이려면 .와 /으로 움직이세요")
while 1:
key_input_1 = getch()
if key_input_1 == '0':
break
elif key_input_1 == '.':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL8_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position + step_8 >= dxl_goal_position_8[1]:
print(f"max angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL8_ID, ADDR_GOAL_POSITION, present_position + step_8)
elif key_input_1 == '/':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL8_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position - step_8 <= dxl_goal_position_8[0]:
print(f"min angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL8_ID, ADDR_GOAL_POSITION, present_position - step_8)
# 9번 모터 제어
elif key_input == '9':
print("모터{key_input}에 들어왔습니다. 종료하려면 0을 누르세요. 움직이려면 .와 /으로 움직이세요")
while 1:
key_input_1 = getch()
if key_input_1 == '0':
break
elif key_input_1 == '.':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL9_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position + step_9 >= dxl_goal_position_9[1]:
print(f"max angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL9_ID, ADDR_GOAL_POSITION, present_position + step_9)
elif key_input_1 == '/':
present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL9_ID, ADDR_PRESENT_POSITION)
print(f"{present_position}")
if present_position - step_9 <= dxl_goal_position_9[0]:
print(f"min angle")
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL9_ID, ADDR_GOAL_POSITION, present_position - step_9)
else :
print("1,2,3,4,5,6,7,8,9,0 중 선택해주세요. 나가려면 esc를 눌러주세요.")
# Disable Dynamixel Torque
#Disable_Torque(TORQUE_DISABLE, DXL1_ID)
#Disable_Torque(TORQUE_DISABLE, DXL2_ID)
#Disable_Torque(TORQUE_DISABLE, DXL4_ID)
#Disable_Torque(TORQUE_DISABLE, DXL6_ID)
#Disable_Torque(TORQUE_DISABLE, DXL7_ID)
#Disable_Torque(TORQUE_DISABLE, DXL8_ID)
#Disable_Torque(TORQUE_DISABLE, DXL9_ID)
# Close port
portHandler.closePort()
코드 함수화도 몇개 했고, 조금씩 구색을 갖추어 간다.
하지만 아직 멀었다..
현재는 python으로 하고 있지만, 내 주력 언어인 C++로 구현할 예정이다.
이렇게 모터를 제어하는 부분은 전부 끝이 났다.
이제 이걸로 어떻게 활용할지는 생각해보고, 일단 C++로 구현을 목적으로 두겠다.