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!
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