duoke

# -*- coding: utf-8 -*-
"""
Created on Wed Nov 19 16:58:24 2025

@author: luogan
"""

#/usr/bin/python
import duco_cobot

# robot = duco_cobot.DucoCobot(ip="192.168.192.207", port=7003)
robot = duco_cobot.DucoCobot(ip="192.168.192.246", port=8000)
robot.open()
tcp_pose = robot.get_tcp_pose()
print("TCP姿态：", tcp_pose)  # 输出：[x, y, z, rx, ry, rz]

#1. 机器人基础控制（电源、使能、启停）

# 上电（block=True：阻塞等待完成，返回0表示成功）
if robot.power_on(block=True) != 0:
    print("上电失败")

# 使能机器人（允许运动）
robot.enable(block=True)

# 检查机器人是否正在运动
print("机器人是否运动中：", robot.robotmoving())  # 返回True/False

# 急停（紧急情况下调用）
# robot.stop(block=True)

# 下电（使用完后调用）
# robot.power_off(block=True)

'''
2. OP 结构体使用（运动触发参数）
多数运动函数（如movej、movel）需要OP类型参数，用于设置运动触发条件（如 IO 触发、距离触发）。
'''

# 创建OP实例并设置参数
op = duco_cobot.OP()
op.trig_io_1 = 1  # 触发IO1（1表示启用）
op.trig_dist_1 = 0.05  # 触发距离1（单位：米）
op.trig_time_1 = 1000  # 触发时间1（单位：毫秒）
op.time_or_dist_1 = 0  # 0=距离触发，1=时间触发

# 查看OP参数
print("当前OP触发IO1：", op.trig_io_1)



#（1）关节空间运动（movej）
#按关节角度列表运动，适用于已知关节位置的场景。
# 目标关节角度（6轴机器人示例，单位：弧度）
joints = [0, -0.5, 0.8, 0, 0.3, 0]  # [关节1, 关节2, ..., 关节6]

# 运动参数
v = 0.5  # 速度比例（0-1，1=最大速度）
a = 0.5  # 加速度比例（0-1，1=最大加速度）
r = 0.01  # 过渡半径（米，用于平滑拐角）

# 执行关节运动（使用上面定义的op参数）
result = robot.movej(
    joints_list=joints,
    v=v,
    a=a,
    r=r,
    block=True,  # 阻塞等待运动完成
    op=op
)

if result == 0:
    print("关节运动完成")
else:
    print("运动失败，错误码：", result)
    
#（2）线性运动（movel）
#TCP（工具中心点）沿直线运动，适用于精确路径控制。
# 目标TCP姿态（[x, y, z, rx, ry, rz]，单位：米/弧度）
# x/y/z：空间位置；rx/ry/rz：姿态（欧拉角）
target_pose = [0.3, 0.2, 0.5, 3.14, 0, 1.57]

# 运动参数
v = 0.2  # 线速度（米/秒）
a = 0.2  # 线加速度（米/秒²）
r = 0.005  # 过渡半径
q_near = [0, 0, 0, 0, 0, 0]  # 临近关节位置（用于逆解参考）
tool = "default"  # 工具名（默认工具）
wobj = "default"  # 工件坐标系名（默认坐标系）

# 执行线性运动
robot.movel(
    p=target_pose,
    v=v,
    a=a,
    r=r,
    q_near=q_near,
    tool=tool,
    wobj=wobj,
    block=True,
    op=op
)

#（3）圆弧运动（movec）
#TCP 沿圆弧运动（通过起点、中间点、终点定义圆弧）。

p1 = [0.3, 0.2, 0.5, 3.14, 0, 1.57]  # 圆弧起点
p2 = [0.4, 0.3, 0.5, 3.14, 0, 1.57]  # 圆弧终点
mode = 0  # 圆弧方向（0=顺时针，1=逆时针）

robot.movec(
    p1=p1,
    p2=p2,
    v=0.2,
    a=0.2,
    r=0.01,
    mode=mode,
    q_near=q_near,
    tool=tool,
    wobj=wobj,
    block=True,
    op=op
)

#4. 数据获取（姿态、关节、力反馈等）


# 获取当前TCP姿态（[x,y,z,rx,ry,rz]）
tcp_pose = robot.get_tcp_pose()
print("当前TCP姿态：", tcp_pose)  # 输出示例：[0.3, 0.2, 0.5, 3.14, 0, 1.57]

# 获取当前关节位置（弧度）
joints = robot.get_actual_joints_position()
print("当前关节位置：", joints)  # 输出示例：[0, -0.5, 0.8, 0, 0.3, 0]

# 获取TCP力反馈（[fx, fy, fz, tx, ty, tz]，单位：N/N·m）
tcp_force = robot.get_tcp_force()
print("TCP力反馈：", tcp_force)

# 获取机器人状态（状态码列表）
robot_state = robot.get_robot_state()
print("机器人状态：", robot_state)




'''
四、常见问题与注意事项
连接失败：检查 IP 和端口是否正确，机器人是否联网，防火墙是否放行端口。
运动参数范围：v和a通常建议在0.1-0.8之间（避免过急运动），r（过渡半径）建议≥0.005 米。
单位说明：
位置：米（x/y/z）、弧度（rx/ry/rz、关节角度）；
速度：米 / 秒（线性）、弧度 / 秒（关节）；
力：牛顿（N）、牛・米（N・m）。
错误处理：所有返回int32_t的函数，0表示成功，非 0 表示错误，可通过get_last_error()获取详情。
非阻塞任务：当block=False时，函数返回任务 ID，可通过get_noneblock_taskstate(id)查询状态（0 = 完成，1 = 执行中，-1 = 失败）。
通过以上示例，可覆盖机器人控制的绝大多数场景。如需更详细的某类功能（如 Modbus 通信、系统参数配置），可进一步补充说明。
'''

import duco_cobot
import time

# --- 1. 初始化机器人对象 ---
# 替换为你的机器人 IP 地址和端口号
robot_ip = "192.168.192.246"
robot_port = 7003

try:
    # 创建 DucoCobot 实例
    robot = duco_cobot.DucoCobot(robot_ip, robot_port)
    print(f"成功创建机器人实例，准备连接到 {robot_ip}:{robot_port}")

    # --- 2. 打开网络连接 ---
    if robot.open() == 0:
        print("机器人连接成功！")

        # --- 3. 机器人上电并使能 ---
        # 阻塞模式上电，直到上电完成
        if robot.power_on(block=True) == 0:
            print("机器人上电成功")

            # 阻塞模式使能，直到使能完成
            if robot.enable(block=True) == 0:
                print("机器人使能成功，准备执行运动")

                # --- 4. 准备 servoj 参数 ---
                # a. 目标关节位置 (joints_list)
                #    假设是一个6轴机器人，单位是弧度 (rad)
                #    这里我们让机器人运动到一个中间姿态
                #    你可以根据实际需求修改这些值
                target_joints = [0.0, 0.02, 0.3, 0.0, 0.0, 0.0]
                print(f"目标关节位置: {target_joints}")

                # b. 速度 (v)
                #    单位通常是 rad/s，根据机器人手册确认
                speed = 0.1  # 设置一个适中的速度

                # c. 加速度 (a)
                #    单位通常是 rad/s²
                acceleration = 0.2  # 设置一个适中的加速度

                # d. 是否阻塞 (block)
                #    False: 非阻塞模式，函数立即返回，运动在后台进行
                #    True:  阻塞模式，函数直到运动完成才返回
                #    对于 servoj，通常使用非阻塞模式，以便可以连续发送新的目标点
                is_blocking = False

                # e. 比例增益 (kp)
                #    用于调整伺服响应的比例系数
                kp = 500.0  # 示例值，请根据实际控制需求调整

                # f. 微分增益 (kd)
                #    用于调整伺服响应的微分系数
                kd = 5.0    # 示例值，请根据实际控制需求调整

                # g. 速度平滑系数 (smooth_vel)
                #    用于平滑速度曲线，减少抖动
                smooth_vel = 0.1  # 示例值

                # h. 加速度平滑系数 (smooth_acc)
                #    用于平滑加速度曲线，减少冲击
                smooth_acc = 0.1  # 示例值

                # --- 5. 调用 servoj 进行关节伺服控制 ---
                print("执行 servoj 运动...")
                status = robot.servoj(
                    joints_list=target_joints,
                    v=speed,
                    a=acceleration,
                    block=is_blocking,
                    kp=kp,
                    kd=kd,
                    smooth_vel=smooth_vel,
                    smooth_acc=smooth_acc
                )

                if status == 0:
                    print("servoj 运动指令已成功发送")

                    # 如果是在非阻塞模式下，我们需要等待一段时间让机器人完成运动
                    # 这里只是一个简单的延时示例，实际应用中可能需要更复杂的逻辑
                    # 比如检查机器人是否到达目标位置或运动是否停止
                    print("等待机器人运动到目标位置...")
                    time.sleep(3)  # 等待3秒

                    # --- 6. 运动后可以获取当前关节位置进行验证 ---
                    current_joints = robot.get_actual_joints_position()
                    print(f"运动后实际关节位置: {current_joints}")

                else:
                    print(f"servoj 运动指令发送失败，状态码: {status}")

                # --- 7. 禁用和下电 ---
                robot.disable(block=True)
                print("机器人已禁用")
                robot.power_off(block=True)
                print("机器人已下电")

            else:
                print("机器人使能失败")
        else:
            print("机器人上电失败")

        # --- 8. 关闭网络连接 ---
        robot.close()
        print("机器人连接已关闭")

    else:
        print(f"机器人连接失败，请检查 IP: {robot_ip} 和端口: {robot_port}")

except Exception as e:
    print(f"发生异常: {e}")