apo/RoboRally
1
0
Fork 0
forked from Telos4/RoboRally
Code Issues Pull Requests Releases Wiki Activity

added some output

Browse Source
This commit is contained in:
Simon Pirkelmann 2021-09-09 21:36:38 +02:00
parent 7962199c3a
commit 029068b259
1 changed files with 3 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
remote_control/robot.py
View File

@ -111,12 +111,14 @@ class ControlledRobot(Robot):
def start_control(self): def start_control(self):
if self.controller is not None: if self.controller is not None:
print("starting control")
self.controller.start() self.controller.start()
else: else:
raise Exception("Error: Cannot start control: there is not controller attached to the robot!") raise Exception("Error: Cannot start control: there is not controller attached to the robot!")
def stop_control(self): def stop_control(self):
if self.controller is not None: if self.controller is not None:
print("stopping control")
self.controller.stop() self.controller.stop()
else: else:
raise Exception("Error: Cannot stop control: there is not controller attached to the robot!") 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 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_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference
e_pos = np.linalg.norm(v[0:2]) 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}") print(f"e_pos = {e_pos}, e_ang = {e_angle}")
close_to_target = e_pos < 0.05 and abs(e_angle) < 0.1 close_to_target = e_pos < 0.05 and abs(e_angle) < 0.1
time.sleep(0.1) time.sleep(0.1)