-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.py
54 lines (49 loc) · 1.57 KB
/
main.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
import dynamixel_sdk as dxl
import cv2
ADDR_MX_TORQUE_ENABLE = 24
ADDR_MX_CW_COMPLIANCE_MARGIN = 26
ADDR_MX_CCW_COMPLIANCE_MARGIN = 27
ADDR_MX_CW_COMPLIANCE_SLOPE = 28
ADDR_MX_CCW_COMPLIANCE_SLOPE = 29
ADDR_MX_GOAL_POSITION = 30
ADDR_MX_MOVING_SPEED = 32
ADDR_MX_PRESENT_POSITION = 36
ADDR_MX_PUNCH = 48
PROTOCOL_VERSION = 1.0
DXL_IDS = [1, 2, 3, 4]
DEVICENAME = 'COM5'
BAUDRATE = 1000000
TORQUE_ENABLE = 1
TORQUE_DISABLE = 0
portHandler = dxl.PortHandler(DEVICENAME)
packetHandler = dxl.PacketHandler(PROTOCOL_VERSION)
portHandler.openPort()
portHandler.setBaudRate(BAUDRATE)
for DXL_ID in DXL_IDS:
packetHandler.write1ByteTxRx(
portHandler, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
packetHandler.write2ByteTxRx(
portHandler, DXL_ID, ADDR_MX_CW_COMPLIANCE_MARGIN, 0)
packetHandler.write2ByteTxRx(
portHandler, DXL_ID, ADDR_MX_CCW_COMPLIANCE_MARGIN, 0)
packetHandler.write1ByteTxRx(
portHandler, DXL_ID, ADDR_MX_CW_COMPLIANCE_SLOPE, 32)
packetHandler.write1ByteTxRx(
portHandler, DXL_ID, ADDR_MX_CCW_COMPLIANCE_SLOPE, 32)
packetHandler.write2ByteTxRx(
portHandler, DXL_ID, ADDR_MX_MOVING_SPEED, 100)
cap = cv2.VideoCapture(0)
# Check if the camera opened successfully
if not cap.isOpened():
print("Error: Could not open camera.")
exit()
# Capture a single frame
ret, frame = cap.read()
# Check if the frame was captured successfully
if not ret:
print("Error: Could not read frame.")
exit()
# Save the captured frame to an image file
cv2.imwrite("captured_image.jpg", frame)
# Release the camera
cap.release()