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)