def determinePositionOnNextBlackLine(blackLine):
global position
global lookingDirection
color.driveToBlack(db, motorWheelLeft, motorWheelRight)
newPosition = blackLine[1]
print("Determining position on Black line")
if(newPosition.x == 0):
newPosition.x = getDistanceToWall()
lookingDirection = Vector(0, 1)
else:
newPosition.y = getDistanceToWall()
lookingDirection = Vector(1, 0)
print("Determined: " + str(newPosition))
print("Now looking towards: " + str(lookingDirection))
position = newPosition
def main():
'''arm.open()
arm.down()
arm.close()'''
arm.up()
nav.followCoordinatePath(half)
color.driveToBlack(nav.db, nav.motorWheelLeft, nav.motorWheelRight)
nav.followCoordinatePath(secoundHalf)
nav.turn(-50)
nav.driveForward(500, 100)
nav.turn(-50)
color.driveToBlack(nav.db, nav.motorWheelLeft, nav.motorWheelRight)
#nav.followCoordinatePath(toThePlant)
nav.driveForward(-50, 100)
nav.turn(-50)
nav.driveForward(200, 100)
arm.down()
arm.open()
arm.up()
arm.close()
arm.down()
arm.up()
nav.returnToStart(M07)
def driveToFireTruck():
arm.close()
arm.up()
nav.driveDistance(100, 300)
color.driveToBlack(nav.db, nav.motorWheelLeft, nav.motorWheelRight)
nav.turn(90)
nav.driveDistance(8, 300)
nav.turn(-90)
nav.driveDistance(30, 300)
color.driveToBlack(nav.db, nav.motorWheelLeft, nav.motorWheelRight)
nav.turn(-45)
nav.driveDistance(25, 300)
nav.turn(-80)
color.driveToBlack(nav.db, nav.motorWheelLeft, nav.motorWheelRight)
nav.driveDistance(-5, 300)
arm.open()
arm.down()
arm.close()
arm.up()
nav.driveDistance(-20, 300)
def driveToBlack():
color.driveToBlack(db, motorWheelLeft, motorWheelRight)
def main():
arm.up()
arm.close()
nav.followCoordinatePath(M18)
color.driveToBlack(nav.db, nav.motorWheelLeft, nav.motorWheelRight)