def move_cursor_on_screen(self, dx=0, dy=0, dz=0):
if self.rotation == 1:
dx, dy = -dy, dx
if self.rotation == 2:
dx, dy = -dx, -dy
if self.rotation == 3:
dx, dy = dy, -dx
v.position.x = constrain(v.position.x+dx, 0, lv.xsize-1)
v.position.y = constrain(v.position.y+dy, 0, lv.ysize-1)
v.position.z = constrain(v.position.z+dz, 0, lv.zsize-1)
return v.position
def move_cursor_on_screen(self, dx=0, dy=0, dz=0):
if self.rotation == 1:
dx, dy = -dy, dx
if self.rotation == 2:
dx, dy = -dx, -dy
if self.rotation == 3:
dx, dy = dy, -dx
v.position.x = constrain(v.position.x + dx, 0, lv.xsize - 1)
v.position.y = constrain(v.position.y + dy, 0, lv.ysize - 1)
v.position.z = constrain(v.position.z + dz, 0, lv.zsize - 1)
return v.position
def lookup(self, position: Vector2):
"""
Look up flow field Vector value at given position
:param position: Vector2 with x, y coords of considered place
:return: Vector at that position (copy)
"""
col = utils.constrain(int(position.x / self.resolution), self.cols - 1)
row = utils.constrain(int(position.y / self.resolution), self.rows - 1)
return self.field[row][col]
def update(self):
AMkeyPressed = False
self.events = self.eventHandler.get()
for event in self.events:
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_LEFT:
self.radarPosition = utils.constrain(
self.radarPosition - 1, 1, 8)
if event.key == pygame.K_RIGHT:
self.radarPosition = utils.constrain(
self.radarPosition + 1, 1, 8)
if event.key == pygame.K_SPACE:
self.radarOn = not self.radarOn
if event.key == pygame.K_1:
self.shieldOn[1] = not self.shieldOn[1]
if event.key == pygame.K_2:
self.shieldOn[2] = not self.shieldOn[2]
if event.key == pygame.K_3:
self.shieldOn[3] = not self.shieldOn[3]
if event.key == pygame.K_4:
self.shieldOn[4] = not self.shieldOn[4]
if event.key == pygame.K_5:
self.shieldOn[5] = not self.shieldOn[5]
if event.key == pygame.K_6:
self.shieldOn[6] = not self.shieldOn[6]
if event.key == pygame.K_7:
self.shieldOn[7] = not self.shieldOn[7]
if event.key == pygame.K_8:
self.shieldOn[8] = not self.shieldOn[8]
if event.key == pygame.K_e:
self.antiMissilePressed(True)
AMkeyPressed = True
if (event.type == pygame.KEYDOWN
and event.key == pygame.K_ESCAPE) or (event.type
== pygame.QUIT):
self.quit = True
if AMkeyPressed == False:
self.antiMissilePressed(False)
if self.controller:
controls = self.ser.readline().strip().split(" ")
self.ser.flushInput()
#print(controls)
self.radarPosition = int(controls[0])
if int(controls[1]): self.radarOn = not self.radarOn
for i in range(0, 8):
self.shieldOn[i + 1] = int(controls[i + 2])
self.antiMissilePressed(int(controls[10]))
def checkLimits(self, joint, angle_vect, angle_elem, collection):
"""
Check if the identify joint value is in the range imposed by the human
loded into the simulation env. If not, the joint value is adjusted.
Joint value is finally filtered.
@param joint joint
@param angle_vector vector of the joint values
@param angle_elem position of the joint value to be checked in angle_vector
"""
if angle_vect is not None:
angle = angle_vect[angle_elem]
if angle is not None and joint is not None:
lower,upper = joint.GetLimits()
angle = utils.constrain(angle, lower, upper)
collection.append(float(angle))
if len(collection) > FILTERING_SAMPLES:
filtered_angles = signal.filtfilt(self.B, self.A, list(collection))
filtered_angle = filtered_angles[len(collection)-1]
if filtered_angle < lower: filtered_angle = lower
if filtered_angle > upper: filtered_angle = upper
self.currentDOFvalues[joint.GetDOFIndex()] = filtered_angle
with self.env:
self.body.SetDOFValues(self.currentDOFvalues)
def start(board, steps=100, size=20):
for i in range(1, steps + 1):
os.system('clear')
print('step:', i, '/', steps)
print_board(board, size)
time.sleep(0.1)
board = constrain(advance(board), size)
def checkLimits(self, joint, angle_vect, angle_elem):
"""
Check if the identify joint value is in the range imposed by the human
loded into the simulation env. If not, the joint value is adjusted
@param joint joint
@param angle_vector vector of the joint values
@param angle_elem position of the joint value to be checked in angle_vector
"""
if angle_vect is not None:
angle = angle_vect[angle_elem]
if angle is not None and joint is not None:
lower,upper = joint.GetLimits()
angle = utils.constrain(angle, lower, upper)
self.currentDOFvalues[joint.GetDOFIndex()] = angle
with self.env:
self.body.SetDOFValues(self.currentDOFvalues)
def update(self):
"""
Standart Euler integration
Updates bahavior tree
"""
# updates behavior in machine state
self.behavior.update(self)
# Updates velocity at every step and limits it to max_speed
self.velocity += self.acceleration * 1
self.velocity = limit(self.velocity, self.max_speed)
# updates position
self.location += self.velocity
# Prevents it from crazy spinning due to very low noise speeds
if self.velocity.length() > 0.5:
self.rotation = atan2(self.velocity.y, self.velocity.x)
# Constrains position to limits of screen
self.location = constrain(self.location, SCREEN_WIDTH, SCREEN_HEIGHT)
self.acceleration *= 0
# Memory of positions to draw Track
self.memory_location.append((self.location.x, self.location.y))
# size of track
if len(self.memory_location) > SIZE_TRACK:
self.memory_location.pop(0)
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
from utils import lerp, sq, constrain
from math import sqrt
if __name__ == "__main__":
D = 0.9
SIZE = 16
data = []
pal = []
for i in range(8):
c = int(lerp(0, 255, float(i) / 7))
pal.extend([c, c, c])
im = Image.new('L', (SIZE, SIZE * 8))
im.putpalette(pal)
pix = im.load()
for size in range(8, 16):
r = lerp(2.0, 1.0, float(size - 8) / 7)
for i in range(SIZE):
for j in range(SIZE):
u = lerp(-r, r, float(i) / (SIZE - 1))
v = lerp(-r, r, float(j) / (SIZE - 1))
d = sq(1.4 - constrain(sqrt(sq(u) + sq(v)), 0.0, 1.4))
pix[i, j + (size - 8) * SIZE] = int(constrain(d, 0.0, 1.0) * 7)
im.save("plotter-flares.png", "PNG")
#!/usr/bin/env python2
from PIL import Image
from utils import constrain, lerp, dist
if __name__ == "__main__":
size = (128, 128)
data = []
for i in range(size[0]):
for j in range(size[1]):
x = lerp(-1.5, 1.5, float(i) / size[0])
y = lerp(-1.5, 1.5, float(j) / size[1])
pixel = 1.0 - dist(0.0, 0.0, x, y)
data.append(int(constrain(pixel, 0.0, 1.0) * 255))
light = Image.new('L', size)
light.putdata(data)
light.save("light.png", "PNG")
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
from utils import lerp, sq, constrain
from math import sqrt
if __name__ == "__main__":
D = 0.9
SIZE = 16
data = []
pal = []
for i in range(8):
c = int(lerp(0, 255, float(i) / 7))
pal.extend([c, c, c])
im = Image.new('L', (SIZE, SIZE * 8))
im.putpalette(pal)
pix = im.load()
for size in range(8, 16):
r = lerp(2.0, 1.0, float(size - 8) / 7)
for i in range(SIZE):
for j in range(SIZE):
u = lerp(-r, r, float(i) / (SIZE - 1))
v = lerp(-r, r, float(j) / (SIZE - 1))
d = sq(1.4 - constrain(sqrt(sq(u) + sq(v)), 0.0, 1.4))
pix[i, j + (size - 8) * SIZE] = int(constrain(d, 0.0, 1.0) * 7)
im.save("plotter-flares.png", "PNG")
if __name__ == "__main__":
image = Image.open(sys.argv[1])
image = image.convert("L")
bumpmap = array("H")
name = os.path.splitext(sys.argv[1])[0] + ".bin"
for y in range(image.size[1]):
for x in range(image.size[0]):
this = image.getpixel((x, y))
if y < image.size[1] - 1:
down = image.getpixel((x, y + 1))
else:
down = this
if x < image.size[0] - 1:
right = image.getpixel((x + 1, y))
else:
right = this
# scale down the difference between pixels
du = (this - down) * 0.25
dv = (this - right) * 0.25
# light texture size is 128 * 128
u = (constrain(int(du), -64, 63) + y) & 127
v = (constrain(int(dv), -64, 63) + x) & 127
bumpmap.append(u * 128 + v)
bumpmap.byteswap()
with open(name, "w") as f:
bumpmap.tofile(f)
if __name__ == "__main__":
image = Image.open(sys.argv[1])
image = image.convert("L")
bumpmap = array("H")
name = os.path.splitext(sys.argv[1])[0] + ".bin"
for y in range(image.size[1]):
for x in range(image.size[0]):
this = image.getpixel((x, y))
if y < image.size[1] - 1:
down = image.getpixel((x, y + 1))
else:
down = this
if x < image.size[0] - 1:
right = image.getpixel((x + 1, y))
else:
right = this
# scale down the difference between pixels
du = (this - down) * 0.25
dv = (this - right) * 0.25
# light texture size is 128 * 128
u = (constrain(int(du), -64, 63) + y) & 127
v = (constrain(int(dv), -64, 63) + x) & 127
bumpmap.append(u * 128 + v)
bumpmap.byteswap()
with open(name, "w") as f:
bumpmap.tofile(f)
for i in range(8):
pal.extend([0, 0, 0])
for i in range(16):
c = int(lerp(0, 255, float(i + 1) / 16))
pal.extend([c, c, c])
for i in range(4):
pal.extend([255, 255, 255])
for i in range(4):
c = int(lerp(255, 0, float(i + 1) / 4))
pal.extend([c, c, c])
for i in range(size[0]):
for j in range(size[1]):
x = lerp(-D, D, float(i) / size[0])
y = lerp(-D, D, float(j) / size[1])
d = dist(x, y, 0, 0)
if d < D:
p = constrain(int(sq(1.0 - d) * 128), 0, 31)
else:
p = 0
data.append(int(p))
im = Image.new('L', size)
im.putdata(data)
im.putpalette(pal)
im.save(sys.argv[1], "PNG")
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 cruise():
""" Tracks the black line.
Acquires images from front camera and uses it to do pure pursuit.
Uses functions in driver.py to drive the pi car.
There is a three-step process to reach the goal.
Step 1.
Employs CameraCapturer class to acquire images from front camera and
rectify lens distortion.
Step 2.
Chooses the ROI and binarizes the it. Then uses morphology method to
get the target point.
Step 3.
According to target point, applies pure pursuit algorithm and uses
functions in driver.py to drive the car.
Args:
None
Returns:
None
"""
# Initialize CameraCapturer and drive
cap = camera_capturer.CameraCapturer("front")
d = driver.driver()
d.setStatus(motor=0.4, servo=0, mode="speed")
last_time = time.time()
target = OFFSET # -int(cap.width / 5)
# Parameters of PID controller
kp = 1.2
ki = 0
kd = 0.1
# Initialize error to 0 for PID controller
error_p = 0
error_i = 0
error_d = 0
error = 0
servo = 0
last_servo = 0
last_angle = 0
n = 0.2
try:
while True:
this_time = time.time()
if this_time - last_time > PERIOD: # Execute the code below every
# PERIOD time
last_time = this_time
# d.setStatus(motor=0, servo=n, mode="speed")
# n = -n
# continue
# ----------------------------------------------------------- #
# Start your code here #
# Image processing. Outputs a target_point.
frame = cap.get_frame()
start = time.time()
skel, img_bin_rev = image_processing.image_process(frame)
white_rate = \
np.size(img_bin_rev[img_bin_rev == 255]) / img_bin_rev.size
if white_rate > 0.3:
print("stay", white_rate)
d.setStatus(servo=last_servo)
continue
target_point, width, _, img_DEBUG, angle = \
choose_target_point(skel, target)
end = time.time()
if angle == 0:
angle = last_angle
pass
else:
# Update the PID error
error_p = angle # **(9/7)
error_i += error_p
error_d = error_p - error
error = error_p
# PID controller
servo = utils.constrain(
-kp * error_p - ki * error_i - kd * error_d, 1, -1)
d.setStatus(servo=servo)
last_servo = servo
# print("servo: ", servo, "error_p: ", error_p)
# img_DEBUG[:, target] = 255
# if DEBUG:
# # cv.imshow("frame", frame)
# cv.imshow("img_bin_rev", img_bin_rev)
# cv.imshow("img_DEBUG", img_DEBUG)
# cv.waitKey(300)
# ----------------------------------------------------------- #
else:
# time.sleep(0.01)
pass
except KeyboardInterrupt:
d.setStatus(servo=0, motor=0)
def parking():
""" Tracks the black line.
Acquires images from front camera and uses it to do pure pursuit.
Uses functions in driver.py to drive the pi car.
There is a three-step process to reach the goal.
Step 1.
Employs CameraCapturer class to acquire images from front camera and
rectify lens distortion.
Step 2.
Chooses the ROI and binarizes the it. Then uses morphology method to
get the target point.
Step 3.
According to target point, applies pure pursuit algorithm and uses
functions in driver.py to drive the car.
Args:
None
Returns:
None
"""
# Initialize CameraCapturer and drive
# cap = camera_capturer.CameraCapturer("rear")
# d = driver.driver()
# d.setStatus(motor=0.2, servo=0, mode="speed")
# parking_state
# 0: initial state
# 1: on parking
# 2: waiting for another parking instruction
parking_state = 0
target_lot = 3
current_lot = 3
# cap = camera_capturer.CameraCapturer("rear")
# d = driver.driver()
# d.setStatus(motor=0.4, servo=0, mode="speed")
target_x = 0
target_y = 0
while True:
if parking_state == 0:
print("------------------------------------------------------")
print("Which lot would you like to park? :-)")
try:
target_lot = int(input())
if target_lot == -1:
break
elif target_lot < 0 or target_lot > 4:
raise lotOutOfRangeError(
"The lot number must be 1, 2, 3 or 4.")
except:
print("Invalid input! The lot number must be 1, 2, 3 or 4.")
continue
parking_state = 1
if parking_state == 1:
print("Start parking...")
print("Target lot: %d" % (target_lot))
# Start
# img = cap.get_frame()
img = get_frame()
print("New image catched!")
width = img.shape[1]
height = img.shape[0]
roi = img[int(height * 2 / 5):height, 0:int(width // 2), :]
lines, linePoints = line_detection.line_detection(roi)
result = line_detection.digit_detection(roi, lines, linePoints)
result = result[1:]
print("result", result)
if result[0] in [1, 2, 3, 4]:
current_lot = result[0]
delta = current_lot - target_lot
if delta == 0:
pass
else:
parking_control.gostraight(delta * LOT_LENGTH)
img = get_frame()
print("New image catched!")
width = img.shape[1]
height = img.shape[0]
roi = img[int(height * 2 / 5):height, 0:int(width // 2), :]
lines, linePoints = line_detection.line_detection(roi)
result = line_detection.digit_detection(
roi, lines, linePoints)
result = result[1:]
print("result", result)
else:
current_lot = 2
delta = current_lot - target_lot
if delta == 0:
pass
else:
parking_control.gostraight(delta * LOT_LENGTH)
img = get_frame()
print("New image catched!")
width = img.shape[1]
height = img.shape[0]
roi = img[int(height * 2 / 5):height, 0:int(width // 2), :]
lines, linePoints = line_detection.line_detection(roi)
result = line_detection.digit_detection(
roi, lines, linePoints)
result = result[1:]
print("result", result)
x, y = coordinate_transform(result)
print("x", x, "y", y)
x = utils.constrain(x, 40, 20)
y = utils.constrain(y, 82, 70)
print("x", x, "y", y)
target_x = x + LOT_WIDTH // 2 + 5
target_y = y - LOT_LENGTH // 2 + 5
print("target", target_x, target_y)
parking_control.park(target_x, target_y, 0)
parking_control.gostraight(-5)
current_lot = target_lot
print("Parking finished!")
parking_state = 2
if parking_state == 2:
print("------------------------------------------------------")
print("Which lot would you like to park? :-)")
try:
target_lot = int(input())
if target_lot == -1:
break
elif target_lot < 0 or target_lot > 4:
raise lotOutOfRangeError(
"The lot number must be 1, 2, 3 or 4.")
except:
print("Invalid input! The lot number must be 1, 2, 3 or 4.")
continue
if current_lot == target_lot:
print("Already in lot %d" % (current_lot))
else:
parking_control.gostraight(5)
parking_control.reverse(target_x, target_y)
# delta = current_lot - target_lot
# print("distance", delta*LOT_LENGTH)
# parking_control.gostraight(delta*LOT_LENGTH)
parking_state = 1
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
for i in range(8):
pal.extend([0, 0, 0])
for i in range(16):
c = int(lerp(0, 255, float(i + 1) / 16))
pal.extend([c, c, c])
for i in range(4):
pal.extend([255, 255, 255])
for i in range(4):
c = int(lerp(255, 0, float(i + 1) / 4))
pal.extend([c, c, c])
for i in range(size[0]):
for j in range(size[1]):
x = lerp(-D, D, float(i) / size[0])
y = lerp(-D, D, float(j) / size[1])
d = dist(x, y, 0, 0);
if d < D:
p = constrain(int(sq(1.0 - d) * 128), 0, 31)
else:
p = 0
data.append(int(p))
im = Image.new('L', size)
im.putdata(data)
im.putpalette(pal)
im.save("metaball.png", "PNG")
#!/usr/bin/env python3
from utils import constrain, lerp, dist
import sys
if __name__ == '__main__':
size = (128, 128)
print('const u_short light_w = 128;')
print('const u_short light_h = 128;')
print('')
print('u_char %s[] = {' % sys.argv[1])
for i in range(size[0]):
sys.stdout.write(' ')
for j in range(size[1]):
x = lerp(-1.5, 1.5, float(i) / size[0])
y = lerp(-1.5, 1.5, float(j) / size[1])
pixel = 1.0 - dist(0.0, 0.0, x, y)
sys.stdout.write(' 0x%02x,' %
int(constrain(pixel, 0.0, 1.0) * 255))
sys.stdout.write('\n')
print('};')
