From 28d0049714f1afbfda4de6757ddd94be0f3ea6b6 Mon Sep 17 00:00:00 2001 From: gac-S_Changer Date: Mon, 27 Feb 2017 09:55:51 +0100 Subject: [PATCH] Closedown --- .../usr/usrapp/ForceDetection/comTcp.pgx | 352 ++++++++++-------- script/RobotTCP.py | 16 +- script/local.py | 2 +- 3 files changed, 192 insertions(+), 178 deletions(-) diff --git a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comTcp.pgx b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comTcp.pgx index 0a0f87b..f0db3d7 100644 --- a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comTcp.pgx +++ b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comTcp.pgx @@ -36,174 +36,198 @@ rx = "" else msg_id = left(rx, 4) - if msg_id == "EVT " + msg = right(rx, len(rx)-4) rx="" - if events[0] == "" - sSio = msg_id - else - sSio = msg_id + events[0] - for index=0 to 8 - events[index]= events[index+1] - endFor - events[9] = "" - endIf - else - msg = right(rx, len(rx)-4) - rx="" - //If same id, repeat last answer - if msg_id != last_id - for index=0 to 9 - args[index]="" - endFor - index = find(msg, " ") - if index < 0 - cmd = msg - else - cmd = left(msg, index) - msg = right(msg, len(msg) - index -1) - arg=0 - while (len(msg) > 0 and (arg < 10)) - index = find(msg, array_separator) - if index<0 - args[arg] = msg - msg ="" - else - args[arg] = left(msg, index) - msg = right(msg, len(msg) - index -1) - endIf - arg = arg+1 - endWhile - endIf - tx = "" - ex = "" - //General commands - switch(cmd) - case "eval" - tx=$exec(args[0]) - break - - case "get_var","get_bool" - if cmd == "get_var" - ret = getData(args[0], aux) - else - ret = getData(args[0], auxb) - endIf - switch(ret) - case -1 - ex = "The variable does not exists" - break - case -2 - ex = "The variable library does not exist" - break - case -3 - ex = "The index is out of range" - break - case -4 - ex = "The data's type does not match the variable's type" - break - default - if cmd == "get_var" - tx = toString(".4", aux) + //If same id, repeat last answer + if msg_id != last_id + for index=0 to 9 + args[index]="" + endFor + index = find(msg, " ") + if index < 0 + cmd = msg + else + cmd = left(msg, index) + msg = right(msg, len(msg) - index -1) + arg=0 + while (len(msg) > 0 and (arg < 10)) + index = find(msg, array_separator) + if index<0 + args[arg] = msg + msg ="" + else + args[arg] = left(msg, index) + msg = right(msg, len(msg) - index -1) + endIf + arg = arg+1 + endWhile + endIf + tx = "" + ex = "" + //General commands + switch(cmd) + case "eval" + tx=$exec(args[0]) + break + + case "get_status" + aux = workingMode(count) + tx = toString("", aux ) + array_separator + tx = tx + toString("", count ) + array_separator + if isPowered() + tx = tx + "1" + array_separator + else + tx = tx + "0" + array_separator + endIf + aux = getMonitorSpeed() + tx = tx + toString("", aux ) + array_separator + if isEmpty() + tx = tx + "1" + array_separator + else + tx = tx + "0" + array_separator + endIf + if isSettled() + tx = tx + "1" + array_separator + else + tx = tx + "0" + array_separator + endIf + + if events[0] != "" + tx = tx + events[0] + for index=0 to 8 + events[index]= events[index+1] + endFor + events[9] = "" + endIf + break + + case "get_var","get_bool" + if cmd == "get_var" + ret = getData(args[0], aux) + else + ret = getData(args[0], auxb) + endIf + switch(ret) + case -1 + ex = "The variable does not exists" + break + case -2 + ex = "The variable library does not exist" + break + case -3 + ex = "The index is out of range" + break + case -4 + ex = "The data's type does not match the variable's type" + break + default + if cmd == "get_var" + tx = toString(".4", aux) + else + if auxb + tx = "1" else - if auxb - tx = "1" - else - tx = "0" - endIf - endIf - break - endSwitch - break + tx = "0" + endIf + endIf + break + endSwitch + break - case "get_arr" - toNum(args[1], count, ok) - for index = 0 to count - if cmd == "get_var" - ret = getData(args[0], aux) - else - ret = getData(args[0], auxb) - endIf - ret = getData(args[0] + "[" + toString("", index )+ "]", aux) - switch(ret) - case -1 - ex = "The variable does not exists" - return - break - case -2 - ex = "The variable library does not exist" - return - break - case -3 - ex = "The index is out of range" - return - break - case -4 - ex = "The data's type does not match the variable's type" - return - break - default - tx = tx + toString(".4", aux) + "|" - break - endSwitch - endFor - break - //case "get_str" - // //$exec("s = " + args[0]) TODO: MAKES THE CONTROLLER TO CRASH! - // //tx = s - //break - case "get_pnt" - $exec("p = " + args[0]) - $exec("t = " + args[0]) - tx = tx + toString(".4", p.trsf.x) + "|" - tx = tx + toString(".4", p.trsf.y) + "|" - tx = tx + toString(".4", p.trsf.z) + "|" - tx = tx + toString(".4", p.trsf.rx) + "|" - tx = tx + toString(".4", p.trsf.ry) + "|" - tx = tx + toString(".4", p.trsf.rz) + "|" - break - case "get_jnt" - $exec("j = " + args[0]) - tx = tx + toString(".4", j.j1) + "|" - tx = tx + toString(".4", j.j2) + "|" - tx = tx + toString(".4", j.j3) + "|" - tx = tx + toString(".4", j.j4) + "|" - tx = tx + toString(".4", j.j5) + "|" - tx = tx + toString(".4", j.j6) + "|" - break - case "get_trf" - $exec("t = " + args[0]) - tx = tx + toString(".4", t.x) + "|" - tx = tx + toString(".4", t.y) + "|" - tx = tx + toString(".4", t.z) + "|" - tx = tx + toString(".4", t.rx) + "|" - tx = tx + toString(".4", t.ry) + "|" - tx = tx + toString(".4", t.rz) + "|" - break - case "get_help" - toNum(args[0], aux, ok) - if ok == true - tx = help(aux) - else - ex = "Invalid code: " + args[0] - endIf - break - default - //App specific - call onCommandTcp(cmd,args, tx, ex) - if ((len(tx) == 0) and (len(ex) == 0)) - ex = "Invalid command: " + cmd - endIf - break - endSwitch - last_id = msg_id - endIf - if len(ex) == 0 - sSio = msg_id + tx - else - msg_id = replace (msg_id,"*",1,3) - sSio = msg_id + ex - endIf + case "get_arr" + toNum(args[1], count, ok) + for index = 0 to count + if cmd == "get_var" + ret = getData(args[0], aux) + else + ret = getData(args[0], auxb) + endIf + ret = getData(args[0] + "[" + toString("", index )+ "]", aux) + switch(ret) + case -1 + ex = "The variable does not exists" + return + break + case -2 + ex = "The variable library does not exist" + return + break + case -3 + ex = "The index is out of range" + return + break + case -4 + ex = "The data's type does not match the variable's type" + return + break + default + tx = tx + toString(".4", aux) + "|" + break + endSwitch + endFor + break + + //case "get_str" + // //$exec("s = " + args[0]) TODO: MAKES THE CONTROLLER TO CRASH! + // //tx = s + //break + + case "get_pnt" + $exec("p = " + args[0]) + $exec("t = " + args[0]) + tx = tx + toString(".4", p.trsf.x) + "|" + tx = tx + toString(".4", p.trsf.y) + "|" + tx = tx + toString(".4", p.trsf.z) + "|" + tx = tx + toString(".4", p.trsf.rx) + "|" + tx = tx + toString(".4", p.trsf.ry) + "|" + tx = tx + toString(".4", p.trsf.rz) + "|" + break + + case "get_jnt" + $exec("j = " + args[0]) + tx = tx + toString(".4", j.j1) + "|" + tx = tx + toString(".4", j.j2) + "|" + tx = tx + toString(".4", j.j3) + "|" + tx = tx + toString(".4", j.j4) + "|" + tx = tx + toString(".4", j.j5) + "|" + tx = tx + toString(".4", j.j6) + "|" + break + + case "get_trf" + $exec("t = " + args[0]) + tx = tx + toString(".4", t.x) + "|" + tx = tx + toString(".4", t.y) + "|" + tx = tx + toString(".4", t.z) + "|" + tx = tx + toString(".4", t.rx) + "|" + tx = tx + toString(".4", t.ry) + "|" + tx = tx + toString(".4", t.rz) + "|" + break + + case "get_help" + toNum(args[0], aux, ok) + if ok == true + tx = help(aux) + else + ex = "Invalid code: " + args[0] + endIf + break + + default + //App specific + call onCommandTcp(cmd,args, tx, ex) + if ((len(tx) == 0) and (len(ex) == 0)) + ex = "Invalid command: " + cmd + endIf + break + endSwitch + last_id = msg_id endIf + if len(ex) == 0 + sSio = msg_id + tx + else + msg_id = replace (msg_id,"*",1,3) + sSio = msg_id + ex + endIf endIf endIf endWhile diff --git a/script/RobotTCP.py b/script/RobotTCP.py index 59cdd57..fab2f02 100644 --- a/script/RobotTCP.py +++ b/script/RobotTCP.py @@ -1,5 +1,3 @@ -import threading - class RobotTCP(TcpDevice, Stoppable): def __init__(self, name, server, timeout = 1000, retries = 1): TcpDevice.__init__(self, name, server) @@ -17,7 +15,6 @@ class RobotTCP(TcpDevice, Stoppable): self.empty = None self.working_mode = None self.status = None - self.lock = threading.Lock() def _sendReceive(self, msg_id, msg = "", timeout = None): tx = self.header if (self.header != None) else "" @@ -330,7 +327,7 @@ class RobotTCP(TcpDevice, Stoppable): #self.setState(State.Busy if self.status=="move" else State.Ready) if not self.settled: self.setState(State.Busy) if not self.empty: self.setState(State.Paused) - else self.setState(State.Ready) + else: self.setState(State.Ready) def doUpdate(self): try: @@ -355,12 +352,5 @@ class RobotTCP(TcpDevice, Stoppable): def on_event(self,ev): pass - - def mount(self, puck, sample): - return self.execute('mount', puck, sample) - - - -add_device(RobotTCP("robot_tcp", "129.129.126.100:1000"), force = True) -robot_tcp.setPolling(100) -#robot_tcp.set_monitor_speed(20) + + \ No newline at end of file diff --git a/script/local.py b/script/local.py index a313f3b..582475d 100644 --- a/script/local.py +++ b/script/local.py @@ -7,7 +7,7 @@ from ch.psi.pshell.serial import TcpDevice from ch.psi.pshell.modbus import ModbusTCP -run("RobotTCP") +run("RobotMX")