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

Improve simple control algorithm and add drawing and mouse control

simple_control
Valentin Ochs 2020-11-14 18:41:31 +01:00
parent 213c09d246
commit 27f114a6d4
1 changed files with 90 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
remote_control/simple_control.py
View File

@ -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)