def updateSensors():
global queue
global uiQueue
global endAll
while not endAll:
prox = getProximity()
floor = getFloor()
try:
uiQueue.put([prox[0], prox[1], floor[0], floor[1]], block=True)
if (floor[0] < FLOOR_THRESHOLD or floor[1] < FLOOR_THRESHOLD
) and floor[0] != 0 and floor[1] != 0:
queue.put(EVENT_BORDER, block=True)
print("EVENT_BORDER")
print(floor[0], floor[1])
elif (prox[0] + prox[1]) / 2 > PROX_THRESHOLD:
queue.put((prox[0] + prox[1]) / 2, block=False)
print("EVENT_OBSTACLE")
else:
queue.put(EVENT_NONE, block=False)
print("EVENT_NONE")
except:
pass
time.sleep(0.03)
def updateSensors():
global queue
global uiQueue
global endAll
while not endAll:
prox = getProximity()
floor = getFloor()
try:
uiQueue.put([prox[0], prox[1], floor[0], floor[1]], block=False)
except:
pass
if (floor[0] < FLOOR_THRESHOLD or floor[1] < FLOOR_THRESHOLD
) and floor[0] != 0 and floor[1] != 0:
fsm.runAction("line")
elif (prox[0] + prox[1]) / 2 > PROX_THRESHOLD:
if prox[0] > prox[1]:
fsm.runAction("obstacleRight")
else:
fsm.runAction("obstacleLeft")
else:
fsm.runAction("noObstacle")
time.sleep(0.03)
def updateSensors():
global endAll
while not endAll:
prox = getProximity()
floor = getFloor()
if (floor[0] < FLOOR_THRESHOLD or floor[1] < FLOOR_THRESHOLD
) and floor[0] != 0 and floor[1] != 0:
fsm.runAction("seeLine")
elif (prox[0] + prox[1]) / 2 > PROX_THRESHOLD:
fsm.runAction("seeTrash")
time.sleep(0.03)
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):
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)