def turnLeft(): Angle(91, 1, 1)
def turnRight(): # if there's a wall in front, will need to implement check left or right to determine to turn left or right after encountering an obstacle Angle(91, 2, 1)
# Main logic try: while True: checkMagnet() if(grovepi.ultrasonicRead(middleUltra) < 20): if (grovepi.ultrasonicRead(leftUltra) < 25 and grovepi.ultrasonicRead(rightUltra) < 25): backUp(1) elif (grovepi.ultrasonicRead(leftUltra) > 20): turnLeft() elif (grovepi.ultrasonicRead(rightUltra) > 20): turnRight() elif(grovepi.ultrasonicRead(leftUltra) < 6): if(grovepi.ultrasonicRead(leftUltra) < 3): Angle(8, 1, -1) else: Angle(12, 2, 1) elif(grovepi.ultrasonicRead(rightUltra) < 6): if(grovepi.ultrasonicRead(rightUltra) < 3): Angle(8, 2, -1) else: Angle(12, 1, 1) if((grovepi.analogRead(sensor1) > 40 or grovepi.analogRead(sensor2) > 40)): backUp(2) if(totalmag > 100): backUp(3) else: BP.set_motor_power(BP.PORT_A + BP.PORT_D, -30)
def turnRight(orientation): Angle(91, 2, 1) orientation[0] -= 1 if (orientation[0] == 0): # resets to right position orientation[0] = 4
def turnLeft(orientation): Angle(91, 1, 1) orientation[0] += 1 if (orientation[0] == 5): # resets orientation to forward orientation[0] = 1
def turnRight(orientation): Angle(91, 2, 1) orientation -= 1 if (orientation == 0): # resets to right position orientation = 4 return orientation
def turnLeft(orientation): Angle(91, 1, 1) orientation += 1 if (orientation == 5): # resets orientation to forward orientation = 1 return orientation