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,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 @c5e45cf
@@ -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 @c5e45cf
@@ -175,6 +175,11 @@ class ControlRobot(threading.Thread): @@ -175,6 +175,11 @@ class ControlRobot(threading.Thread):
175 175
176 return coord_raw, coord_robot_raw, markers_flag 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 def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered): 183 def robot_move_decision(self, distance_target, new_robot_coordinates, current_robot_coordinates, current_head_filtered):
179 """ 184 """
180 There are two types of robot movements. 185 There are two types of robot movements.
@@ -277,6 +282,7 @@ class ControlRobot(threading.Thread): @@ -277,6 +282,7 @@ class ControlRobot(threading.Thread):
277 282
278 if not self.robot_target_queue.empty(): 283 if not self.robot_target_queue.empty():
279 self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait() 284 self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
  285 + self.robot_motion_reset()
280 self.robot_target_queue.task_done() 286 self.robot_target_queue.task_done()
281 287
282 if not self.object_at_target_queue.empty(): 288 if not self.object_at_target_queue.empty():