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 navigateToWaypoint(X, Y, current): #X is desired X,Y is desired Y
#assuming we have access to our x,y,theta values (position and direction of robot)
#take dY = Y-y;dX = X-x
#we need to turn (phi - theta) degrees with phi = atan2(dY,dX).
#then move forward a distance of sqrt(pow(dY,2)+pow(dX,2))
dY = Y - current[1]
dX = X - current[0]
print(dY)
print(dX)
phi = math.atan2(dY, dX)
dist = math.sqrt(math.pow(dY, 2) + math.pow(dX, 2))
if dX > 0:
angle = phi - current[2] #align with point if dX +ve
else:
angle = phi - (current[2] - math.pi) #offset by pi if dX -ve
print(angle)
print(dist)
right(angle)
forward(dist) #idk if this is how it works in python
current[0] = current[0] + dX
current[1] = current[1] + dY
current[3] = current[3] + phi
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
#!/usr/bin/env python3
import move
import signal
from gpiozero import DistanceSensor
min_distance = 0.15
sonic_sensor = DistanceSensor(
echo=18, trigger=17, threshold_distance=min_distance)
speed = 0.5
def exit_gracefully():
exit()
sonic_sensor.when_in_range = lambda: move.stop()
sonic_sensor.when_out_of_range = lambda: move.forward()
signal.signal(signal.SIGINT, exit_gracefully)
move.forward(speed)
while True:
sonic_sensor.wait_for_in_range()
move.backward(speed)
sonic_sensor.wait_for_out_of_range()
move.right(speed, 0.75)
exit_gracefully()
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