From 27f114a6d44d9087ab0913001aba2769b7a8317e Mon Sep 17 00:00:00 2001 From: Valentin Ochs Date: Sat, 14 Nov 2020 18:41:31 +0100 Subject: [PATCH] Improve simple control algorithm and add drawing and mouse control --- remote_control/simple_control.py | 107 ++++++++++++++++++++++++++----- 1 file changed, 90 insertions(+), 17 deletions(-) diff --git a/remote_control/simple_control.py b/remote_control/simple_control.py index fbaa822..36cb778 100644 --- a/remote_control/simple_control.py +++ b/remote_control/simple_control.py @@ -13,18 +13,27 @@ args = parser.parse_args() bot = args.bot meas = args.meas +w = 640 +h = 480 pygame.init() -pygame.display.set_mode((640, 480)) +surf = pygame.display.set_mode((640, 480)) +bg = pygame.Color(0,0,0) +botcol = pygame.Color(255, 0, 0) +setcol = pygame.Color(0, 255, 0) -meas_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) +meas_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +try: + meas_socket.connect((meas, 42424)) # connect to robot +except socket.error: + print("could not connect to measurement socket") rc_socket = socket.socket() try: rc_socket.connect((bot, 1234)) # connect to robot except socket.error: print("could not connect to bot socket") -meas_socket.sendto(f"{args.id}\n".encode(), (meas, 42424)) +meas_socket.sendall(f"{args.id}\n".encode()) class Bot(object): FAR, CLOSE, ANGLE, REACHED = range(4) @@ -41,6 +50,10 @@ class Bot(object): self.pps = True self.pas = True + print() + print(f"GOING TO {x},{y}") + print() + self.state = Bot.FAR def rotate(self, angle): @@ -56,26 +69,58 @@ class Bot(object): dp = ((self.tx - x)**2 + (self.ty - y)**2)**0.5 da = atan2(self.ty - y, self.tx - x) - angle - if da > pi: - da -= 2*pi - elif da < -pi: - da += 2*pi - # Position state: Determine if the angle to the target flips over +-pi/2, i.e. going past the target now def pst(angle): - return abs(angle) > pi + while angle > pi: + angle -= 2*pi + while angle <-pi: + angle += 2*pi + return abs(angle) > pi/2 # Angle state: When 'close' to target angle (absolute value <= pi/2) use the sign, else 0. When the angle flips over 0, the difference is +- 2 def ast(angle): + while angle > pi: + angle -= 2*pi + while angle <-pi: + angle += 2*pi return 0 if abs(angle) >= pi/2 else 1 if angle >= 0 else -1 # Move with speed and change towards angle - def control(speed, angle): + def control(speed, angle, backwards = True): + ca = cos(angle) speed = speed * cos(angle) + + + # Move angle to [0..2*pi] + while angle < 0: + angle += 2*pi + while angle > 2*pi: + angle -= 2*pi + + if backwards: + # If we can go backwards, angle should be pi or 0 + if angle >= pi/2 and angle < 3*pi/2: + # pi/2 .. 3/2 pi should move towards pi + angle -= pi + print(f"Go for pi, {angle}") + else: + # Everything else should be 0. Make sure angles in 3/2 pi .. 2 pi don't lead to huge outputs. + if angle >= 3*pi/2: + angle -= 2*pi + print(f"Go for 0, {angle}") + else: + # Have to go forward, just try to get angle to 0 + # Correct angle to be [-pi..pi] + while angle > pi: + angle -= 2*pi + print(f"Always go for 0, {angle}") ul, ur = speed, speed + if speed == 0 and abs(angle) < 0.5: + angle = 0.5 if angle > 0 else -0.5 if angle < 0 else 0 ul -= angle * 0.5 ur += angle * 0.5 vd = max(1, abs(ul)/umax, abs(ur)/umax) ul /= vd ur /= vd + print(f"Movement: {ul},{ur}") self.sock.send(f'({ul},{ur})\n'.encode()) self.pps = pst(angle) self.pas = ast(angle) @@ -83,20 +128,25 @@ class Bot(object): if self.state == Bot.FAR: if dp <= 0.3: # Close to the target - self.state = Bot.NEAR + self.state = Bot.CLOSE + self.pps = pst(da) else: # When far, just move towards the target + print(f"FAR {umax}, {da}") control(umax, da) - if self.state == Bot.NEAR: - if pst(da) != self.pps: + if self.state == Bot.CLOSE: + if pst(da) != self.pps and dp <= 0.1: # The angle flips over +- pi/2, go for the angle setpoint self.state = Bot.ANGLE + self.pas = 0 else: - control(umax * dp / 0.3, da) + print(f"CLOSE {umax*dp/0.3} {da} {pst(da)} {self.pps}") + control(0.3+(umax*dp/0.3), da) if self.state == Bot.ANGLE: if self.ta == None: # No angle setpoint, we're done self.state = Bot.REACHED + bot.stop() else: # Use angle setpoint for angle difference da = self.ta - angle @@ -106,18 +156,25 @@ class Bot(object): if abs(ast(da) - self.pas) == 2: # Done self.state = Bot.REACHED + bot.stop() else: # Don't move, just rotate - control(0, da) + print(f"ANGL {0} {da} {ast(da)} {self.pas}") + control(0, da, False) + +meas_file = meas_socket.makefile('rw', encoding='utf8', newline='\n') bot = Bot(rc_socket) bot.move(0, 0, 0) running = True ml = 0.5 + +scale = 220 / 0.5 + while running: - received = json.loads(str(meas_socket.recv(1024), "utf-8").strip()) - print("Received: {}".format(received)) + received = json.loads(meas_file.readline().strip()) + received = json.loads(meas_file.readline().strip()) # Extract position, angle x = received['x'] @@ -138,6 +195,22 @@ while running: if event.key == pygame.K_ESCAPE: bot.stop() running = False + elif event.type == pygame.MOUSEBUTTONDOWN: + x,y = pygame.mouse.get_pos() + bot.move((x-w/2)/scale, (y-h/2)/scale) + + surf.fill(bg) + pygame.draw.circle(surf, botcol, (w/2 + scale * x, h/2 + scale * y), 10, 2) + pygame.draw.line(surf, botcol, (w/2 + scale * x, h/2 + scale * y), (w/2 + scale * x + 15 * cos(a), h/2 + scale * y + 15 * sin(a))) + + if bot.tx != None and bot.ty != None: + pygame.draw.circle(surf, setcol, (w/2 + scale * bot.tx, h/2 + scale * bot.ty), 10, 2) + if bot.ta != None: + pygame.draw.line(surf, setcol, + (w/2 + scale * bot.tx, h/2 + scale * bot.ty), + (w/2 + scale * bot.tx + 15 * cos(bot.ta), h/2 + scale * bot.ty + 15 * sin(bot.ta))) + pygame.display.flip() + if running: bot.step(x, y, a)