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)