fkeickl
搞電的沒(méi)前途
級(jí)別: 略有小成
|
MODULE MainModule VAR socketdev socket_client; VAR robtarget zhuaqudian{4, 6} := [ [[[304.27,-308.43,388.88],[0.00605288,-0.921946,0.386945,-0.0159119],[-1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[302.63,-500.33,388.88],[0.00611035,-0.921938,0.386966,-0.0158309],[-1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[301.71,-691.96,388.88],[0.00627679,-0.921946,0.386953,-0.0156304],[-1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[103.85,-307.68,388.88],[0.00626996,-0.921947,0.386949,-0.0156282],[-1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[105.97,-499.35,388.88],[0.006271,-0.921942,0.386961,-0.0156303],[-1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[103.92,-690.97,388.88],[0.00627827,-0.921947,0.386949,-0.0156317],[-1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]], [[[-122.05,-310.89,388.88],[0.00645941,-0.921936,0.386973,-0.0156182],[-2,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-124.22,-502.28,388.88],[0.00648626,-0.921942,0.38696,-0.0156026],[-2,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-117.92,-694.13,388.88],[0.00646402,-0.921923,0.387005,-0.0155904],[-2,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-316.41,-307.47,388.88],[0.00645302,-0.921949,0.386938,-0.0156957],[-2,0,-4,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-319.25,-504.26,388.88],[0.00656645,-0.921893,0.387072,-0.0156806],[-2,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-314.56,-695.07,388.88],[0.00652637,-0.921896,0.387066,-0.0156755],[-2,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]], [[[300.57,694.78,388.88],[0.00621611,-0.921927,0.386999,-0.0155978],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[302.88,502.19,388.88],[0.00621539,-0.921926,0.387003,-0.0155974],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[301.33,309.79,388.88],[0.00621415,-0.921925,0.387004,-0.0156002],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[107.10,694.54,388.88],[0.0062171,-0.921926,0.387002,-0.0155948],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[105.49,503.12,388.88],[0.00621575,-0.921925,0.387004,-0.0155983],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[106.00,311.19,388.88],[0.00621052,-0.92192,0.387017,-0.0156025],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]], [[[-122.27,693.85,388.88],[0.00620335,-0.921927,0.386999,-0.0156261],[1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-122.85,502.21,388.88],[0.00621437,-0.921928,0.386997,-0.0156302],[1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-125.59,309.09,388.88],[0.00622092,-0.921929,0.386993,-0.0156214],[1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-320.87,694.16,388.88],[0.0062127,-0.921927,0.386998,-0.0156269],[1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-320.92,503.94,388.88],[0.00621883,-0.921923,0.387007,-0.0156311],[1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], [[-323.41,311.32,388.88],[0.00621518,-0.921925,0.387003,-0.015623],[1,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]] ]; VAR robtarget yuandian := [[584.30,98.67,388.88],[0.000927732,0.9636,-0.267343,0.00115184],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; VAR robtarget touliao := [[595.42,114.45,115.41],[8.54151E-05,0.951234,-0.308469,-6.39389E-05],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; VAR robtarget newguodudian := [[111.88,-309.89,388.88],[0.000206237,2.54575E-05,-1,-0.000101683],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; PROC main() VAR bool retval; SocketClose socket_client; SocketCreate socket_client; SocketConnect socket_client, "192.168.100.61", 8088; WHILE TRUE DO WaitDI DI02,0; Reset DO00; Reset DO01; Reset DO02; Reset DO03; Reset DO04; IF DI00=0 AND DI01= 0 THEN gotoAreaRobotPos 1, retval; ELSEIF DI00 = 1 AND DI01= 0 THEN gotoAreaRobotPos 2, retval; ELSEIF DI00 = 0 AND DI01= 1 THEN gotoAreaRobotPos 3, retval; ELSEIF DI00 = 1 AND DI01= 1 THEN gotoAreaRobotPos 4, retval; ENDIF IF(not retval) THEN WHILE DI04=0 DO WaitDI DI04, 1; WaitTime 0.05; ENDWHILE ENDIF ENDWHILE ENDPROC PROC jiexiweizhi(string sp, num index, inout robtarget data{*}, inout bool isError, inout bool isLastLayer) VAR num count := 1; VAR num pos_count := 1; VAR bool is_sucess := TRUE; VAR num para{7}:=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; VAR string inputstr; VAR num found; VAR string split_string := ""; VAR num split_index; VAR string remain_string := ""; VAR bool isRecvComplete := FALSE; VAR bool isParseComplete := FALSE; isError := FALSE; isLastLayer := FALSE; WHILE not isRecvComplete DO SocketReceive socket_client, \Str:=inputstr, \Time:=WAIT_MAX; found := StrMatch(inputstr, 1, ";"); IF found <= StrLen(inputstr) THEN isRecvComplete := TRUE; inputstr := StrPart(inputstr, 1, found - 1); ENDIF split_index := 1; split_string := ""; isParseComplete := FALSE; WHILE not isParseComplete DO found := StrMatch(inputstr, split_index, sp); IF found <= StrLen(inputstr) THEN IF strLen(remain_string) > 0 THEN split_string := remain_string + StrPart(inputstr, split_index, found - split_index); remain_string := ""; ELSE split_string := StrPart(inputstr, split_index, found - split_index); ENDIF is_sucess := strtoval(split_string, para{count}); IF is_sucess THEN Incr count; ENDIF IF count = 8 THEN parsePos para, data{pos_count}; Incr pos_count; count := 1; ENDIF split_index := found + 1; ELSE isParseComplete := TRUE; remain_string := StrPart(inputstr, split_index, StrLen(inputstr) - split_index + 1); ENDIF ENDWHILE ENDWHILE found := StrMatch(remain_string, 1, "error"); IF found <= StrLen(remain_string) THEN isError := TRUE; ENDIF found := StrMatch(remain_string, 1, "yes"); IF found <= StrLen(remain_string) THEN isLastLayer := TRUE; ENDIF ENDPROC PROC parsePos(num data{*}, inout robtarget robpos) robpos.trans.x := data{1}; robpos.trans.y := data{2}; robpos.trans.z := data{3}; robpos.rot.q1 := data{4}; robpos.rot.q2 := data{5}; robpos.rot.q3 := data{6}; robpos.rot.q4 := data{7}; ENDPROC PROC gotoAreaRobotPos(num index, inout bool retVal) VAR robtarget new_position{6}; VAR string inputstr; VAR num found; VAR bool flag2 := TRUE; VAR bool isError; VAR bool isLastLayer; retval := TRUE; 程序如上,其中 PROC main() VAR bool retval; SocketClose socket_client; SocketCreate socket_client; SocketConnect socket_client, "192.168.100.61", 8088; 不知道什么意思 |
|
---|---|---|
|