Commit b50c2ba8a1d165065ed381e297a8cfdb83373d3f
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
... | ... | @@ -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
... | ... | @@ -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
... | ... | @@ -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(): | ... | ... |