def playEndMusic():
stop()
music = [(60, 0.7), (20, 0.2), (40, 0.1)] * 5
for note in music:
setMusicNote(note[0])
time.sleep(note[1])
setMusicNote(0)
def turnRight():
print("RIGHT")
vel = getVel()
tankDrive(vel, vel)
time.sleep(1.3)
tankDrive(vel, -vel)
time.sleep(2.2)
stop()
def turnLeft():
print("LEFT")
vel = getVel()
tankDrive(vel, vel)
time.sleep(1.3)
tankDrive(-vel, vel)
time.sleep(2.2)
stop()
def turnLeftReverse():
print("LEFT")
vel = getVel()
tankDrive(vel, -vel)
time.sleep(2.2)
tankDrive(-vel, -vel)
time.sleep(1.3)
stop()
def periodicFunc(robot):
global cmds, currentPoint
l1 = robot.get_floor(0)
l2 = robot.get_floor(1)
if l1 + l2 < 80:
currentPoint = currentPoint + 1
stop()
g.visualizeGraphWithPath(pathNodes,
showCost=True,
currentPos=path[currentPoint])
viewGraph()
time.sleep(0.1)
robot.set_musical_note(40)
time.sleep(0.5)
robot.set_musical_note(0)
time.sleep(0.1)
cmd = cmds[0]
cmds = cmds[1:]
if cmd == 0:
turnLeft()
elif cmd == 1:
turnRight()
elif cmd == 2:
forwardAtIntersection()
stop()
if cmd == 3:
end()
return
if len(cmds) == 0:
end()
return
g.visualizeGraphWithPath(
pathNodes,
showCost=True,
currentEdge=[path[currentPoint], path[currentPoint + 1]])
viewGraph()
else:
error = l1 - l2
speed = 15
kP = 0.3
tankDrive(int(speed + error * kP), int(speed - error * kP))
time.sleep(0.05)
def periodicFunc(robot):
command = -1
try:
command = queue.get(block=True)
except:
pass
if command > 10:
x = command / 5
tankDrive(-(5 + x), 5 + x)
elif command == EVENT_BORDER:
stop()
beepSync(0.5)
forward(40, 2.0)
playEndMusic()
endAll = True
print("FINISHED")
end()
else:
tankDrive(30, 30)
time.sleep(0.1)
def periodicFunc(robot):
global cmds
l1 = robot.get_floor(0)
l2 = robot.get_floor(1)
if l1 + l2 < 80:
stop()
time.sleep(0.1)
robot.set_musical_note(40)
time.sleep(0.5)
robot.set_musical_note(0)
time.sleep(0.1)
cmd = cmds[0]
cmds = cmds[1:]
if cmd == 0:
turnLeft()
elif cmd == 1:
turnRight()
elif cmd == 2:
forwardAtIntersection()
stop()
if cmd == 3:
end()
if len(cmds) == 0:
end()
else:
error = l1 - l2
speed = 15
kP = 0.3
tankDrive(int(speed + error * kP), int(speed - error * kP))
time.sleep(0.05)
def forwardAtIntersection():
print("FWD")
vel = getVel()
tankDrive(vel, vel)
time.sleep(1.3)
stop()
def alignCb():
stop()
beepSync(0.5)
def stopBeep():
stop()
beepSync(0.5)
def exitCb():
stop()
beepSync(0.5)
forward(50, 1.5)
beepSync(0.5)
end()
def periodicFunc(robot):
global cmds, currentPoint, currentHeading, mapGrid
l1 = robot.get_floor(0)
l2 = robot.get_floor(1)
if l1 + l2 < 80:
currentPoint = currentPoint + 1
stop()
g.visualizeGraphWithPath(pathNodes,
showCost=True,
currentPos=path[currentPoint])
viewGraph()
time.sleep(0.1)
robot.set_musical_note(40)
time.sleep(0.5)
robot.set_musical_note(0)
time.sleep(0.1)
cmd = cmds[0]
cmds = cmds[1:]
oldHeading = currentHeading
currentHeading = getNewHeading(currentHeading, cmd)
if cmd == 0:
turnLeft()
if getProximity()[0] + getProximity()[1] > 40:
# get coord nearby
nextCoord = path[currentPoint + 1]
if mapGrid[nextCoord] == 0:
mapGrid[nextCoord] = 1
solvePath(mapGrid, path[currentPoint])
g.visualizeGraphWithPath(pathNodes,
showCost=True,
currentPos=path[currentPoint])
viewGraph()
turnLeftReverse()
currentHeading = oldHeading
continue
elif cmd == 1:
turnRight()
elif cmd == 2:
forwardAtIntersection()
stop()
if cmd == 3:
end()
return
if len(cmds) == 0:
end()
return
g.visualizeGraphWithPath(
pathNodes,
showCost=True,
currentEdge=[path[currentPoint], path[currentPoint + 1]])
viewGraph()
else:
error = l1 - l2
speed = 15
kP = 0.3
tankDrive(int(speed + error * kP), int(speed - error * kP))
time.sleep(0.05)
