def startReversing():
tankDrive(-70, -70)
def stopReversing():
fsm.runAction("backupCompleted")
Timer(1.5, stopReversing).start()
def startRotating():
tankDrive(-40, 40)
def stopRotating():
fsm.runAction("rotateCompleted")
Timer(random.uniform(0.3, 0.7), stopRotating).start()
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):
global started
if not started:
fsm.runAction("start")
started = True
if fsm.getState() == "aligning":
prox = getProximity()
if abs(prox[0] - prox[1]) < 3:
fsm.runAction("alignCompleted")
pass
else:
speed = int(0.5 * (prox[1] - prox[0]))
tankDrive(speed, -speed)
time.sleep(0.1)
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 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 forwardAtIntersection():
print("FWD")
vel = getVel()
tankDrive(vel, vel)
time.sleep(1.3)
stop()
return "completed"
else:
return "rotating"
fsm.addTransition("backupCompleted", "reversing", backupCompletedCb)
def alignCb():
stop()
beepSync(0.5)
fsm.setEnterCallback("aligning", alignCb)
fsm.setEnterCallback("free", lambda: tankDrive(45, 45))
def startRotating():
tankDrive(-40, 40)
def stopRotating():
fsm.runAction("rotateCompleted")
Timer(random.uniform(0.3, 0.7), stopRotating).start()
fsm.setEnterCallback("rotating", startRotating)
fsm.setEnterCallback("pushing", lambda: tankDrive(100, 100))
def turnRightCb():
tankDrive(50, -50)
def turnLeftCb():
tankDrive(-50, 50)
def freeCb():
tankDrive(50, 50)
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)
