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

Goal ball alignment robuster

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-24 11:33:37 +02:00
parent e0955d0fee
commit 0e6d8a1acb
2 changed files with 45 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
pykick/move_robot.py
View File

@@ -8,11 +8,12 @@ if __name__ == "__main__":
mover.stand_up() mover.stand_up()
while True: while True:
amount = float(raw_input('How much: ')) amount = float(raw_input('How much: '))
mover.move_to(0, 0, 0.5 * amount) mover.move_to(0, amount, 0)
mover.wait() # mover.move_to(0, 0, 0.5 * amount)
mover.move_to(0, -0.3 * amount, 0) # mover.wait()
mover.wait() # mover.move_to(0, -0.3 * amount, 0)
mover.move_to(-0.1 * abs(amount), 0, 0) # mover.wait()
# mover.move_to(-0.1 * abs(amount), 0, 0)
# amount = float(raw_input('How much: ')) # amount = float(raw_input('How much: '))
# if axis == 0: # if axis == 0:
# mover.move_to(amount, 0, 0) # mover.move_to(amount, 0, 0)

75
pykick/striker.py
View File

@@ -25,7 +25,8 @@ class Striker(object):
fps=30, cam_id=1) fps=30, cam_id=1)
self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]), self.ball_finder = BallFinder(tuple(ball_hsv[0]), tuple(ball_hsv[1]),
ball_min_radius) ball_min_radius)
self.field_finder = FieldFinder(tuple(field_hsv[0]), tuple(field_hsv[1])) self.field_finder = FieldFinder(tuple(field_hsv[0]),
tuple(field_hsv[1]))
self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1])) self.goal_finder = GoalFinder(tuple(goal_hsv[0]), tuple(goal_hsv[1]))
self.lock_counter = 0 self.lock_counter = 0
self.loss_counter = 0 self.loss_counter = 0
@@ -41,8 +42,11 @@ class Striker(object):
(self.tts_thread is None or not self.tts_thread.isAlive()) (self.tts_thread is None or not self.tts_thread.isAlive())
and text != self.last_speak and text != self.last_speak
): ):
if (self.last_speak=="Where is the ball? I am searching for it" and text=="Going to rotate"): if (
text="I have found the ball" self.last_speak == "Where is the ball? I am searching for it"
and text == "Going to rotate"
):
text = "I have found the ball"
self.tts_thread = Thread( self.tts_thread = Thread(
target=lambda text: self.speaker.say(str(text)), target=lambda text: self.speaker.say(str(text)),
args=(text,) args=(text,)
@@ -186,6 +190,9 @@ class Striker(object):
self.mover.move_to(0, 0, yaw) self.mover.move_to(0, 0, yaw)
self.mover.wait() self.mover.wait()
# def move_sideways(self, dy):
# sign = 1 if dy > 0 else -1
def align_to_ball(self): def align_to_ball(self):
ball_angles = self.get_ball_angles_from_camera(self.lower_camera) ball_angles = self.get_ball_angles_from_camera(self.lower_camera)
if ball_angles is None: if ball_angles is None:
@@ -207,19 +214,19 @@ class Striker(object):
def align_to_goal(self): def align_to_goal(self):
ball_angles = self.get_ball_angles_from_camera(self.lower_camera) ball_angles = self.get_ball_angles_from_camera(self.lower_camera)
if ball_angles is None: if ball_angles is None:
self.speak("Cannot see the ball") self.mover.move_to(-0.1, 0, 0)
raise ValueError('No ball') self.mover.wait()
ball_angles = self.get_ball_angles_from_camera(self.lower_camera)
if ball_angles is None:
self.speak("Cannot see the ball")
raise ValueError('No ball')
x, y = ball_angles x, y = ball_angles
print(x, y) print(x, y)
self.speak("Turn to ball") self.speak("Turn to ball")
self.turn_to_ball(x, y, tol=0.15) self.turn_to_ball(x, y, tol=0.15)
goal_center_x = None goal_center_x = self.goal_search()
for i in range(3):
if goal_center_x is None:
goal_center_x = self.get_goal_center_angle_from_camera(
self.upper_camera
)
print('Goal center:', goal_center_x) print('Goal center:', goal_center_x)
if goal_center_x is not None and abs(goal_center_x) < 0.1: if goal_center_x is not None and abs(goal_center_x) < 0.1:
self.speak("Goal and ball are aligned") self.speak("Goal and ball are aligned")
@@ -227,7 +234,6 @@ class Striker(object):
#raise SystemExit #raise SystemExit
return True return True
if y > 0.35: if y > 0.35:
self.speak("moving backward") self.speak("moving backward")
self.mover.move_to(-0.05, 0, 0) self.mover.move_to(-0.05, 0, 0)
@@ -239,10 +245,13 @@ class Striker(object):
self.mover.wait() self.mover.wait()
# return False # return False
self.mover.move_to(0, 0.2, 0) sign = -1 if goal_center_x > 0 else 1
num_steps = int(min(abs(goal_center_x), 0.2) // 0.05)
self.speak("Moving sideways") self.speak("Moving sideways")
print('Moving sideways') print('Moving sideways')
self.mover.wait() for _ in range(num_steps):
self.mover.move_to(0, 0.05 * sign, 0)
self.mover.wait()
print('Finished moving') print('Finished moving')
return False return False
@@ -251,8 +260,7 @@ class Striker(object):
self.upper_camera.close() self.upper_camera.close()
self.lower_camera.close() self.lower_camera.close()
def ball_and_goal_search(self): def goal_search(self):
ball_x = None
goal_center_x = None goal_center_x = None
angles = [0, -pi/6, -pi/3, pi/6, pi/3] angles = [0, -pi/6, -pi/3, pi/6, pi/3]
for angle in angles: for angle in angles:
@@ -268,15 +276,10 @@ class Striker(object):
print('Goal found: ' + str(goal_center_x) print('Goal found: ' + str(goal_center_x)
if goal_center_x is not None if goal_center_x is not None
else 'Goal not found at ' + str(angle)) else 'Goal not found at ' + str(angle))
if ball_x is None: if goal_center_x is not None:
bx = self.get_ball_angles_from_camera( self.mover.set_head_angles(0, 0)
self.lower_camera, mask=False return goal_center_x
) self.mover.set_head_angles(0, 0)
ball_x = bx[0] + angle if bx is not None else None
print('Ball found: ' + str(ball_x) if ball_x is not None
else 'Ball not found at ' + str(angle))
if ball_x is not None and goal_center_x is not None:
return ball_x, goal_center_x
return None return None
@@ -413,17 +416,17 @@ if __name__ == '__main__':
striker.mover.wait() striker.mover.wait()
state = 'tracking' state = 'tracking'
elif state == 'align': # elif state == 'align':
striker.mover.set_head_angles(0, 0.25, 0.3) # striker.mover.set_head_angles(0, 0.25, 0.3)
sleep(0.5) # sleep(0.5)
try: # try:
success = striker.align_to_ball() # success = striker.align_to_ball()
sleep(0.3) # sleep(0.3)
if success: # if success:
state = 'kick' # state = 'kick'
except ValueError: # except ValueError:
striker.mover.set_head_angles(0, 0, 0.3) # striker.mover.set_head_angles(0, 0, 0.3)
state = 'tracking' # state = 'tracking'
elif state == 'goal_align': elif state == 'goal_align':
# print(striker.ball_and_goal_search()) # print(striker.ball_and_goal_search())