diff --git a/pykick/move_robot.py b/pykick/move_robot.py deleted file mode 100644 index a10d51c..0000000 --- a/pykick/move_robot.py +++ /dev/null @@ -1,25 +0,0 @@ -from .movements import NaoMover -from .utils import read_config - - -if __name__ == "__main__": - cfg = read_config() - mover = NaoMover(cfg['ip'], cfg['port']) - mover.stand_up() - while True: - amount = float(raw_input('How much: ')) - mover.move_to(0, amount, 0) - # mover.move_to(0, 0, 0.5 * amount) - # mover.wait() - # mover.move_to(0, -0.3 * amount, 0) - # mover.wait() - # mover.move_to(-0.1 * abs(amount), 0, 0) - # amount = float(raw_input('How much: ')) - # if axis == 0: - # mover.move_to(amount, 0, 0) - # elif axis == 1: - # mover.move_to(0, amount, 0) - # elif axis == 2: - # mover.move_to(0, 0, amount) - # else: - # print('Axis out of range (0, 1, 2)') diff --git a/pykick/movements.py b/pykick/movements.py index 198d58d..2ced372 100644 --- a/pykick/movements.py +++ b/pykick/movements.py @@ -1,8 +1,11 @@ # from time import sleep +import argparse from math import radians from naoqi import ALProxy +from .utils import read_config + class NaoMover(object): @@ -89,7 +92,6 @@ class NaoMover(object): joints.append(sides[side] + joint) angles.append(radians(angle)) self.mp.angleInterpolation(joints, angles, wait, True) - # sleep(wait) self.stand_up(0.5) self.set_arm_stiffness() @@ -174,8 +176,13 @@ class NaoMover(object): if not self.ready_to_move: self.mp.moveInit() self.ready_to_move = True - self.mp.post.moveTo(front, side, rotation,[['MaxStepX', 0.07999999821186066], ['MaxStepY', 0.1599999964237213], ['MaxStepTheta', 0.5235987901687622], ['MaxStepFrequency', 1.0], ['StepHeight', 0.02]]) - + self.mp.post.moveTo(front, side, rotation, [ + ['MaxStepX', 0.07999999821186066], + ['MaxStepY', 0.1599999964237213], + ['MaxStepTheta', 0.5235987901687622], + ['MaxStepFrequency', 1.0], + ['StepHeight', 0.02] + ]) def move_toward(self, front, side, rotation): if not self.ready_to_move: @@ -183,10 +190,50 @@ class NaoMover(object): self.ready_to_move = True self.mp.post.moveToward(front, side, rotation) - def wait(self): self.mp.waitUntilMoveIsFinished() def stop_moving(self): print('STOOOP') self.mp.stopMove() + + + +if __name__ == '__main__': + + cfg = read_config() + + mover = NaoMover(cfg['ip'], cfg['port']) + parser = argparse.ArgumentParser() + actions = parser.add_mutually_exclusive_group() + actions.add_argument("-s", "--stand", action="store_true", + help="let the robot stand up") + actions.add_argument("-k", "--kick", action="store_true", + help="let the robot do a fancy kick") + actions.add_argument("-r", "--rest", action="store_true", + help="let the robot rest") + actions.add_argument("-m", "--move", action="store_true", + help="move around") + + args = parser.parse_args() + + if args.stand: + mover.stand_up() + + elif args.rest: + mover.rest() + + # perform a (fancy) kick + elif args.kick: + mover.stand_up() + mover.kick() + + elif args.move: + mover.stand_up() + while True: + amount_x = float(raw_input('How much x: ')) + amount_y = float(raw_input('How much y: ')) + amount_z = float(raw_input('How much z: ')) + print(amount_x, amount_y, amount_z) + mover.move_to(amount_x, amount_y, amount_z) + mover.wait() diff --git a/pykick/nao_defaults.json b/pykick/nao_defaults.json index 127d891..915ce55 100644 --- a/pykick/nao_defaults.json +++ b/pykick/nao_defaults.json @@ -11,7 +11,6 @@ 255 ] ], - "cam": 1, "goal": [ [ 0, @@ -24,6 +23,7 @@ 255 ] ], + "fps": 10, "res": 2, "ball_min_radius": 0.01, "field": [ @@ -38,7 +38,7 @@ 255 ] ], - "fps": 10, + "cam": 1, "ip": "192.168.0.11", "port": 9559 } \ No newline at end of file diff --git a/pykick/striker.py b/pykick/striker.py index 061ae15..0903b23 100644 --- a/pykick/striker.py +++ b/pykick/striker.py @@ -1,7 +1,6 @@ from __future__ import print_function from __future__ import division -import argparse from threading import Thread from math import pi,tan,asin from time import sleep, time @@ -157,7 +156,6 @@ class Striker(object): y_angle=angles[1] y_angle=pi/2-y_angle-15*pi/180 distance = 0.5 * tan(y_angle) - # vertical distance to ball at the end vert_dist=0.3 @@ -183,7 +181,7 @@ class Striker(object): print() def run_to_ball(self): - self.mover.move_toward(1, 3 * self.dy, 0) + self.mover.move_toward(1, min(2 * self.dy, 1), 0) def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, fancy=False, m_delta=0.2): @@ -313,9 +311,12 @@ class Striker(object): def close(self): self.is_over = True - self.tts_thread.join() + if self.tts_thread.isAlive(): + self.tts_thread.join() self.upper_camera.close() self.lower_camera.close() + self.mover.stop_moving() + # self.mover.stand_up() def goal_search(self): goal_center_x = None @@ -389,124 +390,88 @@ class Striker(object): if __name__ == '__main__': - cfg = read_config() - striker = Striker( - nao_ip=cfg['ip'], - nao_port=cfg['port'], - res=cfg['res'], - ball_hsv=cfg['ball'], - goal_hsv=cfg['goal'], - field_hsv=cfg['field'], - ball_min_radius=cfg['ball_min_radius'], - ) + try: # Hit Ctrl-C to stop, cleanup and exit + cfg = read_config() + striker = Striker( + nao_ip=cfg['ip'], nao_port=cfg['port'], + res=cfg['res'], ball_hsv=cfg['ball'], + goal_hsv=cfg['goal'], field_hsv=cfg['field'], + ball_min_radius=cfg['ball_min_radius'], + ) - # allow additional arguments when running the function like - # stand - # rest - # kick - # if no argument is given the state machine is run + state = 'tracking' + init_soll = 0.0 + align_start = 0.15 + curve_start = -0.1 + curve_stop = 0.1 + soll = init_soll + while True: + # meassure time for debbuging + loop_start = time() + print('State:', state) - parser = argparse.ArgumentParser() - parser.add_argument("-s", "--stand", action="store_true", - help="let the robot stand up") - parser.add_argument("-k", "--kick", action="store_true", - help="let the robot do a fancy kick") - parser.add_argument("-r", "--rest", action="store_true", - help="let the robot rest") + if state == 'tracking': + # start ball approach when ball is visible + print('Soll angle') + striker.ball_tracking(tol=0.05) + state = 'ball_approach' - args = parser.parse_args() + elif state == 'ball_approach': + # bil = striker.get_ball_angles_from_camera( + # striker.lower_camera + # ) # Ball in lower - if args.stand: - striker.mover.stand_up() - striker.close() + # print('Ball in lower', bil) + print('Continue running') + striker.run_to_ball() + # state = 'tracking' - elif args.rest: - striker.mover.rest() - striker.close() + elif state == 'goal_align': + try: + if striker.align_to_goal(): + striker.speak('I am aligning to ball') + state = "align" + except ValueError: + state = 'tracking' - # perform a (fancy) kick - elif args.kick: - striker.mover.stand_up() - striker.mover.kick(fancy=True) - striker.close() - - # perform normal state-machine if no input argument is given - # (see diagram above) - else: - try: # Hit Ctrl-C to stop, cleanup and exit - state = 'tracking' - # init_soll = 0.0 - # align_start = 0.15 - # curve_start = -0.1 - # curve_stop = 0.1 - # soll = init_soll - - while True: - # meassure time for debbuging - loop_start = time() - print('State:', state) - - if state == 'tracking': - # start ball approach when ball is visible - print('Soll angle') - striker.ball_tracking(tol=0.05) - state = 'ball_approach' - - elif state == 'ball_approach': - # bil = striker.get_ball_angles_from_camera( - # striker.lower_camera - # ) # Ball in lower - - # print('Ball in lower', bil) - print('Continue running') - striker.run_to_ball() - # state = 'tracking' - - elif state == 'goal_align': - try: - if striker.align_to_goal(): - striker.speak('I am aligning to ball') - state = "align" - except ValueError: - 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: - striker.speak('I am going. To kick ass') - state = 'kick' - except ValueError: - pass - # striker.mover.set_head_angles(0, 0, 0.3) - # state = 'tracking' - - elif state == 'simple_kick': - striker.mover.set_head_angles(0,0.25,0.3) - striker.ball_tracking(tol=0.10, soll=0) - print('Doing the simple kick') - - # just walk a short distance forward, ball should be near - # and it will probably be kicked in the right direction - striker.speak("Simple Kick") - sleep(1) - striker.mover.move_to_fast(0.5, 0, 0) - striker.mover.wait() - break - # state = 'tracking' - - elif state == 'kick': - print('KICK!') - striker.mover.stand_up() + 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) - striker.mover.kick(fancy=True, foot='L') - break + if success: + striker.speak('I am going. To kick ass') + state = 'kick' + except ValueError: + pass + # striker.mover.set_head_angles(0, 0, 0.3) + # state = 'tracking' - loop_end = time() - print('Loop time:', loop_end - loop_start) - print('\n\n') - finally: - striker.close() + elif state == 'simple_kick': + striker.mover.set_head_angles(0,0.25,0.3) + striker.ball_tracking(tol=0.10, soll=0) + print('Doing the simple kick') + + # just walk a short distance forward, ball should be near + # and it will probably be kicked in the right direction + striker.speak("Simple Kick") + sleep(1) + striker.mover.move_to_fast(0.5, 0, 0) + striker.mover.wait() + break + # state = 'tracking' + + elif state == 'kick': + print('KICK!') + striker.mover.stand_up() + sleep(0.3) + striker.mover.kick(fancy=True, foot='L') + break + + loop_end = time() + print('Loop time:', loop_end - loop_start) + print('\n\n') + finally: + striker.close() + striker.mover.rest()