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

Nice curved approach

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-25 11:20:41 +02:00
parent 770f4bdd52
commit b479805a45
2 changed files with 47 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
pykick/nao_defaults.json
View File

@@ -11,7 +11,6 @@
255 255
] ]
], ],
"cam": 1,
"goal": [ "goal": [
[ [
0, 0,
@@ -24,12 +23,13 @@
255 255
] ]
], ],
"fps": 10,
"res": 2, "res": 2,
"ball_min_radius": 0.01, "ball_min_radius": 0.01,
"field": [ "field": [
[ [
24, 33,
51, 63,
60 60
], ],
[ [
@@ -38,7 +38,7 @@
255 255
] ]
], ],
"fps": 10, "cam": 1,
"ip": "192.168.0.11", "ip": "192.168.0.11",
"port": 9559 "port": 9559
} }

52
pykick/striker.py
View File

@@ -84,6 +84,9 @@ class Striker(object):
print(e) print(e)
return None return None
if cam == self.lower_camera:
mask = False
if mask: if mask:
field = self.field_finder.find(frame) field = self.field_finder.find(frame)
frame = self.field_finder.mask_it(frame, field) frame = self.field_finder.mask_it(frame, field)
@@ -119,7 +122,7 @@ class Striker(object):
def distance_to_ball(self): def distance_to_ball(self):
return 0.5 return 0.5
def ball_tracking(self): def ball_tracking(self, soll=0):
"""Track the ball using the feed from top and bottom camera. """Track the ball using the feed from top and bottom camera.
Returns Returns
@@ -154,7 +157,7 @@ class Striker(object):
if abs(x) > 0.15: if abs(x) > 0.15:
# self.speak('Align to the ball') # self.speak('Align to the ball')
self.mover.stop_moving() self.mover.stop_moving()
self.turn_to_ball(x, y) self.turn_to_ball(x, y, soll=soll)
return False return False
else: else:
return True return True
@@ -162,7 +165,7 @@ class Striker(object):
def run_to_ball(self): def run_to_ball(self):
self.mover.move_to(1, 0, 0) self.mover.move_to(1, 0, 0)
def turn_to_ball(self, ball_x, ball_y, tol=0.05): def turn_to_ball(self, ball_x, ball_y, tol=0.05, soll=0):
"""Align robot to the ball. """Align robot to the ball.
If head is not centered at the ball (within tolerance), then If head is not centered at the ball (within tolerance), then
@@ -173,7 +176,7 @@ class Striker(object):
# only the x ball angle is relevant for alignment # only the x ball angle is relevant for alignment
d_yaw, d_pitch = ball_x, 0 d_yaw, d_pitch = ball_x, 0
print('ball yaw:', d_yaw) print('ball yaw in camera:', d_yaw)
# center head at the ball # center head at the ball
if (abs(d_yaw) > 0.01): if (abs(d_yaw) > 0.01):
@@ -184,13 +187,15 @@ class Striker(object):
yaw = self.mover.get_head_angles()[0] yaw = self.mover.get_head_angles()[0]
print('head yaw', yaw) print('head yaw', yaw)
d_yaw = yaw - soll
print('head d_yaw', d_yaw)
# align body with the head # align body with the head
if abs(yaw) > tol: if abs(d_yaw) > tol:
print('Going to rotate') print('Going to rotate')
self.speak("Going to rotate") self.speak("Going to rotate")
self.mover.set_head_angles(0, 0, 0.5) self.mover.set_head_angles(soll, 0, 0.5)
self.mover.move_to(0, 0, yaw) self.mover.move_to(0, 0, d_yaw)
self.mover.wait() self.mover.wait()
# def move_sideways(self, dy): # def move_sideways(self, dy):
@@ -315,7 +320,9 @@ class Striker(object):
# check if ball visible ---> rotate head to the ball # check if ball visible ---> rotate head to the ball
# ^ | | # ^ | |
# | | no | # | | no |
# | v | # +--- ball scan rotation | # | | # | v |
# +--- ball scan rotation |
# | |
# | no V # | no V
# | +---------- already rotating body? # | +---------- already rotating body?
# | | | # | | |
@@ -380,6 +387,11 @@ if __name__ == '__main__':
else: else:
try: # Hit Ctrl-C to stop, cleanup and exit try: # Hit Ctrl-C to stop, cleanup and exit
state = 'tracking' state = 'tracking'
start_soll = -0.8
align_start = 0.29
curve_start = -0.1
curve_stop = 0.1
soll = start_soll
# t = None # t = None
while True: while True:
# meassure time for debbuging # meassure time for debbuging
@@ -389,21 +401,31 @@ if __name__ == '__main__':
if state == 'tracking': if state == 'tracking':
# start ball approach when ball is visible # start ball approach when ball is visible
if striker.ball_tracking(): print('Soll angle', soll)
striker.mover.set_head_angles(soll, 0, 0.5)
sleep(0.3)
if striker.ball_tracking(soll=soll):
striker.speak("Ball approach") striker.speak("Ball approach")
state = 'ball_approach' state = 'ball_approach'
elif state == 'ball_approach': elif state == 'ball_approach':
ball_in_lower = striker.get_ball_angles_from_camera( bil = striker.get_ball_angles_from_camera(
striker.lower_camera striker.lower_camera
) ) # Ball in lower
print(ball_in_lower) print('Ball in lower', bil)
if (ball_in_lower is not None and ball_in_lower[1] > 0.20): if bil is not None:
if bil[1] > curve_start and bil[1] < curve_stop:
striker.speak('Going linear')
soll = (-start_soll / (curve_stop - curve_start)
* bil[1]
+ start_soll
/ (curve_stop - curve_start) * curve_stop)
elif bil[1] > align_start:
print('Ball is close enough, stop approach') print('Ball is close enough, stop approach')
striker.mover.stop_moving() striker.mover.stop_moving()
striker.speak('Align to goal') striker.speak('Align to goal')
state = 'goal_align' state = 'align'
else: continue
print('Continue running') print('Continue running')
striker.run_to_ball() striker.run_to_ball()
state = 'tracking' state = 'tracking'