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):
for i in range(0, 200):
print(i)
tankDrive(int(i/4.0), int(i/4.0))
time.sleep(1.0 / 200.0)
tankDrive(50, 50)
time.sleep(3.0)
tankDrive(50,50)
for r in range(0, 200):
i = 200 - r
print(i)
tankDrive(int(i/4.0), int(i/4.0))
time.sleep(1.0 / 200.0)
end()
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):
for i in sol:
if i == 2:
robot.set_wheel(1, 15)
robot.set_wheel(0, 15)
time.sleep(3.5)
robot.set_wheel(1, 0)
robot.set_wheel(0, 0)
if i == 0:
robot.set_wheel(1, -25)
robot.set_wheel(0, 25)
time.sleep(1.02)
robot.set_wheel(0, 0)
robot.set_wheel(1, 0)
if i == 1:
robot.set_wheel(1, 25)
robot.set_wheel(0, -25)
time.sleep(1.02)
robot.set_wheel(0, 0)
robot.set_wheel(1, 0)
end()
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 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)