def process_lift_axis(self, i):
i = math.copysign(deadzone(i, RobotMap.OI.lift_deadband)**2, i)
if i > 0:
# lifting
mapped = map_value(i, 0, 1, 0, RobotMap.IntakeLift.max_power_up)
else:
# lowering
mapped = map_value(i, -1, 0, -RobotMap.IntakeLift.max_power_down,
0)
return mapped
def art_step(map):
if map.stage is not None:
stage_mode, stage_step, stage1_y, stage1_last_index = map.stage
stage1_last = map.stones[stage1_last_index] if stage1_last_index is not None else None
else:
stage_mode = 0
stage1_y = None
stage1_last = None
stage_step = 0
# Color range
global min_l, max_l
if min_l is None:
min_l = min(map.stones, key=lambda x: x.color[0]).color[0]
if max_l is None:
max_l = max(map.stones, key=lambda x: x.color[0]).color[0]
global flower_seeds
# Generate seeds
if flower_seeds is None:
max_x, max_y = map.size[0], map.size[1]
flower_seeds = []
for i in range(9):
margin_y = 180
y = map_value(i, 0, 8, margin_y, max_y - margin_y)
if i % 2 == 0:
x = max_x - 400.0
else:
x = max_x - 1150.0
flower_seeds.append((x, y))
index, new_center, new_angle = None, None, None
# clean unusable holes
map.holes = [h for h in map.holes if not in_workarea(h) and h.center[0] + h.size <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1)]
if stage_mode == 0: # Clear area
sel = [s for s in map.stones if not s.flag and not in_workarea(s) and s.center[0] + s.size[0] > WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1) and s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step) ]
if sel:
s = sel[0]
index = s.index
bucket = map_value(s.color[0], min_l, max_l, 0, len(flower_seeds) + 1)
bucket = constrain(int(bucket), 0, len(flower_seeds) - 1)
new_center, new_angle = find_flower_pos(map, s, flower_seeds[bucket])
elif stage_mode == 1: # Fill line
untouched_sel = [s for s in map.stones if s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1)]
workarea_sel = [s for s in map.stones if in_workarea(s)]
max_fill = 2000
rand_thresh = max(max_fill - len(workarea_sel), 0) / float(max_fill)
if len(workarea_sel) > max_fill * 0.5 and random() > rand_thresh:
total_sel = workarea_sel
else:
total_sel = workarea_sel + untouched_sel
total_sel = workarea_sel + untouched_sel
sel = [s for s in total_sel if not s.flag]
stripes = 4
stripe_gap = map.maxstonesize
stripe_width = ((1650 - 50) - stripe_gap * (stripes - 1)) / stripes
if sel:
if stage1_y is None:
# first run of this stage
stage1_y = 50
s = find_by_stripe(0, sel)
stage1_last = s
else:
# gaps
for i in range(0, stripes - 1):
if stage1_y >= 50 + (stripe_width + stripe_gap) * i + stripe_width and stage1_y < 50 + (stripe_width + stripe_gap) * (i + 1):
stage1_y = 50 + (stripe_width + stripe_gap) * (i + 1)
break
# find stripe index
si = 0
for i in range(0, stripes):
if stage1_y >= 50 + (stripe_width + stripe_gap) * i and stage1_y < 50 + (stripe_width + stripe_gap) * i + stripe_width:
si = i
break
s = find_by_stripe(si, sel)
stage1_y += stage1_last.size[1] + s.size[1] + 5
stage1_last = s
index = s.index
new_angle = 0
x = WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 0.5)
new_center = x, stage1_y
if stage1_y > 1650:
stage1_y = None
stage_step += 1
stage_mode = 0
elif stage_mode == 2: # Done
pass
if index is not None:
force_advance = False
log.debug('Art stage mode %d: stone %s => new center: %s, new angle: %s', stage_mode, index, str(new_center), str(new_angle))
else:
force_advance = True
stage_mode = min(stage_mode + 1, MAX_STAGE_MODE)
log.debug('Art stage mode %d: None', stage_mode)
stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map
return index, new_center, new_angle, stage, force_advance
def art_step(map):
if map.stage is not None:
stage_mode, stage_step, stage1_y, stage1_last_index = map.stage
stage1_last = map.stones[stage1_last_index] if stage1_last_index is not None else None
else:
stage_mode = 0
stage1_y = None
stage1_last = None
stage_step = 0
# Color range
global min_l, max_l
if min_l is None:
min_l = min(map.stones, key=lambda x: x.color[0]).color[0]
if max_l is None:
max_l = max(map.stones, key=lambda x: x.color[0]).color[0]
global flower_seeds
# Generate seeds
if flower_seeds is None:
max_x, max_y = map.size[0], map.size[1]
flower_seeds = []
for i in range(9):
margin_y = 180
y = map_value(i, 0, 8, margin_y, max_y - margin_y)
if i % 2 == 0:
x = max_x - 400.0
else:
x = max_x - 1150.0
flower_seeds.append((x, y))
index, new_center, new_angle = None, None, None
# clean unusable holes
map.holes = [
h
for h in map.holes
if not in_workarea(h) and h.center[0] + h.size <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1)
]
art_center = (0, map.size[1] / 2)
if stage_mode == 0: # Clear area
sel = [
s
for s in map.stones
if not s.flag
and not in_workarea(s)
and s.center[0] + s.size[0] > WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1)
and s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step)
]
if sel:
s = sel[0]
index = s.index
bucket = map_value(s.color[0], min_l, max_l, 0, len(flower_seeds) + 1)
bucket = constrain(int(bucket), 0, len(flower_seeds) - 1)
new_center, new_angle = find_flower_pos(map, s, flower_seeds[bucket])
elif stage_mode == 1: # Fill line
untouched_sel = [
s
for s in map.stones
if s.center[0] + s.size[0] <= WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 1)
]
workarea_sel = [s for s in map.stones if in_workarea(s)]
max_fill = 2000
rand_thresh = max(max_fill - len(workarea_sel), 0) / float(max_fill)
if len(workarea_sel) > max_fill * 0.5 and random() > rand_thresh:
total_sel = workarea_sel
else:
total_sel = workarea_sel + untouched_sel
total_sel = workarea_sel + untouched_sel
sel = [s for s in total_sel if not s.flag]
if sel:
if stage1_y is None:
# first run of this stage
stage1_y = 50
# pick random stone
if stage1_last is not None:
s = find_most_distant_color(stage1_last, sel)
else:
s = choice(sel)
stage1_last = s
else:
s = find_best_match(stage1_last, sel)
rc = abs(math.cos(math.radians(stage1_last.angle)))
rs = abs(math.sin(math.radians(stage1_last.angle)))
a, b = stage1_last.size[1] * rc, stage1_last.size[0] * rs
c, d = s.size[1] * rc, s.size[0] * rs
e, f = math.sqrt(a * a + b * b), math.sqrt(c * c + d * d)
stage1_y += e + f + 5
stage1_last = s
index = s.index
x = WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 0.5)
new_center = x, stage1_y
new_angle = atan_angle(art_center, new_center)
if stage1_y > 1650:
stage1_y = None
stage_step += 1
stage_mode = 0
elif stage_mode == 2: # Done
pass
if index is not None:
force_advance = False
log.debug(
"Art stage mode %d: stone %s => new center: %s, new angle: %s",
stage_mode,
index,
str(new_center),
str(new_angle),
)
else:
force_advance = True
stage_mode = min(stage_mode + 1, MAX_STAGE_MODE)
log.debug("Art stage mode %d: None", stage_mode)
stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map
return index, new_center, new_angle, stage, force_advance
def process_turbo_mode(self):
modifier = self.get_turbo_mode_modifier()
x = map_value(modifier, 0, 1, RobotMap.Drivetrain.max_motor_power,
RobotMap.Drivetrain.turbo_mode_power)
return x
def process_climb_axis(self, i):
i = math.copysign(deadzone(i, RobotMap.OI.climb_deadband)**2, i)
return map_value(i, -1, 1, -RobotMap.Climber.max_power,
RobotMap.Climber.max_power)
def art_step(map):
if map.stage is not None:
stage_mode, stage_step, stage1_y, stage1_last_index = map.stage
stage1_last = map.stones[
stage1_last_index] if stage1_last_index is not None else None
else:
stage_mode = 0
stage1_y = None
stage1_last = None
stage_step = 0
# Color range
global min_l, max_l
if min_l is None:
min_l = min(map.stones, key=lambda x: x.color[0]).color[0]
if max_l is None:
max_l = max(map.stones, key=lambda x: x.color[0]).color[0]
global flower_seeds
# Generate seeds
if flower_seeds is None:
max_x, max_y = map.size[0], map.size[1]
flower_seeds = []
for i in range(9):
margin_y = 180
y = map_value(i, 0, 8, margin_y, max_y - margin_y)
if i % 2 == 0:
x = max_x - 400.0
else:
x = max_x - 1150.0
flower_seeds.append((x, y))
index, new_center, new_angle = None, None, None
# clean unusable holes
map.holes = [
h for h in map.holes
if not in_workarea(h) and h.center[0] + h.size <= WORKAREA_START_X -
(map.maxstonesize + 10) * (stage_step + 1)
]
art_center = (0, map.size[1] / 2)
if stage_mode == 0: # Clear area
sel = [
s for s in map.stones
if not s.flag and not in_workarea(s) and s.center[0] +
s.size[0] > WORKAREA_START_X - (map.maxstonesize + 10) *
(stage_step + 1) and s.center[0] + s.size[0] <= WORKAREA_START_X -
(map.maxstonesize + 10) * (stage_step)
]
if sel:
s = sel[0]
index = s.index
bucket = map_value(s.color[0], min_l, max_l, 0,
len(flower_seeds) + 1)
bucket = constrain(int(bucket), 0, len(flower_seeds) - 1)
new_center, new_angle = find_flower_pos(map, s,
flower_seeds[bucket])
elif stage_mode == 1: # Fill line
untouched_sel = [
s for s in map.stones
if s.center[0] + s.size[0] <= WORKAREA_START_X -
(map.maxstonesize + 10) * (stage_step + 1)
]
workarea_sel = [s for s in map.stones if in_workarea(s)]
max_fill = 2000
rand_thresh = max(max_fill - len(workarea_sel), 0) / float(max_fill)
if len(workarea_sel) > max_fill * 0.5 and random() > rand_thresh:
total_sel = workarea_sel
else:
total_sel = workarea_sel + untouched_sel
total_sel = workarea_sel + untouched_sel
sel = [s for s in total_sel if not s.flag]
if sel:
if stage1_y is None:
# first run of this stage
stage1_y = 50
# pick random stone
if stage1_last is not None:
s = find_most_distant_color(stage1_last, sel)
else:
s = choice(sel)
stage1_last = s
else:
s = find_best_match(stage1_last, sel)
rc = abs(math.cos(math.radians(stage1_last.angle)))
rs = abs(math.sin(math.radians(stage1_last.angle)))
a, b = stage1_last.size[1] * rc, stage1_last.size[0] * rs
c, d = s.size[1] * rc, s.size[0] * rs
e, f = math.sqrt(a * a + b * b), math.sqrt(c * c + d * d)
stage1_y += e + f + 5
stage1_last = s
index = s.index
x = WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 0.5)
new_center = x, stage1_y
new_angle = atan_angle(art_center, new_center)
if stage1_y > 1650:
stage1_y = None
stage_step += 1
stage_mode = 0
elif stage_mode == 2: # Done
pass
if index is not None:
force_advance = False
log.debug(
'Art stage mode %d: stone %s => new center: %s, new angle: %s',
stage_mode, index, str(new_center), str(new_angle))
else:
force_advance = True
stage_mode = min(stage_mode + 1, MAX_STAGE_MODE)
log.debug('Art stage mode %d: None', stage_mode)
stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map
return index, new_center, new_angle, stage, force_advance
def art_step(map):
if map.stage is not None:
stage_mode, stage_step, stage1_y, stage1_last_index = map.stage
stage1_last = map.stones[
stage1_last_index] if stage1_last_index is not None else None
else:
stage_mode = 0
stage1_y = None
stage1_last = None
stage_step = 0
# Color range
global min_l, max_l
if min_l is None:
min_l = min(map.stones, key=lambda x: x.color[0]).color[0]
if max_l is None:
max_l = max(map.stones, key=lambda x: x.color[0]).color[0]
global flower_seeds
# Generate seeds
if flower_seeds is None:
max_x, max_y = map.size[0], map.size[1]
flower_seeds = []
for i in range(9):
margin_y = 180
y = map_value(i, 0, 8, margin_y, max_y - margin_y)
if i % 2 == 0:
x = max_x - 400.0
else:
x = max_x - 1150.0
flower_seeds.append((x, y))
index, new_center, new_angle = None, None, None
# clean unusable holes
map.holes = [
h for h in map.holes
if not in_workarea(h) and h.center[0] + h.size <= WORKAREA_START_X -
(map.maxstonesize + 10) * (stage_step + 1)
]
if stage_mode == 0: # Clear area
sel = [
s for s in map.stones
if not s.flag and not in_workarea(s) and s.center[0] +
s.size[0] > WORKAREA_START_X - (map.maxstonesize + 10) *
(stage_step + 1) and s.center[0] + s.size[0] <= WORKAREA_START_X -
(map.maxstonesize + 10) * (stage_step)
]
if sel:
s = sel[0]
index = s.index
bucket = map_value(s.color[0], min_l, max_l, 0,
len(flower_seeds) + 1)
bucket = constrain(int(bucket), 0, len(flower_seeds) - 1)
new_center, new_angle = find_flower_pos(map, s,
flower_seeds[bucket])
elif stage_mode == 1: # Fill line
untouched_sel = [
s for s in map.stones
if s.center[0] + s.size[0] <= WORKAREA_START_X -
(map.maxstonesize + 10) * (stage_step + 1)
]
workarea_sel = [s for s in map.stones if in_workarea(s)]
max_fill = 2000
rand_thresh = max(max_fill - len(workarea_sel), 0) / float(max_fill)
if len(workarea_sel) > max_fill * 0.5 and random() > rand_thresh:
total_sel = workarea_sel
else:
total_sel = workarea_sel + untouched_sel
total_sel = workarea_sel + untouched_sel
sel = [s for s in total_sel if not s.flag]
stripes = 4
stripe_gap = map.maxstonesize
stripe_width = ((1650 - 50) - stripe_gap * (stripes - 1)) / stripes
if sel:
if stage1_y is None:
# first run of this stage
stage1_y = 50
s = find_by_stripe(0, sel)
stage1_last = s
else:
# gaps
for i in range(0, stripes - 1):
if stage1_y >= 50 + (
stripe_width + stripe_gap
) * i + stripe_width and stage1_y < 50 + (
stripe_width + stripe_gap) * (i + 1):
stage1_y = 50 + (stripe_width + stripe_gap) * (i + 1)
break
# find stripe index
si = 0
for i in range(0, stripes):
if stage1_y >= 50 + (stripe_width +
stripe_gap) * i and stage1_y < 50 + (
stripe_width +
stripe_gap) * i + stripe_width:
si = i
break
s = find_by_stripe(si, sel)
stage1_y += stage1_last.size[1] + s.size[1] + 5
stage1_last = s
index = s.index
new_angle = 0
x = WORKAREA_START_X - (map.maxstonesize + 10) * (stage_step + 0.5)
new_center = x, stage1_y
if stage1_y > 1650:
stage1_y = None
stage_step += 1
stage_mode = 0
elif stage_mode == 2: # Done
pass
if index is not None:
force_advance = False
log.debug(
'Art stage mode %d: stone %s => new center: %s, new angle: %s',
stage_mode, index, str(new_center), str(new_angle))
else:
force_advance = True
stage_mode = min(stage_mode + 1, MAX_STAGE_MODE)
log.debug('Art stage mode %d: None', stage_mode)
stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map
return index, new_center, new_angle, stage, force_advance
def art_step(map):
if map.stage is not None:
stage_mode, stage_step, stage1_y, stage1_last_index = map.stage
stage1_last = map.stones[stage1_last_index] if stage1_last_index is not None else None
else:
stage_mode = 0
stage1_y = None
stage1_last = None
stage_step = 0
# Color range
global min_l, max_l
if min_l is None:
min_l = min(map.stones, key=lambda x: x.color[0]).color[0]
if max_l is None:
max_l = max(map.stones, key=lambda x: x.color[0]).color[0]
global flower_seeds
# Generate seeds
if flower_seeds is None:
max_x, max_y = map.size[0], map.size[1]
flower_seeds = []
for i in range(9):
margin_y = 180
y = map_value(i, 0, 8, margin_y, max_y - margin_y)
if i % 2 == 0:
x = max_x - 400.0
else:
x = max_x - 1150.0
flower_seeds.append((x, y))
index, new_center, new_angle = None, None, None
map.holes = []
art_center = (WORKAREA_START_X / 2, map.size[1] / 2)
print stage_mode
if stage_mode == 0: # Clear circle strip
sel = [s for s in map.stones if not s.flag and distance(s.center, art_center) >= 100.0 * (stage_step) and distance(s.center, art_center) < 100.0 * (stage_step + 1) ]
if sel:
s = choice(sel)
index = s.index
bucket = map_value(s.color[0], min_l, max_l, 0, len(flower_seeds) + 1)
bucket = constrain(int(bucket), 0, len(flower_seeds) - 1)
new_center, new_angle = find_flower_pos(map, s, flower_seeds[bucket])
elif stage_mode == 1: # Fill circle strip
sel = [s for s in map.stones if not s.flag and distance(s.center, art_center) >= 100.0 * (stage_step + 1) ]
if sel:
s = find_darkest(sel)
index = s.index
new_center, new_angle = find_flower_pos(map, s, art_center, max_radius=(100.0 * (stage_step + 1)), workarea=False)
if new_center is None or new_angle is None:
stage_mode = 0
stage_step += 1
elif stage_mode == 2: # Done
pass
if index is not None:
force_advance = False
log.debug('Art stage mode %d: stone %s => new center: %s, new angle: %s', stage_mode, index, str(new_center), str(new_angle))
else:
force_advance = True
stage_mode = min(stage_mode + 1, MAX_STAGE_MODE)
log.debug('Art stage mode %d: None', stage_mode)
stage = stage_mode, stage_step, stage1_y, stage1_last.index if stage1_last else None # Do not store in map
return index, new_center, new_angle, stage, force_advance
