From 029068b259edca7e4f79ec969b5f97ea18818511 Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Thu, 9 Sep 2021 21:36:38 +0200 Subject: [PATCH] added some output --- remote_control/robot.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/remote_control/robot.py b/remote_control/robot.py index 605ad41..23caeed 100644 --- a/remote_control/robot.py +++ b/remote_control/robot.py @@ -111,12 +111,14 @@ class ControlledRobot(Robot): def start_control(self): if self.controller is not None: + print("starting control") self.controller.start() else: raise Exception("Error: Cannot start control: there is not controller attached to the robot!") def stop_control(self): if self.controller is not None: + print("stopping control") self.controller.stop() else: raise Exception("Error: Cannot stop control: there is not controller attached to the robot!") @@ -141,6 +143,7 @@ class ControlledRobot(Robot): angles_unwrapped = np.unwrap([current_pos[2], target_pos[2]]) # unwrap angle to avoid jump in data e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference e_pos = np.linalg.norm(v[0:2]) + #print(f"target_pos = {target_pos}, current_pos = {current_pos}, e_pos = {e_pos}, e_ang = {e_angle}") print(f"e_pos = {e_pos}, e_ang = {e_angle}") close_to_target = e_pos < 0.05 and abs(e_angle) < 0.1 time.sleep(0.1)