218 lines
7.3 KiB
Python
218 lines
7.3 KiB
Python
import socket
|
|
import pygame
|
|
import json
|
|
from math import sin,cos,atan2,pi
|
|
from argparse import ArgumentParser
|
|
|
|
parser = ArgumentParser()
|
|
parser.add_argument('bot', metavar='bot', type=str, help='ip address of the controlled robot')
|
|
parser.add_argument('id', metavar='id', type=str, help='id of the controlled robot')
|
|
parser.add_argument('meas', metavar='meas', type=str, help='ip address of the measurement server')
|
|
args = parser.parse_args()
|
|
|
|
bot = args.bot
|
|
meas = args.meas
|
|
|
|
w = 640
|
|
h = 480
|
|
|
|
pygame.init()
|
|
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_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.sendall(f"{args.id}\n".encode())
|
|
|
|
class Bot(object):
|
|
FAR, CLOSE, ANGLE, REACHED = range(4)
|
|
def __init__(self, control):
|
|
self.sock = control
|
|
self.state = Bot.REACHED
|
|
self.tx = self.ty = None
|
|
self.ta = None
|
|
|
|
def move(self, x, y, angle = None):
|
|
self.tx = x
|
|
self.ty = y
|
|
self.ta = angle
|
|
self.pps = True
|
|
self.pas = True
|
|
|
|
print()
|
|
print(f"GOING TO {x},{y}")
|
|
print()
|
|
|
|
self.state = Bot.FAR
|
|
|
|
def rotate(self, angle):
|
|
self.ta = angle
|
|
self.state = Bot.ANGLE
|
|
|
|
def stop(self):
|
|
self.sock.send(f'(0,0)\n'.encode())
|
|
def step(self, x, y, angle):
|
|
umax = 1.0
|
|
|
|
if self.tx != None:
|
|
dp = ((self.tx - x)**2 + (self.ty - y)**2)**0.5
|
|
da = atan2(self.ty - y, self.tx - x) - angle
|
|
|
|
# Position state: Determine if the angle to the target flips over +-pi/2, i.e. going past the target now
|
|
def pst(angle):
|
|
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, 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)
|
|
|
|
if self.state == Bot.FAR:
|
|
if dp <= 0.3:
|
|
# Close to the target
|
|
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.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:
|
|
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
|
|
# Difference between angle states:
|
|
# +- 1 means flipping between 'closer than pi/2' and 'further than pi/2'
|
|
# +- 2 means abs(angle) < pi/2 and the sign changes -> In position!
|
|
if abs(ast(da) - self.pas) == 2:
|
|
# Done
|
|
self.state = Bot.REACHED
|
|
bot.stop()
|
|
else:
|
|
# Don't move, just rotate
|
|
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(meas_file.readline().strip())
|
|
received = json.loads(meas_file.readline().strip())
|
|
|
|
# Extract position, angle
|
|
x = received['x']
|
|
y = received['y']
|
|
a = received['angle']
|
|
|
|
events = pygame.event.get()
|
|
for event in events:
|
|
if event.type == pygame.KEYDOWN:
|
|
if event.key == pygame.K_RIGHT:
|
|
bot.move(ml, 0, pi)
|
|
if event.key == pygame.K_LEFT:
|
|
bot.move(-ml, 0, 0)
|
|
if event.key == pygame.K_UP:
|
|
bot.move(0, ml, pi*3/2)
|
|
if event.key == pygame.K_DOWN:
|
|
bot.move(0, -ml, pi/2)
|
|
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)
|
|
|
|
|