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

Improving sideways approach

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-30 12:45:05 +02:00
parent ffeed426e6
commit 5c4cfa0842
2 changed files with 45 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
pykick/__main__.py
View File

@@ -70,13 +70,19 @@ if __name__ == '__main__':
striker.mover.stand_up() striker.mover.stand_up()
state = 'init' state = 'init'
init_soll = 0.0 # init_soll = 0.0
align_start = 0.15 # align_start = 0.15
curve_start = -0.1 # curve_start = -0.1
curve_stop = 0.1 # curve_stop = 0.1
soll = init_soll # soll = init_soll
striker.speak("Initialized") striker.speak("Initialized")
approach_steps = 0
loop_start = time()
while True: while True:
loop_end = time()
print('Loop time:', loop_end - loop_start)
print('\n\n')
# meassure time for debbuging # meassure time for debbuging
loop_start = time() loop_start = time()
print('State:', state) print('State:', state)
@@ -85,7 +91,9 @@ if __name__ == '__main__':
striker.mover.set_head_angles(0, 0) striker.mover.set_head_angles(0, 0)
striker.speak("Start the Ball tracking") striker.speak("Start the Ball tracking")
striker.ball_tracking(tol=0.05) striker.ball_tracking(tol=0.05)
striker.speak("I have found the Ball, starting with. Goal search") striker.speak(
"I have found the Ball, starting with. Goal search"
)
goal_center = striker.goal_search() goal_center = striker.goal_search()
if goal_center <0: if goal_center <0:
striker.speak("I have found the. goal on the right") striker.speak("I have found the. goal on the right")
@@ -93,7 +101,7 @@ if __name__ == '__main__':
else: else:
striker.speak("I have found the. goal on the left") striker.speak("I have found the. goal on the left")
approach = -1 approach = -1
striker.mover.set_head_angles(0, 0) striker.mover.set_head_angles(0, 0)
#approach = 1 if goal_center < 0 else -1 #approach = 1 if goal_center < 0 else -1
#approach = 1 #approach = 1
@@ -105,22 +113,29 @@ if __name__ == '__main__':
print('Soll angle') print('Soll angle')
striker.ball_tracking(tol=0.05) striker.ball_tracking(tol=0.05)
# break # break
state = 'align' if approach_steps < 2:
state = 'ball_approach'
else:
state = 'goal_align'
elif state == 'ball_approach': elif state == 'ball_approach':
# bil = striker.get_ball_angles_from_camera( bil = striker.get_ball_angles_from_camera(
# striker.lower_camera striker.lower_camera
# ) # Ball in lower ) # Ball in lower
# print('Ball in lower', bil)
#striker.speak("I have found the ball. Starting ball approach")
try: try:
d = striker.distance_to_ball() d = striker.distance_to_ball()
except ValueError: except ValueError:
state = 'tracking' if bil is not None:
state = 'goal_align'
striker.speak('Ball is close. Align to. Goal')
else:
state = 'tracking'
continue continue
print('Distance to ball', d) print('Distance to ball', d)
striker.speak("The distance to the ball is approximately "+str(round(d,2))+" Meters") striker.speak("The distance to the ball is approximately "
+ str(round(d,2)) + " Meters")
angle = striker.walking_direction(approach, d) angle = striker.walking_direction(approach, d)
d_run = d * cos(angle) d_run = d * cos(angle)
print('Approach angle', angle) print('Approach angle', angle)
@@ -128,14 +143,16 @@ if __name__ == '__main__':
striker.mover.move_to(0, 0, angle) striker.mover.move_to(0, 0, angle)
striker.mover.wait() striker.mover.wait()
striker.run_to_ball(d_run) striker.run_to_ball(d_run)
striker.speak("I think I have reached the ball. I will start rotating") striker.speak("I think I have reached the ball. " +
"I will start rotating")
approach_steps += 1
striker.mover.move_to(0, 0, -pi/2 * approach) striker.mover.move_to(0, 0, -pi/2 * approach)
striker.mover.wait()
state = 'tracking' state = 'tracking'
elif state == 'goal_align': elif state == 'goal_align':
try: try:
if striker.align_to_goal(): if striker.align_to_goal():
striker.speak('Why am I in align. This makes no sense')
state = "align" state = "align"
except ValueError: except ValueError:
state = 'tracking' state = 'tracking'
@@ -176,10 +193,6 @@ if __name__ == '__main__':
##striker.speak("Nice kick. Let's do a dance") ##striker.speak("Nice kick. Let's do a dance")
#striker.mover.dance() #striker.mover.dance()
break break
loop_end = time()
print('Loop time:', loop_end - loop_start)
print('\n\n')
finally: finally:
striker.close() striker.close()
striker.mover.rest() striker.mover.rest()

47
pykick/striker.py
View File

@@ -219,8 +219,7 @@ class Striker(object):
self.mover.move_to(d, 0, 0) self.mover.move_to(d, 0, 0)
self.mover.wait() self.mover.wait()
def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, fancy=False, def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0):
m_delta=0.2):
"""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
@@ -245,43 +244,19 @@ class Striker(object):
print('Head yaw', head_yaw, end=' ') print('Head yaw', head_yaw, end=' ')
d_yaw = head_yaw - soll d_yaw = head_yaw - soll
print('Head d_yaw', d_yaw) print('Head d_yaw', d_yaw)
print('Rotating', self.rotating, end=' ')
print('Rotation direction', self.rot_dir, end=' ')
print('Allowed tolerance', tol) print('Allowed tolerance', tol)
if not fancy: if abs(d_yaw) > tol:
if abs(d_yaw) > tol: self.mover.stop_moving()
self.mover.stop_moving() print('Going to rotate by', d_yaw)
print('Going to rotate by', d_yaw) self.speak('Going to rotate')
self.speak('Going to rotate') self.mover.set_head_angles(soll, head_pitch, 0.3)
self.mover.set_head_angles(soll, head_pitch, 0.3) self.mover.move_to(0, 0, d_yaw)
self.mover.move_to(0, 0, d_yaw) self.mover.wait()
self.mover.wait() return False
return False
else:
print('Ball locked')
return True
else: else:
if not self.rotating: print('Ball locked')
if abs(d_yaw) > tol: return True
self.mover.stop_moving()
self.rotating = True
self.rot_dir = -1 if d_yaw > 0 else 1
print('Going to rotate')
self.speak("Going to rotate")
self.mover.move_toward(0, 0, -self.rot_dir)
# sleep(1.5)
return False
else:
print('Success')
# self.speak('Ball locked')
return True
else:
if d_yaw * self.rot_dir > -tol - m_delta:
self.rotating = False
self.mover.stop_moving()
return False
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,