def square():
#18.6046511628
current = (0, 0, 0)
for i in range(0, 4):
for j in range(4):
forward(10)
previous_pos = current
particles = generate_particles_from_movement(current, 10)
avgX = sum([x for (x, y, theta) in particles]) / 100
avgY = sum([y for (x, y, theta) in particles]) / 100
avgTheta = sum([theta for (x, y, theta) in particles]) / 100
current = (avgX, avgY, avgTheta)
line = (previous_pos[0], previous_pos[1], avgX, avgY)
#plot the points
print("drawLine:" + str(line))
print("drawParticles:" + str(particles))
print("###########################WENT FORWARD#################")
time.sleep(0.2)
left(90)
print("##############TURNED LEFT######################")
particles = generate_particles_from_turn(current, 90)
#plot the new points
print("drawParticles:" + str(particles))
time.sleep(0.1)
def seek_line():
print("Seeking the line")
ret = False
seek_size = 0.35
seek_count = 1
max_seek_count = 5
direction = False
while seek_count <= max_seek_count:
seek_time = seek_size * seek_count
if direction:
print("Looking left")
move.left(speed)
else:
print("Looking right")
move.right(speed)
line_sensor.wait_for_line(seek_time)
if line_sensor.value < 0.5:
ret = True
break
else:
direction = not direction
if direction:
seek_count += 1
continue
return ret
def directions(instructions):
if instructions[0] == "Forward":
move.forward()
elif instructions[0] == "Left":
move.left()
elif instructions[0] == "Right":
move.right()
elif instructions[0] == "Standing":
move.stand()
elif instructions[0] == "Auto":
move.autonom()
def move_cube(input):
for i in range(0, len(input)):
if len(input[i]) == 2 and input[i][1] == '2':
perm = 2
elif len(input[i]) == 2 and input[i][1] == '\'':
perm = 3
else:
perm = 1
j = 0
while j < perm:
if input[i][0] == 'U':
move.upper()
elif input[i][0] == 'D':
move.down()
elif input[i][0] == 'L':
move.left()
elif input[i][0] == 'R':
move.right()
elif input[i][0] == 'F':
move.front()
elif input[i][0] == 'B':
move.back()
j = j + 1
if left_result and right_result:
left_touched = left_result[0]
right_touched = right_result[0]
if left_touched and right_touched:
print "front"
interface.setMotorPwm(motors[0],0)
interface.setMotorPwm(motors[1],0)
# while not interface.motorRotationSpeedReferenceReached(0):
# time.sleep(0.1)
time.sleep(1)
interface.motorDisable(0)
interface.motorDisable(3)
interface.motorEnable(0)
interface.motorEnable(3)
time.sleep(1)
print("stopped")
forward(-10)
time.sleep(0.5)
left(90)
time.sleep(0.5)
elif left_touched:
print "left"
elif right_touched:
print "right"
interface.terminate()
def keyPressed(mode, event):
if (event.key == "Left"):
if (mode.begin == False):
#this function math for rotating left (uses a rotation matrix)
move.left(mode)
else:
if (mode.turn > -10):
#this function math for rotating right (uses a rotation matrix)
move.right(mode)
mode.turn -= 1
if (event.key == "Right"):
if (mode.begin == False):
move.right(mode)
else:
if (mode.turn < 30):
move.left(mode)
mode.turn += 1
if (event.key == 'Up'):
if (mode.begin == False):
#checks if there is a block ahead, if not, walk forward
move.up(mode)
elif (mode.count < 23):
mode.count += 1
if (event.key == 'Down'):
if (mode.begin == False):
#checks if there is a block behind, if not, walk backward
move.down(mode)
elif (mode.count > 0):
mode.count -= 1
else:
pass
#when d is pressed, player turns around and enemy's defense is drawn
if (event.key == 'd' and mode.sold > 0 and not mode.begin):
mode.begin = True
mode.xDir = 1
mode.yDir = 0
mode.xPos = 2
mode.yPos = 12
mode.xCameraPlane = 0
for i in range(1, 22):
mode.map[24][i] = 0
#puts enemy soldiers
for i in range(10):
enemyRow = random.randint(25, 45)
enemyCol = random.randint(3, 21)
mode.enemySold.append((enemyRow, enemyCol))
mode.map[enemyRow][enemyCol] = 1
#puts enemy blocks around soldiers
defense = random.randint(1, 5)
for i in range(defense):
xRand = random.randint(-2, -1)
yRand = random.randint(-1, 1)
mode.map[enemyRow + -1][enemyCol + yRand] = 6
#increases power of shot
if (event.key == 'p'):
if (mode.begin):
mode.power += 10
#fires
if (event.key == 'f' and mode.begin):
mode.fire = True
if (event.key == 't' and mode.begin):
mode.top = True
if (event.key == 'r' and mode.begin and mode.top):
mode.top = False