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,3 +858,7 @@ ROBOT_ARC_THRESHOLD_DISTANCE = 100 #mm
858 ROBOT_VERSOR_SCALE_FACTOR = 70 858 ROBOT_VERSOR_SCALE_FACTOR = 70
859 #Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%. 859 #Robot Working Space is defined as 800mm in Elfin manual. For safety, the value is reduced by 5%.
860 ROBOT_WORKING_SPACE = 760 #mm 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,17 +24,19 @@ class Elfin_Server():
24 def Run(self): 24 def Run(self):
25 return self.cobot.ReadPcsActualPos() 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 It's not possible to send a move command to elfin if the robot is during a move. 29 It's not possible to send a move command to elfin if the robot is during a move.
30 Status 1009 means robot in motion. 30 Status 1009 means robot in motion.
31 """ 31 """
32 status = self.cobot.ReadMoveState() 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 self.cobot.MoveL(target) 35 self.cobot.MoveL(target)
36 - elif type == const.ROBOT_MOTIONS["arc"]: 36 + elif motion_type == const.ROBOT_MOTIONS["arc"]:
37 self.cobot.MoveC(target) 37 self.cobot.MoveC(target)
  38 + elif status == const.ROBOT_MOVE_STATE["error"]:
  39 + self.StopRobot()
38 40
39 def StopRobot(self): 41 def StopRobot(self):
40 # Takes some microseconds to the robot actual stops after the command. 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,6 +277,9 @@ class ControlRobot(threading.Thread):
277 277
278 if not self.robot_target_queue.empty(): 278 if not self.robot_target_queue.empty():
279 self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait() 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 self.robot_target_queue.task_done() 283 self.robot_target_queue.task_done()
281 284
282 if not self.object_at_target_queue.empty(): 285 if not self.object_at_target_queue.empty():