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

Robot died, started refactoring

Browse Source
This commit is contained in:
Pavel Lutskov
2018-06-29 15:15:34 +02:00
parent 3a92d8530d
commit 93ed282170
4 changed files with 135 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
pykick/move_robot.py
View File

@@ -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)')

55
pykick/movements.py
View File

@@ -1,8 +1,11 @@
# from time import sleep # from time import sleep
import argparse
from math import radians from math import radians
from naoqi import ALProxy from naoqi import ALProxy
from .utils import read_config
class NaoMover(object): class NaoMover(object):
@@ -89,7 +92,6 @@ class NaoMover(object):
joints.append(sides[side] + joint) joints.append(sides[side] + joint)
angles.append(radians(angle)) angles.append(radians(angle))
self.mp.angleInterpolation(joints, angles, wait, True) self.mp.angleInterpolation(joints, angles, wait, True)
# sleep(wait)
self.stand_up(0.5) self.stand_up(0.5)
self.set_arm_stiffness() self.set_arm_stiffness()
@@ -174,8 +176,13 @@ class NaoMover(object):
if not self.ready_to_move: if not self.ready_to_move:
self.mp.moveInit() self.mp.moveInit()
self.ready_to_move = True 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): def move_toward(self, front, side, rotation):
if not self.ready_to_move: if not self.ready_to_move:
@@ -183,10 +190,50 @@ class NaoMover(object):
self.ready_to_move = True self.ready_to_move = True
self.mp.post.moveToward(front, side, rotation) self.mp.post.moveToward(front, side, rotation)
def wait(self): def wait(self):
self.mp.waitUntilMoveIsFinished() self.mp.waitUntilMoveIsFinished()
def stop_moving(self): def stop_moving(self):
print('STOOOP') print('STOOOP')
self.mp.stopMove() 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()

4
pykick/nao_defaults.json
View File

@@ -11,7 +11,6 @@
255 255
] ]
], ],
"cam": 1,
"goal": [ "goal": [
[ [
0, 0,
@@ -24,6 +23,7 @@
255 255
] ]
], ],
"fps": 10,
"res": 2, "res": 2,
"ball_min_radius": 0.01, "ball_min_radius": 0.01,
"field": [ "field": [
@@ -38,7 +38,7 @@
255 255
] ]
], ],
"fps": 10, "cam": 1,
"ip": "192.168.0.11", "ip": "192.168.0.11",
"port": 9559 "port": 9559
} }

63
pykick/striker.py
View File

@@ -1,7 +1,6 @@
from __future__ import print_function from __future__ import print_function
from __future__ import division from __future__ import division
import argparse
from threading import Thread from threading import Thread
from math import pi,tan,asin from math import pi,tan,asin
from time import sleep, time from time import sleep, time
@@ -157,7 +156,6 @@ class Striker(object):
y_angle=angles[1] y_angle=angles[1]
y_angle=pi/2-y_angle-15*pi/180 y_angle=pi/2-y_angle-15*pi/180
distance = 0.5 * tan(y_angle) distance = 0.5 * tan(y_angle)
# vertical distance to ball at the end # vertical distance to ball at the end
vert_dist=0.3 vert_dist=0.3
@@ -183,7 +181,7 @@ class Striker(object):
print() print()
def run_to_ball(self): 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, def turn_to_ball(self, ball_x, ball_y, tol=0.15, soll=0, fancy=False,
m_delta=0.2): m_delta=0.2):
@@ -313,9 +311,12 @@ class Striker(object):
def close(self): def close(self):
self.is_over = True self.is_over = True
if self.tts_thread.isAlive():
self.tts_thread.join() self.tts_thread.join()
self.upper_camera.close() self.upper_camera.close()
self.lower_camera.close() self.lower_camera.close()
self.mover.stop_moving()
# self.mover.stand_up()
def goal_search(self): def goal_search(self):
goal_center_x = None goal_center_x = None
@@ -389,58 +390,21 @@ class Striker(object):
if __name__ == '__main__': if __name__ == '__main__':
try: # Hit Ctrl-C to stop, cleanup and exit
cfg = read_config() cfg = read_config()
striker = Striker( striker = Striker(
nao_ip=cfg['ip'], nao_ip=cfg['ip'], nao_port=cfg['port'],
nao_port=cfg['port'], res=cfg['res'], ball_hsv=cfg['ball'],
res=cfg['res'], goal_hsv=cfg['goal'], field_hsv=cfg['field'],
ball_hsv=cfg['ball'],
goal_hsv=cfg['goal'],
field_hsv=cfg['field'],
ball_min_radius=cfg['ball_min_radius'], 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
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")
args = parser.parse_args()
if args.stand:
striker.mover.stand_up()
striker.close()
elif args.rest:
striker.mover.rest()
striker.close()
# 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' state = 'tracking'
# 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
while True: while True:
# meassure time for debbuging # meassure time for debbuging
loop_start = time() loop_start = time()
@@ -510,3 +474,4 @@ if __name__ == '__main__':
print('\n\n') print('\n\n')
finally: finally:
striker.close() striker.close()
striker.mover.rest()