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