【一越智能并联机器人】读取编码器值并发送视觉抓取坐标指令到机器人

## 一越智能并联机器人读取编码器并发送视觉定位坐标示例
### 1. 功能概述
本代码实现了一越智能并联机器人（型号X1000）的基本坐标发送功能。基于【文档内容】中描述的一越智能机器人结合视觉控制方案，该模块提供了机器人坐标指令发送的核心功能。

## 2. 核心代码实现
```
-- 一越智能并联机器人坐标发送功能模块
-- 基于视觉网络通信协议（地址0x01）

--[[模块功能：读取编码器值并发送坐标指令到机器人
通信协议：基于一越智能机器人常规通信协议]]
-- 编码器值读取函数
function readEncoder(id)
    local byteArrayVal = LuaString:ToByteArray("30 30 00 00 00 22 01 06 00 0E 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00", " ")
    local state = LuaCommu:WriteByteArray(id, byteArrayVal)
    LuaSystem:WaitMs(100)
    
    local byteArrayVal = LuaCommu:ReadByteArray(id)
    
    if byteArrayVal then
        LuaLog:Debug("编码器原始响应: " .. LuaString:FromByteArray(byteArrayVal, " "))
    end
    
    if byteArrayVal and byteArrayVal:size() &gt;= 4 then
        local size = byteArrayVal:size()
        local midByte = byteArrayVal:mid(size-4, 4)
        LuaLog:Debug("编码器值字节: " .. LuaString:FromByteArray(midByte, " "))
        return midByte
    end
    
    return nil
end

-- 主函数：发送坐标到机器人
-- 参数说明：
--   robotId：机器人通信通道ID
--   x, y, z, a：坐标值（单位：mm, 弧度）
--   encoderChannel：编码器通道
function sendCoordinateToRobot(robotId, x, y, z, a, encoderChannel)
    -- 构造指令头部
    local headCode = LuaString:ToByteArray("30 30 00 00", " ")
    local byteCountCode = LuaString:ToByteArray("00 22", " ")  -- 34字节
    local commandCode = LuaString:ToByteArray("01 06", " ")
    local addressCode = LuaString:ToByteArray("00 01", " ")  -- 地址0x01
    
    -- 坐标值乘以1000
    local scaledX = math.floor(x * 1000)
    local scaledY = math.floor(y * 1000)
    local scaledZ = math.floor(z * 1000)
    local scaledA = math.floor(a * 1000)
    
    LuaLog:Debug(string.format("坐标转换: X=%.3f-&gt;%d, Y=%.3f-&gt;%d, Z=%.3f-&gt;%d, A=%.3f-&gt;%d", 
        x, scaledX, y, scaledY, z, scaledZ, a, scaledA))
    
    -- 转换为字节数组
    local xBytes = IntByteOrder(scaledX)
    local yBytes = IntByteOrder(scaledY)
    local zBytes = IntByteOrder(scaledZ)
    local aBytes = IntByteOrder(scaledA)
    
    -- 读取编码器值
    local encBytes = readEncoder(encoderChannel or 0)
    if not encBytes then
        LuaLog:Debug("读取编码器失败")
        return false
    end
    
    -- 转换字节序
    local convertedEncBytes = convertOrder(encBytes, 3)
    local retFlag = LuaString:ToByteArray("00 00 00 00", " ")
    local retBytes = convertOrder(retFlag, 3)
    
    -- 构造完整指令
    local writeVal = QByteArray()
    writeVal:appendByteArray(headCode)
    writeVal:appendByteArray(byteCountCode)
    writeVal:appendByteArray(commandCode)
    writeVal:appendByteArray(addressCode)
    writeVal:appendByteArray(xBytes)
    writeVal:appendByteArray(yBytes)
    writeVal:appendByteArray(zBytes)
    writeVal:appendByteArray(aBytes)
    writeVal:appendByteArray(convertedEncBytes)
    writeVal:appendByteArray(retBytes)
    
    -- 验证指令长度
    local cmdSize = writeVal:size()
    LuaLog:Debug("指令长度: " .. cmdSize .. " 字节")
    if cmdSize ~= 34 then
        LuaLog:Debug("指令长度错误，应为34字节")
        return false
    end
    
    -- 发送指令
    local state = LuaCommu:WriteByteArray(robotId, writeVal)
    
    -- 记录日志
    if state then
        local hexStr = LuaString:FromByteArray(writeVal, " ")
        LuaLog:Debug("发送成功: " .. hexStr)
        LuaLog:Debug(string.format("坐标发送完成: 机器人%d", robotId))
    else
        LuaLog:Debug("发送失败")
    end
    return state
end
-- 使用示例
function exampleUsage()
    -- 示例：发送坐标到机器人1
    local success = sendCoordinateToRobot(
        0,      -- 机器人ID
        150.5,  -- X坐标
        200.3,  -- Y坐标
        50.0,   -- Z坐标
        0.0,    -- 角度
        0       -- 编码器通道
    )
    if success then
        LuaLog:Debug("坐标发送成功")
    else
        LuaLog:Debug("坐标发送失败")
    end
```

### 3. 通信协议格式
#### 发送指令格式（34字节）

<table style="width: 960px; border-collapse: collapse; table-layout: fixed; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><thead><tr><th style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><b><div class="ybc-p">字段名称</div></b></th><th style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><b><div class="ybc-p">字节数</div></b></th><th style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><b><div class="ybc-p">十六进制值示例</div></b></th><th style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><b><div class="ybc-p">说明</div></b></th></tr></thead><tbody><tr><td style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">帧头</div></td><td style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">8字节</div></td><td style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">30 30 00 00 00 22 01 06</div></td><td style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">固定不变的起始标识符</div></td></tr><tr><td style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">地址</div></td><td style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">2字节</div></td><td style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">00 01</div></td><td style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">功能地址，不同地址代表不同功能</div></td></tr><tr><td style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">参数1</div></td><td style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">4字节</div></td><td style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">XX XX XX XX</div></td><td style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">X坐标（毫米）×1000</div></td></tr><tr><td style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">参数2</div></td><td style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">4字节</div></td><td style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">XX XX XX XX</div></td><td style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">Y坐标（毫米）×1000</div></td></tr><tr><td style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">参数3</div></td><td style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">4字节</div></td><td style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">XX XX XX XX</div></td><td style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">Z坐标（毫米）×1000</div></td></tr><tr><td style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">参数4</div></td><td style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">4字节</div></td><td style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">XX XX XX XX</div></td><td style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">角度A（弧度）×1000</div></td></tr><tr><td style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">参数5</div></td><td style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">4字节</div></td><td style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">XX XX XX XX</div></td><td style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">编码器值（ENC）</div></td></tr><tr><td style="width: 131px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">参数6</div></td><td style="width: 104px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">4字节</div></td><td style="width: 301px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">XX XX XX XX</div></td><td style="width: 423px; min-width: 0px; padding: 10.7143px 12.8571px 10.7143px 0px; text-align: left; vertical-align: baseline; white-space: normal; word-break: normal; border-width: 1px; border-style: solid; border-color: rgb(224, 224, 224);"><div class="ybc-p">返回标志（0:成功, 1:失败）</div></td></tr></tbody></table>

#### 参数处理规则

1. **坐标值：** 实际值×1000后发送
1. **字节序：** 参数1-6采用低位在前高位在后（d

小端格式）