def playEndMusic(): stop() music = [(60, 0.7), (20, 0.2), (40, 0.1)] * 5 for note in music: setMusicNote(note[0]) time.sleep(note[1]) setMusicNote(0)
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): 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 forwardAtIntersection(): print("FWD") vel = getVel() tankDrive(vel, vel) time.sleep(1.3) stop()
def alignCb(): stop() beepSync(0.5)
def stopBeep(): stop() beepSync(0.5)
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)