pavel/kick-it
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Small but important tweaks

Browse Source
This commit is contained in:
Pavel Lutskov
2018-07-01 17:54:59 +02:00
parent cbdac79911
commit a01189e016
2 changed files with 29 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
pykick/__main__.py
View File

@@ -48,12 +48,12 @@ if __name__ == '__main__':
_, _, gcc = striker.goal_search() _, _, gcc = striker.goal_search()
print('Goal center', gcc, 'Ball dist', bdist) print('Goal center', gcc, 'Ball dist', bdist)
if abs(gcc) < 0.2 or bdist <= 0.35: if abs(gcc) < 0.3 or bdist <= 0.40:
print('Straight approach') print('Straight approach')
state = 'straight_approach' state = 'straight_approach'
approach = 0 approach = 0
elif 0.35 < bdist < 0.50: elif 0.40 < bdist < 0.60:
print('Rdist is hypo') print('Rdist is hypo')
state = 'rdist_is_hypo' state = 'rdist_is_hypo'
approach = 1 if gcc < 0 else - 1 approach = 1 if gcc < 0 else - 1
@@ -91,8 +91,10 @@ if __name__ == '__main__':
striker.mover.move_to(0, 0, angle) striker.mover.move_to(0, 0, angle)
striker.mover.wait() striker.mover.wait()
if rdist > 1.5: if rdist > 2:
striker.run_to_ball(1.5) striker.run_to_ball(rdist / 2)
striker.mover.wait()
striker.mover.move_to(0, 0, -angle)
striker.mover.wait() striker.mover.wait()
else: else:
striker.run_to_ball(rdist) striker.run_to_ball(rdist)
@@ -179,7 +181,7 @@ if __name__ == '__main__':
# [ | yes v ] # [ | yes v ]
# [ | Ball distance <-- Goal angle > thr ] # [ | Ball distance <-- Goal angle > thr ]
# [ | / | \ | ] # [ | / | \ | ]
# [ | > 50 cm / |(35,50) \ < 35cm | no ] # [ | > 60 cm / |(40,60) \ < 40cm | no ]
# [ | / v \ v ] # [ | / v \ v ]
# [ +- Distance is < Walk is hypo \ Straight approach ] # [ +- Distance is < Walk is hypo \ Straight approach ]
# [ | hypo | > until goal align ] # [ | hypo | > until goal align ]
@@ -190,3 +192,4 @@ if __name__ == '__main__':
# [ |( | ( |( Ball Goal align ] # [ |( | ( |( Ball Goal align ]
# [ | \ | \_ | \ <-- align <-- (if lost ball run backwards) ] # [ | \ | \_ | \ <-- align <-- (if lost ball run backwards) ]
# [_______________________________________________________________] # [_______________________________________________________________]
#

40
pykick/striker.py
View File

@@ -15,12 +15,13 @@ from naoqi import ALProxy
class Striker(object): class Striker(object):
def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv, def __init__(self, nao_ip, nao_port, res, ball_hsv, goal_hsv, field_hsv,
ball_min_radius): ball_min_radius, do_capture=False):
# Maintenance # Maintenance
self.run_id = strftime('%Y%m%d%H%M%S') self.run_id = strftime('%Y%m%d%H%M%S')
self.is_over = False self.is_over = False
self.last_goal = 'right' self.last_goal = 'right'
self.doing_caputre = do_capture
# Motion # Motion
self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port) self.mover = NaoMover(nao_ip=nao_ip, nao_port=nao_port)
@@ -34,17 +35,18 @@ class Striker(object):
) )
# POV # POV
self.upper_pov = NaoImageReader( if do_capture:
nao_ip, port=nao_port, res=1, fps=10, cam_id=0, self.upper_pov = NaoImageReader(
video_file='./cam0_' + self.run_id + '.avi' nao_ip, port=nao_port, res=1, fps=10, cam_id=0,
) video_file='./cam0_' + self.run_id + '.avi'
)
self.lower_pov = NaoImageReader( self.lower_pov = NaoImageReader(
nao_ip, port=nao_port, res=1, fps=10, cam_id=1, nao_ip, port=nao_port, res=1, fps=10, cam_id=1,
video_file='./cam1_' + self.run_id + '.avi' video_file='./cam1_' + self.run_id + '.avi'
) )
self.pov_thread = Thread(target=self._pov) self.pov_thread = Thread(target=self._pov)
self.pov_thread.start() self.pov_thread.start()
# Recognition # Recognition
self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]), self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]),
@@ -70,13 +72,13 @@ class Striker(object):
if self.tts_thread.isAlive(): if self.tts_thread.isAlive():
self.tts_thread.join() self.tts_thread.join()
if self.pov_thread.isAlive(): if self.doing_caputre and self.pov_thread.isAlive():
self.pov_thread.join() self.pov_thread.join()
self.upper_pov.close()
self.lower_pov.close()
self.upper_camera.close() self.upper_camera.close()
self.lower_camera.close() self.lower_camera.close()
self.upper_pov.close()
self.lower_pov.close()
self.mover.stop_moving() self.mover.stop_moving()
def _speaker(self): def _speaker(self):
@@ -118,11 +120,11 @@ class Striker(object):
# the robot starts to move arround his z-Axis in the direction where his # the robot starts to move arround his z-Axis in the direction where his
# head is aligned when the head yaw angle has reached his maximum # head is aligned when the head yaw angle has reached his maximum
if yaw > 0.8: if yaw > pi/3:
self.mover.set_head_angles(-pi / 8, pitch, 0.5) self.mover.set_head_angles(-pi / 8, pitch, 0.5)
sleep(0.5) sleep(0.5)
elif yaw < -0.8: elif yaw < -pi/3:
self.mover.move_to_fast(0, 0, -pi / 4) self.mover.move_to(0, 0, -pi / 4)
self.mover.wait() self.mover.wait()
# rotate head to the left, if head yaw angle is equally zero or larger # rotate head to the left, if head yaw angle is equally zero or larger
# rotate head to the right, if head yaw angle is smaller than zero # rotate head to the right, if head yaw angle is smaller than zero
@@ -290,7 +292,7 @@ class Striker(object):
if ball_angles is None: if ball_angles is None:
raise ValueError('No ball') raise ValueError('No ball')
x, y = ball_angles x, y = ball_angles
goal_x, goal_y = 0.088, 0.4 goal_x, goal_y = 0.092, 0.38
dx, dy = goal_x - x, goal_y - y dx, dy = goal_x - x, goal_y - y
dx = -dx * 0.2 if abs(dx) > 0.03 else 0 dx = -dx * 0.2 if abs(dx) > 0.03 else 0
@@ -325,7 +327,7 @@ class Striker(object):
gcl, gcr, gcc = goal gcl, gcr, gcc = goal
print('Goal:', gcl, gcr, gcc) print('Goal:', gcl, gcr, gcc)
if gcl > 0 > gcr: if gcl > 0.15 > -0.22 > gcr:
return True return True
if y > 0.38: if y > 0.38: