越疆机器人与上位机的通讯

        越疆机器人的开发终于整完了。整体来说这个算比较简单，但是由于本身越疆是通过他们自己的上位机进行控制与通讯，并不是单纯用lua解释器去运行，因此效率属实不算高。最近也是在搞UR的开发，这个真的是一言难尽，明天加班的时候再吐槽。

        首先确认一点，越疆机器人在运行代码的时候，是无法通过示教去控制机器人的。话不多说，老样子上代码：

function TcpInit() --创建套接字 
    local port = 8888
    local ip = "192.168.1.1" --机器人作为服务端的IP
    local SeverSocket = 0
    local err 
    err,SeverSocket = TCPCreate(true,ip,port)
    if (err ~= 0) then 
        print("tcp create socket error")
    end
    return SeverSocket
end

function TcpListen(socket) --利用已创建的套接字进行监听
    local err 
    err = TCPStart(socket,0)
    if (err ~= 0) then
      print("socket listen error")
    end
    return socket
end

function TcpRecv(socket) --利用已创建的套接字接收数据
    local err
    local Recvbuf 
    err,Recvbuf = TCPRead(socket,5)
    return Recvbuf,socket 
end

function Analyze(socket) --分析数据并解析
    local Recvbuf    --初始化
    local IntRecv = {}
    local TempInt = {}
    local RecJpos = {}
    local RecPpos = {}
    local Head = {} 
    Recvbuf,socket = TcpRecv(socket) --接收数据
    if (Recvbuf[1] == nil) then --断开重新监听
      print("recieve data from VI error and  error code is",err)
      TCPDestroy(socket)
      socket = TcpInit()
      err = TCPStart(socket, 0) 
      Recvbuf,socket = TcpRecv(socket) --接收流为Recvbuf
      end 
    for i = 0,51 do  
          IntRecv[i] = Recvbuf[i+1]    --用整型数组接收
    end 
    for i = 0,3 do
        Head[i] = IntRecv[i]           --帧头 自己定义
    end
    for i = 0,5 do
        for j = 0,3 do
            TempInt[j] = IntRecv[4 + i*4 + j]  --IEEE754的协议 四字节byte转换位32位float
        end
        RecJpos[i] = Hex_to_float(TempInt)     --用float数组存储收到的关节 6个float
        
        for j = 0,3 do
            TempInt[j] = IntRecv[28 + i * 4 + j] 
        end 
        RecPpos[i] = Hex_to_float(TempInt)     --用float数组存储收到的位置 6个float
     end
    SendData(socket)
end

function SendData(socket)                     --发送数据
    local send_buf = {}
    local err 
    for i = 1,52 do
        send_buf[i] = 0
    end
    local tempInt = {}
    send_buf[1] = 1                          --发送包的帧头
    send_buf[2] = 1
    send_buf[3] = 1
    send_buf[4] = 1
    local send_curJ = GetAngle().joint     --获取当前关节
    local send_curP = GetPose().pose       --获取当前位置
    for i = 0,5 do 
        local tempInt = Float_to_hex(send_curJ[i+1])  --float转换成byte
        local tempInt1 = Float_to_hex(send_curP[i+1])
        for j = 0,3 do 
            send_buf[4 + i * 4 + j + 1] = tempInt[j]
            send_buf[28 + i * 4 + j + 1] = tempInt1[j]
        end
    end
    err = TCPWrite(socket,send_buf)         --发送数据
end

function Hex_to_float(temparray) --input type = {a,b,c,d} as 4 number;output type = float 0.132134 as a double float
    if temparray[0] ~= nil then
      local i = temparray[0] * 16 ^ 6 + temparray[1] * 16 ^ 4 + temparray[2] * 16 ^ 2 + temparray[3]
      local s = ((i >> 31) == 0) and 1 or -1 
      local e = (i >> 23) & 0xFF 
      local m = (e == 0) and (i & 0x7FFFFF) << 1 or (i & 0x7FFFFF) | 0x800000 
      return s * (2 ^ (e - 127)) * (m / 8388608.0)
    else 
      i = 0
    end

end

function FloatToOx(float)  --float转换成二进制
    local FloatDot = 0.0
    local IntOx = {}
    local FinalOx = {}
    local Int
    local count = 0
    local E = 0
    local M = {}
    local sysmbel = 0
    
    for i = 0,31 do
        FinalOx[i] = 0
    end

    --S
    if (float >= 0) then
        FinalOx[0] = 0
        float = float * 1
    else 
        FinalOx[0] = 1
        float = float * -1
    end
    --E
    for i = 0,7 do
        IntOx[i] = 0
    end
    Int = float // 1
    FloatDot = float - Int
    while (Int > 0) do
        IntOx[count] = Int%2
        count = count + 1
        Int = Int // 2
    end
    count = count - 1 
   
    for i = 0,count do 
        M[i] = IntOx[count - i - 1]
    end
    
    --M 
    local DotCount = FloatDot
    for i = 0,22 do
        DotCount = DotCount * 2
        if DotCount < 1 and count < 0 then
            count = count - 1 
        end
    end
    for i = 0,22-count do 
        FloatDot = FloatDot * 2
        M[count+i] = FloatDot//1
        FloatDot = FloatDot - FloatDot//1
    end

    E = 127 + count 
    for i = 0,7 do
        if (E > 0) then
            FinalOx[8-i] = E%2
            E = E // 2
        end
    end
    for i = 9,31 do 
    FinalOx[i] = M[i-9]
    end
    return FinalOx
end

function OxToInt(ox)  --二进制转换为十进制
    local IntArray = {}
    for i = 0,3 do
        IntArray[i] = ox[(i*8)]*2^7+ ox[(i*8+1)]*2^6+ ox[(i*8+2)]*2^5+ ox[(i*8+3)]*2^4+ ox[(i*8+4)]*2^3+ ox[(i*8+5)]*2^2+ ox[(i*8+6)]*2+ ox[(i*8+7)] 
    end
    return IntArray
end

function Float_to_hex(float)  --float转换为十六进制
    local TempOx = {}
    local TempArray = {}
    TempOx = FloatToOx(float)
    TempArray = OxToInt(TempOx)
    return TempArray
end