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

added stand rest and kick as inputarguments to stricker.py

Browse Source
This commit is contained in:
Jonas
2018-06-22 17:56:58 +02:00
parent 28a06ea0e3
commit 66b2e54d13
1 changed files with 90 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

160
pykick/striker.py
View File

@@ -8,7 +8,7 @@ from .utils import read_config
from .imagereaders import NaoImageReader from .imagereaders import NaoImageReader
from .finders import BallFinder, GoalFinder from .finders import BallFinder, GoalFinder
from .movements import NaoMover from .movements import NaoMover
import sys
class Striker(object): class Striker(object):
@@ -271,90 +271,110 @@ if __name__ == '__main__':
ball_min_radius=cfg['ball_min_radius'], ball_min_radius=cfg['ball_min_radius'],
run_after=False run_after=False
) )
args=''
try: try:
# start with ball tracking first args=sys.argv[1]
state = 'tracking' except:
print("error")
# state machine of the striker
while True:
# start time meassure for debbuging if args:
loop_start = time() if args == 'stand':
striker.mover.stand_up()
elif args == 'rest':
striker.mover.rest()
elif args == 'kick':
striker.mover.stand_up()
striker.mover.kick()
striker.mover.rest()
else:
# print the current state of the state machine
print('State:', state)
# actions in the tracking state try:
if state == 'tracking': # start with ball tracking first
state = 'tracking'
# state machine of the striker
while True:
# start ball approach when ball is visible # start time meassure for debbuging
if striker.ball_tracking(): loop_start = time()
state = 'ball_approach'
# actions in the ball_approach state # print the current state of the state machine
elif state == 'ball_approach': print('State:', state)
# get the angle of the ball in the picture of the lower camera # actions in the tracking state
ball_in_lower = striker.get_ball_angles_from_camera( if state == 'tracking':
striker.video_bot
)
# print the angle of the ball in the lower camera # start ball approach when ball is visible
print(ball_in_lower) if striker.ball_tracking():
state = 'ball_approach'
# check if the ball is in the lower camera # actions in the ball_approach state
# and the angle is above a specific threshold (ball is close enough) elif state == 'ball_approach':
if (ball_in_lower is not None
and ball_in_lower[1] > 0.28):
print('Ball is in lower camera, go to align') # get the angle of the ball in the picture of the lower camera
#striker.mover.stop_moving() ball_in_lower = striker.get_ball_angles_from_camera(
#state = 'align' striker.video_bot
)
# perform a simple kick # print the angle of the ball in the lower camera
state='simple_kick' print(ball_in_lower)
# continue moving, if the ball is not close enough # check if the ball is in the lower camera
# or not in the view of the lower camera # and the angle is above a specific threshold (ball is close enough)
else: if (ball_in_lower is not None
print('Continue running') and ball_in_lower[1] > 0.28):
striker.run_to_ball()
# go back to the tracking state print('Ball is in lower camera, go to align')
state = 'tracking' #striker.mover.stop_moving()
#state = 'align'
# actions in the simple_kick state # perform a simple kick
elif state == 'simple_kick': state='simple_kick'
#striker.mover.set_head_angles(0,0.25,0.3)
print('Doing the simple kick')
# just walk a short distance straight forward, # continue moving, if the ball is not close enough
# as the ball should be straight ahead in a small distance # or not in the view of the lower camera
striker.mover.move_to(0.3,0,0) else:
striker.mover.wait() print('Continue running')
striker.run_to_ball()
# go back to the tracking state after the simple_kick # go back to the tracking state
state = 'tracking' state = 'tracking'
elif state == 'align': # actions in the simple_kick state
striker.mover.set_head_angles(0, 0.25, 0.3) elif state == 'simple_kick':
sleep(0.5) #striker.mover.set_head_angles(0,0.25,0.3)
try: print('Doing the simple kick')
success = striker.align_to_ball()
sleep(0.3)
if success:
state = 'kick'
except ValueError:
striker.mover.set_head_angles(0, 0, 0.3)
state = 'tracking'
elif state == 'kick':
print('KICK!')
striker.mover.kick()
break
# stop time meassuring for debbuging and print the time of the loop # just walk a short distance straight forward,
loop_end = time() # as the ball should be straight ahead in a small distance
print('Loop time:', loop_end - loop_start) striker.mover.move_to(0.3,0,0)
finally: striker.mover.wait()
striker.close()
# go back to the tracking state after the simple_kick
state = 'tracking'
elif state == 'align':
striker.mover.set_head_angles(0, 0.25, 0.3)
sleep(0.5)
try:
success = striker.align_to_ball()
sleep(0.3)
if success:
state = 'kick'
except ValueError:
striker.mover.set_head_angles(0, 0, 0.3)
state = 'tracking'
elif state == 'kick':
print('KICK!')
striker.mover.kick()
break
# stop time meassuring for debbuging and print the time of the loop
loop_end = time()
print('Loop time:', loop_end - loop_start)
finally:
striker.close()