How to convert byte array into various LREAL(double) values in TwinCAT3

I’m right now trying to convert my original python socket reading code into structure text in TwinCAT3. The following is the original python code that unpacks the read_data = socket.recv(1116) data into different things:

def ur_read(read_data):
    moment=[0,0,0,0,0,0]
    joint_p=[0,0,0,0,0,0]
    joint_v=[0,0,0,0,0,0]         
    tcp_p=[0,0,0,0,0,0]
    tcp_v=[0,0,0,0,0,0]
    tcp_force=[0,0,0,0,0,0]
    state=0
    time_new=0
    try:
        for i in range(len(read_data)):            
            if [read_data[i],read_data[i+1],read_data[i+2],read_data[i+3]] == [0,0,4,92]:
                time = struct.unpack('>d', read_data[i+4:i+12])
                moment[0], = struct.unpack('>d', read_data[i+204:i+212])
                moment[1], = struct.unpack('>d', read_data[i+212:i+220])
                moment[2], = struct.unpack('>d', read_data[i+220:i+228])
                moment[3], = struct.unpack('>d', read_data[i+228:i+236])
                moment[4], = struct.unpack('>d', read_data[i+236:i+244])
                moment[5], = struct.unpack('>d', read_data[i+244:i+252])
                joint_p[0], = struct.unpack('>d', read_data[i+252:i+260])
                joint_p[1], = struct.unpack('>d', read_data[i+260:i+268])
                joint_p[2], = struct.unpack('>d', read_data[i+268:i+276])
                joint_p[3], = struct.unpack('>d', read_data[i+276:i+284])
                joint_p[4], = struct.unpack('>d', read_data[i+284:i+292])
                joint_p[5], = struct.unpack('>d', read_data[i+292:i+300])
                joint_v[0], = struct.unpack('>d', read_data[i+300:i+308])
                joint_v[1], = struct.unpack('>d', read_data[i+308:i+316])
                joint_v[2], = struct.unpack('>d', read_data[i+316:i+324])
                joint_v[3], = struct.unpack('>d', read_data[i+324:i+332])
                joint_v[4], = struct.unpack('>d', read_data[i+332:i+340])
                joint_v[5], = struct.unpack('>d', read_data[i+340:i+348])
                tcp_p[0], = struct.unpack('>d', read_data[i+444:i+452])
                tcp_p[1], = struct.unpack('>d', read_data[i+452:i+460])
                tcp_p[2], = struct.unpack('>d', read_data[i+460:i+468])
                tcp_p[3], = struct.unpack('>d', read_data[i+468:i+476])
                tcp_p[4], = struct.unpack('>d', read_data[i+476:i+484])
                tcp_p[5], = struct.unpack('>d', read_data[i+484:i+492])
                tcp_v[0], = struct.unpack('>d', read_data[i+492:i+500])
                tcp_v[1], = struct.unpack('>d', read_data[i+500:i+508])
                tcp_v[2], = struct.unpack('>d', read_data[i+508:i+516])
                tcp_v[3], = struct.unpack('>d', read_data[i+516:i+524])
                tcp_v[4], = struct.unpack('>d', read_data[i+524:i+532])
                tcp_v[5], = struct.unpack('>d', read_data[i+532:i+540])
                tcp_force[0], = struct.unpack('>d', read_data[i+540:i+548])
                tcp_force[1], = struct.unpack('>d', read_data[i+548:i+556])
                tcp_force[2], = struct.unpack('>d', read_data[i+556:i+564])
                tcp_force[3], = struct.unpack('>d', read_data[i+564:i+572])
                tcp_force[4], = struct.unpack('>d', read_data[i+572:i+580])
                tcp_force[5], = struct.unpack('>d', read_data[i+580:i+588])
                state,= struct.unpack('>d', read_data[i+1052:i+1060])  
                time_new, = struct.unpack('>d', read_data[i+4:i+12])
                break
    except:
        print('exp')

According to the python code, I’ve written a socket connection and read function block in TwinCAT3. However, I’m right now facing some problem doing data encoding:

FUNCTION_BLOCK DataEncoding
VAR_INPUT
    DataReceived: ARRAY[0..1115] OF BYTE;
END_VAR
VAR_OUTPUT
    moment: ARRAY[0..5] OF LREAL := [6(0.0)]; // LREAL stands for Double
    joint_pos: ARRAY[0..5] OF LREAL := [6(0.0)];
    joint_vel: ARRAY[0..5] OF LREAL := [6(0.0)];
    tcp_pos: ARRAY[0..5] OF LREAL := [6(0.0)];
    tcp_vel: ARRAY[0..5] OF LREAL := [6(0.0)];
    tcp_force: ARRAY[0..5] OF LREAL := [6(0.0)];
END_VAR
VAR
    i: INT;
    BYTEARR_to_STRING: BYTEARR_TO_MAXSTRING;
END_VAR

And in the body part:

FOR i := 0 TO 1116 BY 1 DO
    IF DataReceived[i] = 0 AND DataReceived[i+1] = 0 AND DataReceived[i+2] = 4 AND 
    DataReceived[i+3] = 92
        moment[0] := TO_LREAL(BYTEARR_to_STRING(in:= DataReceived[(i+204)..(i+212)]));
    END_IF
END_FOR

The problem I’ve encountered is that I’m not sure how to write conversion between byte array and LREAL. Furthermore, the array indexing of DataReceived[(i+204)..(i+212)] seems to have some errors. Can anyone teach me how to index specific part like (204 to 212) in ARRAY[0..1115]? And also the tramsformation between byte array and LREAL.

Thanks!

Answer

You can memcopy the bytes to the LREAL.
ADR() is the function that gets the memory address of the variable:

FOR i := 0 TO 1116 BY 1 DO
    IF DataReceived[i] = 0 AND DataReceived[i+1] = 0 AND DataReceived[i+2] = 4 AND DataReceived[i+3] = 92 THEN
        memcpy(ADR(moment[0]), ADR(DataReceived[(i+204)]), 212-204);
    END_IF
END_FOR

Advertisement

Answer