Download as
Browse Code »

Commit b50c2ba8a1d165065ed381e297a8cfdb83373d3f

Authored by Renan
1 parent ec15fe81
Exists in master

ADD: Stops robot when some error occurs 

- reset motion when a new target is set
Showing 3 changed files with 13 additions and 4 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @b50c2ba
... ... @@ -858,3 +858,7 @@ ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
858 858 ROBOT_VERSOR_SCALE_FACTOR = 70
859 859 #Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
860 860 ROBOT_WORKING_SPACE = 760 #mm
  861 +ROBOT_MOVE_STATE = {"free to move": 0,
  862 + "in motion": 1009,
  863 + "waiting for execution": 1013,
  864 + "error": 1025}
... ...
invesalius/data/elfin.py
  Diff comments   View file @b50c2ba
... ... @@ -24,17 +24,19 @@ class Elfin_Server():
24 24 def Run(self):
25 25 return self.cobot.ReadPcsActualPos()
26 26  
27   - def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]):
  27 + def SendCoordinates(self, target, motion_type=const.ROBOT_MOTIONS["normal"]):
28 28 """
29 29 It's not possible to send a move command to elfin if the robot is during a move.
30 30 Status 1009 means robot in motion.
31 31 """
32 32 status = self.cobot.ReadMoveState()
33   - if status != 1009:
34   - if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]:
  33 + if status == const.ROBOT_MOVE_STATE["free to move"]:
  34 + if motion_type == const.ROBOT_MOTIONS["normal"] or motion_type == const.ROBOT_MOTIONS["linear out"]:
35 35 self.cobot.MoveL(target)
36   - elif type == const.ROBOT_MOTIONS["arc"]:
  36 + elif motion_type == const.ROBOT_MOTIONS["arc"]:
37 37 self.cobot.MoveC(target)
  38 + elif status == const.ROBOT_MOVE_STATE["error"]:
  39 + self.StopRobot()
38 40  
39 41 def StopRobot(self):
40 42 # Takes some microseconds to the robot actual stops after the command.
... ...
invesalius/navigation/robot.py
  Diff comments   View file @b50c2ba
... ... @@ -277,6 +277,9 @@ class ControlRobot(threading.Thread):
277 277  
278 278 if not self.robot_target_queue.empty():
279 279 self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
  280 + self.trck_init_robot.StopRobot()
  281 + self.arc_motion_flag = False
  282 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
280 283 self.robot_target_queue.task_done()
281 284  
282 285 if not self.object_at_target_queue.empty():
... ...