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