pshell_config/bernina_robot
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Split updates in slowly and rapidly polled variables

Browse Source
This commit is contained in:
gac-bernina
2023-12-21 16:28:49 +01:00
parent b299877c17
commit ac5bb50a5e
2 changed files with 177 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
script/devices/RobotBernina.py
View File

@@ -334,7 +334,7 @@ class RobotBernina(RobotTCP):
if simulation:
add_device(RobotBernina("robot","localhost:1234"),force = True)
else:
add_device(RobotBernina("robot", "129.129.243.105:1234"), force = True)
add_device(RobotBernina("robot", "129.129.243.106:1234"), force = True)
time.sleep(0.1)

190
script/devices/RobotTCP.py
View File

@@ -49,8 +49,9 @@ class RobotTCP(TcpDevice, Stoppable):
self.tool = None
self.default_desc = None
self.tool_open = None
#self.tool_trsf = [0.0] * 6
self.tool_trsf = [0.0] * 6
self.frame = FRAME_DEFAULT
self.frame_trsf = [0.0] * 6
self.polling_interval = 0.01
self.reset = True
self.default_tolerance = 5
@@ -60,27 +61,33 @@ class RobotTCP(TcpDevice, Stoppable):
self.task_start_retries = 3
self.exception_on_task_start_failure = True #Tasks may start and be finished when checked
self.simulated_point = ""
self.update_start_time = 0
self.polling_env = 1
def doInitialize(self):
super(RobotTCP, self).doInitialize()
self.reset = True
def set_tool(self, tool):
self.tool = tool
self.tool=tool
self.set_str(tool, name="s_actualTool")
self.evaluate("t_actualTool=" + tool)
#self.tool_trsf = self.get_tool_trsf()
self.evaluate("tcp_curtool=" + tool)
if self.cartesian_motors_enabled:
self.update()
self.set_motors_enabled(True)
self.is_tool_open()
def get_tool(self):
def get_tool(self):
return self.tool
def set_frame(self, frame, name="tcp_curframe", change_default=True):
self.evaluate(name + "=" + frame)
def set_frame(self, frame, name=None, change_default=True):
if name is not None:
self.evaluate(name + "=" + frame)
if change_default:
self.frame = frame
self.set_str(frame, name="s_actualFrame")
self.evaluate("f_actualFrame" + "=" + frame)
self.frame=frame
if self.cartesian_motors_enabled:
self.update()
self.set_motors_enabled(True)
@@ -198,9 +205,12 @@ class RobotTCP(TcpDevice, Stoppable):
def get_var(self, name):
return self.execute('get_var', name)
#Makes app crash
#def get_str(self, name='s'):
# return self.execute('get_str', name)
#Makes app crash - Still true?
def get_str(self, name='tcp_s'):
return self.execute('get_str', name)
def set_str(self, l, name = "tcp_s"):
self.evaluate(name + '="' + str(l) + '"')
def get_arr(self, name, size):
return self.execute('get_arr', name, size)
@@ -290,9 +300,9 @@ class RobotTCP(TcpDevice, Stoppable):
self.evaluate("tcp_b=" + cmd)
return self.get_bool()
#def eval_str(self, cmd):
# self.evaluate("s=" + cmd)
# return self.get_str()
def eval_str(self, cmd):
self.evaluate("tcp_s=" + cmd)
return self.get_str()
def eval_jnt(self, cmd):
self.evaluate("tcp_j=" + cmd)
@@ -640,8 +650,157 @@ class RobotTCP(TcpDevice, Stoppable):
if (not self.settled) or (self.current_task is not None): self.setState(State.Busy)
elif not self.empty: self.setState(State.Paused)
else: self.setState(State.Ready)
def doUpdate_env(self):
cur_task = self.current_task #Can change in background so cache it
if self.isSimulated():
sts = [1, 1,"1", 100, "1", "1", 0, 0, \
0, 0, 0, 0, 0, 0, \
0, 0, 0, 0, 0, 0, \
]
else:
sts = self.execute("get_status_env", cur_task)
self._update_working_mode(int(sts[0]), int(sts[1]))
self.powered = sts[2] == "1"
self.speed = int(sts[3])
self.empty = sts[4] == "1"
self.settled = sts[5] == "1"
#TODO: add tool open
if cur_task is not None:
if self.isSimulated():
if time.time() - self.current_task_timestamp > 3.0:
log("Task "+ str(cur_task) + " finished with code: -1", False)
ret = -1 if self.current_task_timestamp>0 else None
self.on_task_finished(self.current_task, ret)
self.current_task, self.current_task_ret = None, ret
else:
if int(sts[6]) < 0:
try:
ret = int(sts[7])
except:
ret = None
log("Task "+ str(cur_task) + " finished with code: " + str(ret), False)
self.on_task_finished(cur_task, ret)
self.current_task, self.current_task_ret = None, ret
self.frame = sts[8]
self.tool = sts[9]
for i in range(6):
self.frame_trsf[i] = float(sts[10 + i])
for i in range(6):
self.tool_trsf[i] = float(sts[16 + i])
self.update_spherical_pos()
ev_index = 22 #7
ev = sts[ev_index] if len(sts)>ev_index else ""
if len(ev.strip()) >0:
self.getLogger().info(ev)
self.on_event(ev)
self._update_state()
self.reset = False
self.setCache({"powered": self.powered,
"speed": self.speed,
"empty": self.empty,
"settled": self.settled,
"task": cur_task,
"mode": self.working_mode,
"status": self.status,
"open": self.tool_open,
}, None)
self.on_poll_status.update({
"connected": self.connected,
"powered": self.powered,
"speed": self.speed,
"settled": self.settled,
"task": self.current_task,
"mode": self.working_mode,
"status": self.status,
"frame": self.frame,
"frame_coordinates": self.frame_trsf,
"tool": self.tool,
"tool_coordinates": self.tool_trsf,
"cartesian_motors_enabled": self.cartesian_motors_enabled,
"spherical_motors_enabled": self.spherical_motors_enabled,
})
def doUpdate_fast(self):
cur_task = self.current_task #Can change in background so cache it
if self.isSimulated():
sts = [1, 1,"1", 100, "1", "1", 0, 0, \
0, 0, 0, 0, 0, 0, \
0, 0, 0, 0, 0, 0, \
]
else:
sts = self.execute("get_status_fast", cur_task)
for i in range(6):
self.joint_pos[ROBOT_JOINT_MOTORS[i]] = float(sts[0 + i])
for i in range(6):
self.cartesian_pos[ROBOT_MOTORS[i]] = float(sts[6 + i])
self.linear_axis_pos = {"z_lin": float(sts[7])}
self.update_spherical_pos()
ev_index = 8 #7
ev = sts[ev_index] if len(sts)>ev_index else ""
if len(ev.strip()) >0:
self.getLogger().info(ev)
self.on_event(ev)
self.setCache({
"pos": self.get_current_point_cached() if self.state==State.Ready else None #TODO: make it calculated in robot by polling funtion
}, None)
pos = self.cartesian_pos.copy()
pos.update(self.spherical_pos)
pos.update(self.joint_pos)
pos.update(self.linear_axis_pos)
self.on_poll_status.update({
"pos": pos,
})
if self.cartesian_motors_enabled:
for m in self.cartesian_motors:
m.readback.update()
if self.spherical_motors_enabled:
for m in self.spherical_motors:
m.readback.update()
if self.linear_axis_motors_enabled:
for m in self.linear_axis_motors:
m.readback.update()
if self.joint_motors_enabled:
for m in self.joint_motors:
m.readback.update()
def doUpdate(self):
try:
self.doUpdate_fast()
except:
if self.state != State.Offline:
print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
self.setState(State.Offline)
if time.time() - self.update_start_time >= self.polling_env:
self.update_start_time = time.time()
try:
self.doUpdate_env()
except:
if self.state != State.Offline:
print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
self.setState(State.Offline)
get_context().sendEvent("polling", self.on_poll_status)
def doUpdate(self):
def doUpdate_backup(self):
try:
start = time.time()
cur_task = self.current_task #Can change in background so cache it
@@ -742,13 +901,16 @@ class RobotTCP(TcpDevice, Stoppable):
if self.state != State.Offline:
print >> sys.stderr, "Update error: " + str(sys.exc_info()[1])
self.setState(State.Offline)
def on_poll_info(self):
on_poll_config_methods ={
"powered": {"cmd":"robot.set_powered", "def_kwargs": ""},
"speed": {"cmd":"robot.set_monitor_speed", "def_kwargs": ""},
"frame": {"cmd": "robot.set_frame", "def_kwargs": "change_default=True",},
"frame_coordinates": {"cmd": "robot.set_trsf", "def_kwargs": "name='f_actualFrame.trsf'",},
"tool": {"cmd":"robot.set_tool", "def_kwargs": ""},
"tool_coordinates": {"cmd": "robot.set_trsf", "def_kwargs": "name='t_actualTool.trsf'",},
"cartesian_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['cartesian']"},
"spherical_motors_enabled": {"cmd":"robot.set_motors_enabled", "def_kwargs": "pseudos=['spherical']"},
}