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

156
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):
@@ -272,89 +272,109 @@ if __name__ == '__main__':
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 if args:
while True: 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()
# start time meassure for debbuging else:
loop_start = time()
# 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'
# start ball approach when ball is visible # state machine of the striker
if striker.ball_tracking(): while True:
state = 'ball_approach'
# actions in the ball_approach state # start time meassure for debbuging
elif state == 'ball_approach': loop_start = time()
# get the angle of the ball in the picture of the lower camera # print the current state of the state machine
ball_in_lower = striker.get_ball_angles_from_camera( print('State:', state)
striker.video_bot
)
# print the angle of the ball in the lower camera # actions in the tracking state
print(ball_in_lower) if state == 'tracking':
# check if the ball is in the lower camera # start ball approach when ball is visible
# and the angle is above a specific threshold (ball is close enough) if striker.ball_tracking():
if (ball_in_lower is not None state = 'ball_approach'
and ball_in_lower[1] > 0.28):
print('Ball is in lower camera, go to align') # actions in the ball_approach state
#striker.mover.stop_moving() elif state == 'ball_approach':
#state = 'align'
# perform a simple kick # get the angle of the ball in the picture of the lower camera
state='simple_kick' ball_in_lower = striker.get_ball_angles_from_camera(
striker.video_bot
)
# continue moving, if the ball is not close enough # print the angle of the ball in the lower camera
# or not in the view of the lower camera print(ball_in_lower)
else:
print('Continue running')
striker.run_to_ball()
# go back to the tracking state # check if the ball is in the lower camera
state = 'tracking' # and the angle is above a specific threshold (ball is close enough)
if (ball_in_lower is not None
and ball_in_lower[1] > 0.28):
# actions in the simple_kick state print('Ball is in lower camera, go to align')
elif state == 'simple_kick': #striker.mover.stop_moving()
#striker.mover.set_head_angles(0,0.25,0.3) #state = 'align'
print('Doing the simple kick')
# just walk a short distance straight forward, # perform a simple kick
# as the ball should be straight ahead in a small distance state='simple_kick'
striker.mover.move_to(0.3,0,0)
striker.mover.wait()
# go back to the tracking state after the simple_kick # continue moving, if the ball is not close enough
state = 'tracking' # or not in the view of the lower camera
else:
print('Continue running')
striker.run_to_ball()
elif state == 'align': # go back to the tracking state
striker.mover.set_head_angles(0, 0.25, 0.3) state = 'tracking'
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 # actions in the simple_kick state
loop_end = time() elif state == 'simple_kick':
print('Loop time:', loop_end - loop_start) #striker.mover.set_head_angles(0,0.25,0.3)
finally: print('Doing the simple kick')
striker.close()
# just walk a short distance straight forward,
# as the ball should be straight ahead in a small distance
striker.mover.move_to(0.3,0,0)
striker.mover.wait()
# 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()