Permalink
Browse files

Merge pull request #7 from robotika/feature/fix-world1

Feature/fix world1
  • Loading branch information...
m3d committed Nov 29, 2017
2 parents a0c1fe3 + 5ae2d45 commit e407cce9ea9088dd9afe8a432a0186d846da236c
Showing with 71 additions and 31 deletions.
  1. +2 −2 README.md
  2. +69 −29 myr2017.py
@@ -17,7 +17,7 @@ https://robotika.cz/competitions/move-your-robot/2017/
python3 -h
usage: myr2017.py [-h] [--host HOST] [--port PORT] [--note NOTE] [--verbose]
[--video-port VIDEO_PORT] [--replay REPLAY] [--force]
[--test {1m,90deg,loops}]
[--test {1m,90deg,loops,enter}]
Navigate Naio robot in "Move Your Robot" competition
@@ -32,7 +32,7 @@ optional arguments:
video"
--replay REPLAY replay existing log file
--force, -F force replay even for failing output asserts
--test {1m,90deg,loops}
--test {1m,90deg,loops,enter}
test cases
```
@@ -22,6 +22,12 @@
VIDEO_COMPRESSION_LEVEL = 7 # zlib parameter
# Row navigation constants
MAX_GAP_SIZE = 13 # defined for plants on both sides
OPEN_SIZE = 17
OFFSET_SIZE = 5
END_OF_ROW_SIZE = 18 + 19 # for 180deg FOV
class WrapperIO:
def __init__(self, soc, log, ignore_ref_output=False):
@@ -63,7 +69,7 @@ def annot(self, annotation):
self.log.write(ANNOT_STREAM, annotation)
def laser2ascii(scan):
def laser2ascii(scan, limit = 1.0):
"Eduro ASCII art"
step = 5
scan2 = [x == 0 and 10000 or x for x in scan]
@@ -76,7 +82,6 @@ def laser2ascii(scan):
mid = int(len(s)/2)
s = s[:mid] + 'C' + s[mid:]
limit = 1.0
left, right = mid, mid
while left > 0 and min_dist_arr[left] > limit:
left -= 1
@@ -99,55 +104,79 @@ def move_straight(robot, how_far):
robot.annot(b'TAG:move_one_meter:END')
def turn_right_90deg(robot):
robot.annot(b'TAG:turn_right_90deg:BEGIN')
robot.turn_right()
def turn_deg(robot, angle_deg):
"""turn robot for desired angle in degrees"""
# left is positive angle, right negative
if angle_deg > 0:
robot.turn_left()
elif angle_deg < 0:
robot.turn_right()
else:
# angle_deg == 0
return
gyro_sum = 0
start_time = robot.time
num_updates = 0
while robot.time - start_time < timedelta(minutes=1):
prev_time = robot.time
robot.update()
dt = robot.time - prev_time
dt = timedelta(microseconds=100000) # hack for simulation
gyro_sum += robot.gyro_raw[2] # time is required!
num_updates += 1
# the updates are 10Hz (based on laser measurements)
angle = (gyro_sum * dt.total_seconds()) * 30.5/1000.0
angle = -(gyro_sum * dt.total_seconds()) * 30.5/1000.0
# also it looks the rotation (in Simulatoz) is clockwise
if angle > 90.0: # TODO lower threshold for minor corrections
if abs(angle) > abs(angle_deg): # TODO lower threshold for minor corrections
break
robot.stop()
print('gyro_sum', gyro_sum, robot.time - start_time, num_updates)
def turn_right_90deg(robot):
robot.annot(b'TAG:turn_right_90deg:BEGIN')
turn_deg(robot, -90)
robot.annot(b'TAG:turn_right_90deg:END')
def turn_left_90deg(robot):
robot.annot(b'TAG:turn_left_90deg:BEGIN')
robot.turn_left()
gyro_sum = 0
turn_deg(robot, 90)
robot.annot(b'TAG:turn_left_90deg:END')
def enter_field(robot, verbose):
robot.annot(b'TAG:enter_field:BEGIN')
if verbose:
print('enter_field')
if robot.time is None:
robot.update()
robot.move_forward()
start_time = robot.time
num_updates = 0
while robot.time - start_time < timedelta(minutes=1):
prev_time = robot.time
while robot.time - start_time < timedelta(minutes=1):
robot.update()
dt = robot.time - prev_time
gyro_sum += robot.gyro_raw[2] # time is required!
num_updates += 1
# the updates are 10Hz (based on laser measurements)
angle = (gyro_sum * dt.total_seconds()) * 30.5/1000.0
# also it looks the rotation (in Simulatoz) is clockwise
if angle < -90.0: # TODO lower threshold for minor corrections
triplet = laser2ascii(robot.laser)
s, left, right = triplet
if left + right < END_OF_ROW_SIZE:
# i.e. there is some obstacle within 1 meter radius
break
if verbose:
print('max_dist', max(robot.laser))
robot.stop()
print('gyro_sum', gyro_sum, robot.time - start_time, num_updates)
robot.annot(b'TAG:turn_left_90deg:END')
robot.update()
robot.annot(b'TAG:enter_field:END')
def navigate_row(robot, verbose):
MAX_GAP_SIZE = 13 # defined for plants on both sides
OPEN_SIZE = 17
OFFSET_SIZE = 5
END_OF_ROW_SIZE = 18 + 19 # for 180deg FOV
"""navigate in 70cm rows or on the field border"""
# first make sure you are in the field
enter_field(robot, verbose)
robot.move_forward()
end_of_row = False
@@ -157,6 +186,9 @@ def navigate_row(robot, verbose):
max_dist = max(robot.laser)
triplet = laser2ascii(robot.laser)
s, left, right = triplet
__, left2, right2 = laser2ascii(robot.laser, limit=1.5)
if left + right < MAX_GAP_SIZE:
if left < right:
robot.move_right()
@@ -170,13 +202,14 @@ def navigate_row(robot, verbose):
# plans on the left
if left < OFFSET_SIZE:
robot.move_right()
elif left > OFFSET_SIZE:
elif left > OFFSET_SIZE and left != left2:
# ignore correction if further radius hits the same object
robot.move_left()
else:
robot.move_forward()
elif left > OPEN_SIZE and right < MAX_GAP_SIZE:
# unexpected case!
if OFFSET_SIZE < right:
if OFFSET_SIZE < right and right != right2:
robot.move_right()
elif OFFSET_SIZE > right:
robot.move_left()
@@ -186,7 +219,7 @@ def navigate_row(robot, verbose):
robot.move_forward()
if verbose:
print('%4d' % max_dist, triplet)
print('%4d' % max_dist, triplet, (left2, right2))
if left + right >= END_OF_ROW_SIZE:
end_of_row = True
@@ -291,6 +324,11 @@ def test_loops(robot, verbose):
robot.update()
def test_enter(robot, verbose):
enter_field(robot, verbose)
robot.wait(timedelta(seconds=3))
def play_game(robot, verbose):
# 1st row
@@ -331,6 +369,8 @@ def run_robot(robot, verbose=False, test_case=None):
test_90deg(robot)
elif test_case == 'loops':
test_loops(robot, verbose=verbose)
elif test_case == 'enter':
test_enter(robot, verbose=verbose)
else:
assert False, test_case # not supported
@@ -350,7 +390,7 @@ def run_robot(robot, verbose=False, test_case=None):
parser.add_argument('--force', '-F', dest='force', action='store_true',
help='force replay even for failing output asserts')
parser.add_argument('--test', dest='test_case', help='test cases',
choices=['1m', '90deg', 'loops'])
choices=['1m', '90deg', 'loops', 'enter'])
args = parser.parse_args()
if args.replay is None:

0 comments on commit e407cce

Please sign in to comment.