1. 把sensor安装到机器人法兰，485接线可以接到机器人末端法兰（建议升级到V3.9版本）或者直接接到控制柜的RS485口

2. 根据鑫精诚sensor手册，机器人发送"0910461C00020400000000C2F5"可以清零sensor，发送"0910019A0001020000CCAA",sensor会以100hz频率返回机器人sensor数据，返回格式：

20 4E Fx_L Fx_H Fy_L Fy_H Fz_L Fz_H Mx_L Mx_H My_L My_H Mz_L Mz_H Crc_L Crc_H

即从第三个字节开始，每2个字节构成一个数据，为int数据，需要自行转化为带符号的整数。

实际力值(N) = 带符号十进制数据 / 10
实际力矩值(N·m) = 带符号十进制数据 / 100

可以使用以下代码（以下代码以sensor接到控制器的rs485为例）。

（代码中也有部分介绍使用坤为sensor的方法，注：坤为返回数据为4个字节一组的float，低字节在前）

机器人示教器中，力控数据源选择lua。

--[[
    -- @Author: qiaoli
    -- @Date: 2023-02-17
    -- @LastEditors:chenliao@elibot.cn
]] --
sleep(0.5)
---------------------------Functions user should implement--------------------
function sensor_init()
    -- 传感器工作前的一些初始化操作,在此函数内实现
    if (TCIorController == 1) then
        tci_send("0910461C00020400000000C2F5", 1)
        sleep(1)
        tci_send("0910019A0001020000CCAA", 1)
        tci_flush()
    end
    if (TCIorController == 2) then
        repeat
            rs485_flush()
            local lenth = rs485_send("0910019A0001020000CCAA", 1)
            ret, recv_buff = rs485_recv(2, 1)
        until (ret ~= 0)
        rs485_flush()
    end
end

function Str2Int(d, BigEndian)
    -- 输入2个字节的字符串，默认低字节在前，高字节在后，例如'6BFF',对应带符号十进制为-149
    -- 如果高字节在前，使用参数BigEndian

    out = tonumber(string.sub(d, 1, 2), 16)
    out = (tonumber(string.sub(d, 3, 4), 16) << 8) | out

    if BigEndian == nil then
        out = string.pack("I2", out)
    else
        out = string.pack(">I2", out)
    end
    out = string.unpack("i2", out)
    return out
end

function Str2float(d,BigEndian)
    -- 输入的d为4个字节 的16进制数据的字符串，默认为低字节在前
    -- 若高字节前，例如'4623A314'，则使用BigEndian，'46 23 A3 14'(高字节在前)对应的浮点数是10472.76953125
  
    out = tonumber(string.sub(d,1,2),16)
    out = (tonumber(string.sub(d,3,4),16) <<8)|out
    out = (tonumber(string.sub(d,5,6),16) <<16)|out
    out = (tonumber(string.sub(d,7,8),16) <<24)|out
  
    if BigEndian==nil then
      out = string.pack('I',out)
    else
      out = string.pack('>I',out)
    end
    out  = string.unpack('f',out)
    return out  
  end

function convert_str_to_torque_array(data)
    -- 将帧字符串转换成力矩数组，每种传感器的实现不唯一，在此函数内实现

    -- 此处以鑫精诚sensor为例，20 4E 6B FF 51 01 39 01 C4 FF B9 FF 35 00 E6 BD
    -- 20 4E 为数据头，之后每2个字节（51 FF）为一个数据的INT数据,E6 BD 为crc校验码

    local f1 = string.sub(data, 5, 8) --截取6BFF 4个字符
    local f2 = string.sub(data, 9, 12) --截取5101 4个字符
    local f3 = string.sub(data, 13, 16) --截取3901 4个字符
    local f4 = string.sub(data, 17, 20) --截取C4FF 4个字符
    local f5 = string.sub(data, 21, 24) --截取B9FF 4个字符
    local f6 = string.sub(data, 25, 28) --截取3500 4个字符

    local F1 = Str2Int(f1) / 10 --将0x6BFF 转为带符号数据(-149)并除10，得到-14.9
    local F2 = Str2Int(f2) / 10
    local F3 = Str2Int(f3) / 10
    local F4 = Str2Int(f4) / 1000
    local F5 = Str2Int(f5) / 1000
    local F6 = Str2Int(f6) / 1000

   
    -- -- 以下以坤为sensor为例
    -- -- 从第3字节开始，每4个字节构成一个float，低字节在前。以0D0A结尾
    -- -- 49 AA B0751CC1 9B3FEDC0 AE5CF040 D4B52DBE 5F55993E 7C1AB0BB 0D0A

    -- local f1 = string.sub(data, 5, 12) --截取B0751CC1  8个字符
    -- local f2 = string.sub(data, 13, 20) --截取9B3FEDC0  8个字符
    -- local f3 = string.sub(data, 21, 28) --截取AE5CF040  8个字符
    -- local f4 = string.sub(data, 29, 36) --截取D4B52DBE  8个字符
    -- local f5 = string.sub(data, 37, 44) --截取5F55993E  8个字符
    -- local f6 = string.sub(data, 45, 52) --截取7C1AB0BB  8个字符

    -- local F1 = Str2float(f1)  --将0xB0751CC1 转为浮点数-9.7787kg（坤为的力单位是kg）
    -- local F2 = Str2float(f2)  
    -- local F3 = Str2float(f3) 
    -- local F4 = Str2float(f4)  --截取D4B52DBE 转为浮点数-0.1696（坤为的力矩单位是kg·m）
    -- local F5 = Str2float(f5) 
    -- local F6 = Str2float(f6) 


    local torque = {F1, F2, F3, F4, F5, F6}

    set_global_variable(FX, F1)
    set_global_variable(FY, F2)
    set_global_variable(FZ, F3)
    set_global_variable(MX, F4)
    set_global_variable(MY, F5)
    set_global_variable(MZ, F6)

    return torque
end
---------------------------End of Functions user should implement-------------

---------------------------Internal function----------------------------------
function str2hex(str)
    --判断输入类型
    if (type(str) ~= "string") then
        return nil, "str2hex invalid input type"
    end
    --滤掉分隔符
    str = str:gsub("[%s%p]", ""):upper()
    --检查内容是否合法
    if (str:find("[^0-9A-Fa-f]") ~= nil) then
        return nil, "str2hex invalid input content"
    end
    --检查字符串长度
    if (str:len() % 2 ~= 0) then
        return nil, "str2hex invalid input lenth"
    end
    --拼接字符串
    local index = 1
    local ret = ""
    for index = 1, str:len(), 2 do
        ret = ret .. string.char(tonumber(str:sub(index, index + 1), 16))
    end

    return ret
end

--[[
-- extract_nice_frame: 从raw_data中返回符合要求的数据帧
-- return: 若无有效数据返回nil, 否则返回完整的一帧数据
--]]
function extract_nice_frame(raw_data)
    local begin_idx, end_idx, valid_data
    local len = string.len(raw_data)
    local actual_frame_len = FRAME_LEN * 2  -- FRAME_LEN * 2 代表16进制数据转换成ASCII后的长度

    if len <  actual_frame_len then     -- 先判断字符长度是否满足
        --elite_print("recv wrong data len: "..len)
        return nil
    end

    if (FRAME_END) then -- 一帧数据包含固定帧尾的情况
        begin_idx, end_idx = string.find(raw_data, FRAME_BEGIN..'.*'..FRAME_END, 1, false)  --string.find 会匹配满足要求的最长字串
        if (not begin_idx or not end_idx) then
            return nil
        elseif (end_idx - begin_idx == actual_frame_len - 1) then -- 正好符合帧的格式，这是大多数情况
            valid_data = string.sub(recv_buff, begin_idx, begin_idx + actual_frame_len -1)
            return valid_data
        elseif (end_idx - begin_idx > actual_frame_len - 1) then
            local idx1,idx2
            idx1 = begin_idx
            repeat
                -- 比较帧最后的字符是否为帧尾
                local sub_star_idx = idx1 + actual_frame_len - string.len(FRAME_END)
                local sub_end_idx = sub_star_idx + string.len(FRAME_END) - 1
                local tmp_tail = string.sub(raw_data, sub_star_idx, sub_end_idx)
                if (tmp_tail == FRAME_END) then
                    valid_data = string.sub(raw_data, idx1, idx1 + actual_frame_len -1)
                    return valid_data
                end

                idx1, idx2 = string.find(raw_data, FRAME_BEGIN, idx1 + string.len(FRAME_BEGIN))
            until(not idx1)
            return nil
        end
    else -- 没有固定帧尾的情况
        begin_idx, end_idx = string.find(raw_data, FRAME_BEGIN)
        if (begin_idx and end_idx and begin_idx <= len - actual_frame_len) then
            local idx1,idx2 = string.find(raw_data, FRAME_BEGIN, end_idx + 1)
            if (not idx1 or idx1 > len - actual_frame_len) then  -- 没固定帧尾时，这两种条件下截取的一定是有效帧
                valid_data = string.sub(recv_buff, begin_idx, begin_idx + actual_frame_len -1)
                return valid_data
            end
        else
            return nil
        end
    end
    return nil
end
---------------------------End of Internal Function--------------------------

-------------------------- User Config First---------------------------------
--  脚本运行前请先配置下面的基本参数
FRAME_LEN = 16 -- 传感器发送的一帧数据帧的长度，单位：字节
FRAME_BEGIN = "204E" -- 数据帧的帧头标识，此处以鑫精诚sensor为例，如果是坤为sensor填入"48AA"
FRAME_END = nil -- 数据帧的帧尾标识, 如果没有帧尾请填入nil,此处以鑫精诚sensor为例.如果是坤为sensor填入"0D0A"
FRAME_MAX_RECV_TM = 40 -- 串口每次接收等待的最大时间，unit: ms

SERIAL_speed = 115200 -- 串口波特率
SERIAL_bits = 8 -- 串口传输数据位
SERIAL_event = "N" -- 串口传输停止位
SERIAL_stop = 1 -- 串口传输停止位
TCIorController = 2 -- 1使用末端Tci, 2使用控制柜485接口
-------------------------- End of User Config First -------------------------

-- 用户变量
Control_script = "D0" --控制脚本运行
SCRIPT_MODE = "D1" --将脚本指令写入到传感器1:停止持续传输数据
FX = "D2" --传感器参数
FY = "D3" --传感器参数
FZ = "D4" --传感器参数
MX = "D5" --传感器参数
MY = "D6" --传感器参数
MZ = "D7" --传感器参数
ID = "D8" --传感器地址

---------------------------Script start------------------------------------
frame_idx = 0
torque = {0, 0, 0, 0, 0, 0}
err_tm = 0
stop_push_force()
if (TCIorController == 1) then
    tci_close()
end
if (TCIorController == 2) then
    rs485_close()
end

sleep(0.5)
if (TCIorController == 1) then
    open = tci_open()
end
if (TCIorController == 2) then
    open = rs485_open()
end

if (open >= 0) then
    if (TCIorController == 1) then
        tci_setopt(SERIAL_speed, SERIAL_bits, SERIAL_event, SERIAL_stop)
    end
    if (TCIorController == 2) then
        rs485_setopt(SERIAL_speed, SERIAL_bits, SERIAL_event, SERIAL_stop)
    end
else
    elite_print("串口打开失败！")
    exit() -- 并没有exit函数，只是为了让系统报错，脚本退出执行
end
sensor_init() -- User implement
start_push_force()
last_tm = os.clock()

-------------------------------main loop-------------------------------------------------------
repeat
    now_tm = os.clock()
    if TCIorController==1 then
        ret, recv_buff = tci_recv(FRAME_MAX_RECV_TM, 1, FRAME_LEN * 2) --接收2帧长度的数据，确保有一帧为有效数据
    end
    if TCIorController ==2 then
        ret,recv_buff = rs485_recv(FRAME_MAX_RECV_TM, 1, FRAME_LEN * 2) --接收2帧长度的数据，确保有一帧为有效数据
    end

      valid_frame = extract_nice_frame(recv_buff)
    --elite_print("valid_frame : ",valid_frame)

    if (valid_frame) then
        torque = convert_str_to_torque_array(valid_frame) -- User implement
        last_tm = now_tm
    else
        torque = {0, 0, 0, 0, 0, 0}
        err_tm = now_tm - last_tm -- 计算传感器数据异常的持继时间
        set_global_variable("D9", err_tm)
    end

    push_external_force(frame_idx, torque)
    tci_flush()
    frame_idx = frame_idx + 1
    if frame_idx >= 65535 then
        frame_idx = 1
    end

    sleep(0.001)
until (get_global_variable("D0") == 0)

stop_push_force()
if TCIorController==1 then
    tci_close()
end
if TCIorController==2 then
    rs485_close()
end