Download as
Browse Code »

Commit c5e45cf458dc45a33641b66f2e2dc43b692b91de

Authored by okahilak
Committed by GitHub
2 parents 72d06b9d aeb03abe
Exists in master

Merge pull request #401 from rmatsuda/deal_with_robot_errors 

ADD: Forces to stop the robot when some error occurs
Showing 3 changed files with 16 additions and 4 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @c5e45cf
... ... @@ -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 @c5e45cf
... ... @@ -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 @c5e45cf
... ... @@ -175,6 +175,11 @@ class ControlRobot(threading.Thread):
175 175  
176 176 return coord_raw, coord_robot_raw, markers_flag
177 177  
  178 + def robot_motion_reset(self):
  179 + self.trck_init_robot.StopRobot()
  180 + self.arc_motion_flag = False
  181 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
  182 +
178 183 def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered):
179 184 """
180 185 There are two types of robot movements.
... ... @@ -277,6 +282,7 @@ class ControlRobot(threading.Thread):
277 282  
278 283 if not self.robot_target_queue.empty():
279 284 self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
  285 + self.robot_motion_reset()
280 286 self.robot_target_queue.task_done()
281 287  
282 288 if not self.object_at_target_queue.empty():
... ...