setBottomCamState = createState("setBottomCamState",
lambda: setCamera("bottom"))
setBottomLedState = createState("setBottomLedState",
lambda: setLED("eyes", 1, 0, 0))
kickRightState = createState("kickRightState", lambda: kick("right"))
kickLeftState = createState("kickLeftState", lambda: kick("left"))
# FSM for following the ball
goToBallFSM = createFSM("goToBallFSM")
addStates(goToBallFSM, rotateLeftState, rotateRightState, walkToBallState,
watchBallState, stopWalkingState, stopWalkingState2,
setBottomLedState, setBottomCamState, kickRightState, kickLeftState,
archRightState, archLeftState)
setInitialState(goToBallFSM, watchBallState)
# Transitions
addTransition(watchBallState, farPositiveYaw, archLeftState)
addTransition(watchBallState, farNegativeYaw, archRightState)
addTransition(watchBallState, positiveYaw, rotateLeftState)
addTransition(watchBallState, negativeYaw, rotateRightState)
addTransition(watchBallState, zeroYaw, walkToBallState)
addTransition(rotateRightState, zeroYaw, walkToBallState)
addTransition(rotateRightState, lambda wm: True, watchBallState)
addTransition(archRightState, zeroYaw, walkToBallState)
addTransition(archRightState, lambda wm: True, watchBallState)
addTransition(rotateLeftState, zeroYaw, walkToBallState)
bottomLedState = createState("bottomLedState", lambda : setLED("eyes", 1, 0, 0)) # Red
topLedState = createState("topLedState", lambda : setLED("eyes", 0, 1, 0)) # Green
topLedState2 = createState("topLedState2", lambda : setLED("eyes", 0, 1, 0)) # Green
stopWalkState = createState("stopWalkState", stopWalking)
# FSM for searching for the ball
lookBallFSM = createFSM("lookBallFSM")
addStates(lookBallFSM, shakeHeadState, stopWalkState,
setTopCameraState, setTopCameraState2,
setBottomCameraState, bottomLedState,
topLedState, lookAtBallState, rotateState,
shakeHeadState2,topLedState2)
setInitialState(lookBallFSM, shakeHeadState)
# Transitions
addTransition(shakeHeadState, seeBall, lookAtBallState)
addTransition(shakeHeadState, shakeHeadTime, setBottomCameraState)
addTransition(setBottomCameraState, cameraDelay, bottomLedState)
addTransition(bottomLedState, lambda wm: True, shakeHeadState2)
addTransition(shakeHeadState2, seeBall, lookAtBallState)
addTransition(shakeHeadState2, shakeHeadTime, setTopCameraState)
addTransition(setTopCameraState, cameraDelay, topLedState)
addTransition(topLedState, seeBall, lookAtBallState)
addTransition(topLedState, noSeeBall, rotateState)
# Import functions we've written
from functions import (leftFoot, rightFoot)
#create states
kickRightState = createState("kickRightState", lambda: kick("right"))
kickLeftState = createState("kickLeftState", lambda: kick("left"))
stopWalkingState = createState("stopWalkingState", stopWalking)
stopWalkingState2 = createState("stopWalkingState2", stopWalking)
# FSM for kicking the ball
kickBallFSM = createFSM("kickBallFSM")
addStates(kickBallFSM, kickRightState, kickLeftState, stopWalkingState,
stopWalkingState2)
setInitialState(kickBallFSM, stopWalkingState)
# Transitions
addTransition(stopWalkingState, leftFoot, kickLeftState)
addTransition(stopWalkingState, rightFoot, kickRightState)
addTransition(kickLeftState, lambda wm: True, stopWalkingState2)
addTransition(kickRightState, lambda wm: True, stopWalkingState2)
# Prints out completed transitions
setPrintTransition(kickBallFSM, True)
lambda: setLED("eyes", 1, 0, 0)) # Red
topLedState = createState("topLedState",
lambda: setLED("eyes", 0, 1, 0)) # Green
topLedState2 = createState("topLedState2",
lambda: setLED("eyes", 0, 1, 0)) # Green
stopWalkState = createState("stopWalkState", stopWalking)
# FSM for searching for the ball
lookBallFSM = createFSM("lookBallFSM")
addStates(lookBallFSM, shakeHeadState, stopWalkState, setTopCameraState,
setTopCameraState2, setBottomCameraState, bottomLedState,
topLedState, lookAtBallState, rotateState, shakeHeadState2,
topLedState2)
setInitialState(lookBallFSM, shakeHeadState)
# Transitions
addTransition(shakeHeadState, seeBall, lookAtBallState)
addTransition(shakeHeadState, shakeHeadTime, setBottomCameraState)
addTransition(setBottomCameraState, cameraDelay, bottomLedState)
addTransition(bottomLedState, lambda wm: True, shakeHeadState2)
addTransition(shakeHeadState2, seeBall, lookAtBallState)
addTransition(shakeHeadState2, shakeHeadTime, setTopCameraState)
addTransition(setTopCameraState, cameraDelay, topLedState)
addTransition(topLedState, seeBall, lookAtBallState)
addTransition(topLedState, noSeeBall, rotateState)
addTransition, addStates,
setInitialState, readWM, setPrintTransition)
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown, communicate, stopWalking,
resetSubFSM, setCamera, setLED)
# Import functions we've written
from functions import (detectTouch, touchDelay, waitForSit, seeBall, noSeeBall, closeToFeet,
seeNao, oneKick, cameraDelay)
waitSittingState = createState("waitSittingState", lambda : None)
standState = createState("standState", stand)
sitState = createState("sitState", sit)
restState = createState("restState", rest)
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, seeNao, sitState)
addTransition(standState, touchDelay, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
testFSM = createFSM("testFSM")
addStates(testFSM, standState, sitState, restState, shutdownState, waitSittingState)
setInitialState(testFSM, waitSittingState)
setPrintTransition(testFSM, True)
stopWalkingState = createState("stopWalkingState", stopWalking)
stopRotateState = createState("stopRotateState", stopWalking)
# Transitions for the findGoalFSM
addTransition(rotateLeftState, seeGoal, stopWalkingState)
addTransition(rotateLeftState, seeGoalLeft, rotateRightState)
addTransition(rotateLeftState, seeGoalSingle, stopRotateState)
addTransition(rotateRightState, seeGoal, stopWalkingState)
addTransition(rotateRightState, seeGoalRight, rotateLeftState)
addTransition(rotateRightState, seeGoalSingle, stopRotateState)
addTransition(stopRotateState, lambda wm: True, lookUpState)
addTransition(lookUpState, seeGoal, stopWalkingState)
addTransition(lookUpState, seeGoalRight, rotateLeftState)
addTransition(lookUpState, seeGoalLeft, rotateRightState)
addTransition(stopWalkingState, lambda wm: True, lookDownState)
findGoalFSM = createFSM("findGoalFSM")
addStates(findGoalFSM, stopWalkingState, lookUpState, lookDownState,
rotateLeftState, rotateRightState, stopRotateState)
setInitialState(findGoalFSM, rotateLeftState)
# Prints all the completed transitions
setPrintTransition(findGoalFSM, True)
stopWalkingState = createState("stopWalkingState", stopWalking)
stopWalkingState2 = createState("stopWalkingState2", stopWalking)
setBottomCamState = createState("setBottomCamState", lambda: setCamera("bottom"))
setBottomLedState = createState("setBottomLedState", lambda: setLED("eyes", 1,0,0))
kickRightState = createState("kickRightState", lambda: kick("right"))
kickLeftState = createState("kickLeftState", lambda: kick("left"))
# FSM for following the ball
kickBallFSM = createFSM("kickBallFSM")
addStates(kickBallFSM, rotateLeftState, rotateRightState, walkToBallState,
watchBallState, stopWalkingState, stopWalkingState2, setBottomLedState,
setBottomCamState, kickRightState, kickLeftState, archRightState, archLeftState)
setInitialState(kickBallFSM, watchBallState)
# Transitions
addTransition(watchBallState, farPositiveYaw, archLeftState)
addTransition(watchBallState, farNegativeYaw, archRightState)
addTransition(watchBallState, positiveYaw, rotateLeftState)
addTransition(watchBallState, negativeYaw, rotateRightState)
addTransition(watchBallState, zeroYaw, walkToBallState)
addTransition(rotateRightState, zeroYaw, walkToBallState)
addTransition(rotateRightState, lambda wm: True, watchBallState)
addTransition(archRightState, zeroYaw, walkToBallState)
addTransition(archRightState, lambda wm: True, watchBallState)
addTransition(rotateLeftState, zeroYaw, walkToBallState)
addTransition(rightStepState, oneStep, lookStraightState)
addTransition(lookStraightState, lambda wm: True, setTopCameraState)
addTransition(setTopCameraState, lambda wm: True, topLedState)
addTransition(topLedState, lambda wm: True, circleLeftState)
addTransition(circleLeftState, seeGoal, stopWalkingState)
addTransition(circleLeftState, seeGoalLeft, circleRightState)
addTransition(circleLeftState, seeGoalSingle, stopRotateState)
addTransition(circleRightState, seeGoal, stopWalkingState)
addTransition(circleRightState, seeGoalRight, circleLeftState)
addTransition(circleRightState, seeGoalSingle, stopRotateState)
addTransition(stopRotateState, lambda wm: True, lookUpState)
addTransition(lookUpState, seeGoal, stopWalkingState)
addTransition(lookUpState, seeGoalRight, circleLeftState)
addTransition(lookUpState, seeGoalLeft, circleRightState)
findGoalFSM = createFSM("findGoalFSM")
addStates(findGoalFSM, stopWalkingState, lookUpState, circleLeftState,
circleRightState, stopRotateState, topLedState, setTopCameraState,
lookStraightState, leftStepState, rightStepState, standState)
setInitialState(findGoalFSM, standState)
# Prints all the completed transitions
setPrintTransition(findGoalFSM, True)
setBottomCamState = createState("setBottomCamState",
lambda: setCamera("bottom"))
setBottomLedState = createState("setBottomLedState",
lambda: setLED("eyes", 1, 0, 0))
kickRightState = createState("kickRightState", lambda: kick("right"))
kickLeftState = createState("kickLeftState", lambda: kick("left"))
# FSM for following the ball
kickBallFSM = createFSM("kickBallFSM")
addStates(kickBallFSM, rotateLeftState, rotateRightState, walkToBallState,
watchBallState, stopWalkingState, stopWalkingState2,
setBottomLedState, setBottomCamState, kickRightState, kickLeftState,
archRightState, archLeftState)
setInitialState(kickBallFSM, watchBallState)
# Transitions
addTransition(watchBallState, farPositiveYaw, archLeftState)
addTransition(watchBallState, farNegativeYaw, archRightState)
addTransition(watchBallState, positiveYaw, rotateLeftState)
addTransition(watchBallState, negativeYaw, rotateRightState)
addTransition(watchBallState, zeroYaw, walkToBallState)
addTransition(rotateRightState, zeroYaw, walkToBallState)
addTransition(rotateRightState, lambda wm: True, watchBallState)
addTransition(archRightState, zeroYaw, walkToBallState)
addTransition(archRightState, lambda wm: True, watchBallState)
addTransition(rotateLeftState, zeroYaw, walkToBallState)
from fsm.functions import (
createState, createFSM,
addTransition, addStates,
setInitialState)
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown)
# Create states
sitState = createState("sitState", sit)
standState = createState("standState", stand)
restState = createState("restState", rest)
shutdownState = createState("shutdownState",
lambda : shutdown("Final state reached"))
# Create a state which commands the robot to say "Hello World!"
sayState = createState("sayState", lambda: say("Hello World"))
# Add transitions between states
addTransition(standState, lambda wm: True, sayState)
addTransition(sayState, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
# Create the FSM and add the states created above
myFSM = createFSM("fsm")
addStates(myFSM, standState, sayState, sitState, restState, shutdownState)
# Set the initial state to standState
setInitialState(myFSM, standState)
watchBallState = createState("watchBallState", lambda wm: lookAtBall(wm))
stopWalkingState = createState("stopWalkingState", stopWalking)
stopWalkingState2 = createState("stopWalkingState2", stopWalking)
setBottomCamState = createState("setBottomCamState",
lambda: setCamera("bottom"))
setBottomLedState = createState("setBottomLedState",
lambda: setLED("eyes", 1, 0, 0))
# FSM for following the ball
followBallFSM = createFSM("followBallFSM")
addStates(followBallFSM, rotateLeftState, rotateRightState, walkToBallState,
watchBallState, stopWalkingState, stopWalkingState2,
setBottomLedState, setBottomCamState)
setInitialState(followBallFSM, watchBallState)
# Transitions
addTransition(watchBallState, positiveYaw, rotateLeftState)
addTransition(watchBallState, negativeYaw, rotateRightState)
addTransition(watchBallState, zeroYaw, walkToBallState)
addTransition(rotateRightState, zeroYaw, walkToBallState)
addTransition(rotateRightState, lambda wm: True, watchBallState)
addTransition(rotateLeftState, zeroYaw, walkToBallState)
addTransition(rotateLeftState, lambda wm: True, watchBallState)
addTransition(walkToBallState, closeToFeet, stopWalkingState)
addTransition(walkToBallState, switchCamera, setBottomCamState)
rightStepState = createState("rightStepState", lambda: setWalkVelocity(0, -0.5, 0))
# Transitions for the findGoalFSM
addTransition(standState, leftFoot, leftStepState)
addTransition(standState, rightFoot, rightStepState)
addTransition(standState, betweenFeet, lookStraightState)
addTransition(leftStepState, oneStep, lookStraightState)
addTransition(rightStepState, oneStep, lookStraightState)
addTransition(lookStraightState, lambda wm: True, setTopCameraState)
addTransition(setTopCameraState, lambda wm: True, topLedState)
addTransition(topLedState, lambda wm: True, circleLeftState)
addTransition(circleLeftState, seeNao, stopWalkingState)
findNaoFSM = createFSM("findNaoFSM")
addStates(findNaoFSM, stopWalkingState, lookUpState,
circleLeftState, circleRightState, stopRotateState, topLedState,
setTopCameraState, lookStraightState, leftStepState, rightStepState,
standState)
setInitialState(findNaoFSM, standState)
# Prints all the completed transitions
setPrintTransition(findNaoFSM, True)
stopWalkingState2 = createState("stopWalkingState2", stopWalking)
setBottomCamState = createState("setBottomCamState", lambda: setCamera("bottom"))
setBottomLedState = createState("setBottomLedState", lambda: setLED("eyes", 1,0,0))
kickRightState = createState("kickRightState", lambda: kick("right"))
kickLeftState = createState("kickLeftState", lambda: kick("left"))
# FSM for following the ball
goToBallFSM = createFSM("goToBallFSM")
addStates(goToBallFSM, rotateLeftState, rotateRightState, walkToBallState,
watchBallState, stopWalkingState, stopWalkingState2, setBottomLedState,
setBottomCamState, kickRightState, kickLeftState, archRightState,
archLeftState)
setInitialState(goToBallFSM, watchBallState)
# Transitions
addTransition(watchBallState, farPositiveYaw, archLeftState)
addTransition(watchBallState, farNegativeYaw, archRightState)
addTransition(watchBallState, positiveYaw, rotateLeftState)
addTransition(watchBallState, negativeYaw, rotateRightState)
addTransition(watchBallState, zeroYaw, walkToBallState)
addTransition(rotateRightState, zeroYaw, walkToBallState)
addTransition(rotateRightState, lambda wm: True, watchBallState)
addTransition(archRightState, zeroYaw, walkToBallState)
addTransition(archRightState, lambda wm: True, watchBallState)
addTransition(rotateLeftState, zeroYaw, walkToBallState)
setInitialState, readWM, setPrintTransition)
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown, communicate,
stopWalking, resetSubFSM, setCamera, setLED)
# Import functions we've written
from functions import (detectTouch, touchDelay, waitForSit, seeBall, noSeeBall,
closeToFeet, seeNao, oneKick, cameraDelay)
waitSittingState = createState("waitSittingState", lambda: None)
standState = createState("standState", stand)
sitState = createState("sitState", sit)
restState = createState("restState", rest)
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, seeNao, sitState)
addTransition(standState, touchDelay, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
testFSM = createFSM("testFSM")
addStates(testFSM, standState, sitState, restState, shutdownState,
waitSittingState)
setInitialState(testFSM, waitSittingState)
setPrintTransition(testFSM, True)
walkToBallState = createState("walkToBallState", lambda : setWalkVelocity(1, 0, 0))
watchBallState = createState("watchBallState", lambda wm : lookAtBall(wm))
stopWalkingState = createState("stopWalkingState", stopWalking)
stopWalkingState2 = createState("stopWalkingState2", stopWalking)
setBottomCamState = createState("setBottomCamState", lambda : setCamera("bottom"))
setBottomLedState = createState("setBottomLedState", lambda : setLED("eyes", 1,0,0))
# FSM for following the ball
followBallFSM = createFSM("followBallFSM")
addStates(followBallFSM, rotateLeftState, rotateRightState, walkToBallState,
watchBallState, stopWalkingState, stopWalkingState2, setBottomLedState,
setBottomCamState)
setInitialState(followBallFSM, watchBallState)
# Transitions
addTransition(watchBallState, positiveYaw, rotateLeftState)
addTransition(watchBallState, negativeYaw, rotateRightState)
addTransition(watchBallState, zeroYaw, walkToBallState)
addTransition(rotateRightState, zeroYaw, walkToBallState)
addTransition(rotateRightState, lambda wm: True, watchBallState)
addTransition(rotateLeftState, zeroYaw, walkToBallState)
addTransition(rotateLeftState, lambda wm: True, watchBallState)
addTransition(walkToBallState, closeToFeet, stopWalkingState)
addTransition(walkToBallState, switchCamera, setBottomCamState)
# Import functions we've written
from functions import (leftFoot, rightFoot)
#create states
kickRightState = createState("kickRightState", lambda: kick("right"))
kickLeftState = createState("kickLeftState", lambda: kick("left"))
stopWalkingState = createState("stopWalkingState", stopWalking)
stopWalkingState2 = createState("stopWalkingState2", stopWalking)
# FSM for kicking the ball
kickBallFSM = createFSM("kickBallFSM")
addStates(kickBallFSM, kickRightState, kickLeftState, stopWalkingState, stopWalkingState2)
setInitialState(kickBallFSM, stopWalkingState)
# Transitions
addTransition(stopWalkingState, leftFoot, kickLeftState)
addTransition(stopWalkingState, rightFoot, kickRightState)
addTransition(kickLeftState, lambda wm: True, stopWalkingState2)
addTransition(kickRightState, lambda wm: True, stopWalkingState2)
# Prints out completed transitions
setPrintTransition(kickBallFSM, True)
addTransition(walkState, closeToObstacle, rotateState)
addTransition(walkState, headDelay, rightHeadState)
addTransition(rightHeadState, closeToObstacle, rotateState)
addTransition(rightHeadState, headDelay, lookDownState)
addTransition(lookDownState, closeToObstacle, rotateState)
addTransition(lookDownState, headDelay, leftHeadState)
addTransition(leftHeadState, closeToObstacle, rotateState)
addTransition(leftHeadState, headDelay, lookDownState2)
addTransition(rotateState, rotateTime, stopWalkState)
addTransition(stopWalkState, closeToObstacle, rotateState)
addTransition(stopWalkState, lambda wm: True, lookDownMoreState)
addTransition(lookDownMoreState, closeToObstacle, rotateState)
addTransition(lookDownMoreState, headDelay, lookDownState2)
# Create the FSM and add the states created above
walkObstacleFSM = createFSM("walkObstacleFSM")
addStates(walkObstacleFSM, setBottomCamState, setBottomLedState,
walkState, rotateState, lookDownState, lookDownState2,
stopWalkState, leftHeadState, rightHeadState, lookDownMoreState)
# Set the initial state to waitSittingState
setInitialState(walkObstacleFSM , setBottomCamState)
setPrintTransition(walkObstacleFSM, True)
addTransition(goToBallFSM, closeToFeet, stopWalkState3)
addTransition(stopWalkState3, waitForKick, lookDownState)
addTransition(lookDownState, seeBall, kickBallFSM)
addTransition(lookDownState, noSeeBall, stopWalkState)
addTransition(kickBallFSM, oneKick, stopWalkState4)
addTransition(stopWalkState4, waitForSit, sitState)
addTransition(goToBallFSM, detectTouch, stopWalkState2)
addTransition(lookBallFSM, detectTouch, stopWalkState2)
addTransition(kickBallFSM, detectTouch, stopWalkState2)
addTransition(stopWalkState2, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
secondFSM = createFSM("secondFSM")
addStates(secondFSM, waitSittingState, standState, sitState, goToBallFSM,
restState, shutdownState, lookBallFSM, kickBallFSM, resetgoToBallState,
stopWalkState, stopWalkState2, stopWalkState3, stopWalkState4,
resetKickBallState, resetLookBallState, setBottomCameraState, bottomLedState,
lookDownState, resetFindGoalState)
setInitialState(secondFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(secondFSM, True)
lambda : shutdown("Final state reached"))
sayBallState = createState("sayBallState", lambda: say("ball!"))
sayNoBallState = createState("sayNoBallState", lambda: say("no ball found!"))
lookAtBallState = createState("lookAtBallState", lambda wm: lookAtBall(wm))
# Add transitions
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, hangHeadState)
addTransition(hangHeadState, lambda wm: True, waitStandingState)
addTransition(waitStandingState, seeBall, sayBallState)
addTransition(waitStandingState, time, sayNoBallState)
addTransition(sayBallState, lambda wm: True, lookAtBallState)
addTransition(lookAtBallState, noSeeBall, sayNoBallState)
addTransition(sayNoBallState, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
# Create the FSM and add the states created above
myFSM = createFSM("fsm")
addStates(myFSM, waitSittingState, standState,
sitState, restState, shutdownState,
hangHeadState, waitStandingState, sayBallState,
sayNoBallState, lookAtBallState)
setPrintTransition(myFSM, True)
# Set the initial state to waitSittingState
setInitialState(myFSM, waitSittingState)
stopWalkingState = createState("stopWalkingState", stopWalking)
stopRotateState = createState("stopRotateState", stopWalking)
# Transitions for the findGoalFSM
addTransition(rotateLeftState, seeGoal, stopWalkingState)
addTransition(rotateLeftState, seeGoalLeft, rotateRightState)
addTransition(rotateLeftState, seeGoalSingle, stopRotateState)
addTransition(rotateRightState, seeGoal, stopWalkingState)
addTransition(rotateRightState, seeGoalRight, rotateLeftState)
addTransition(rotateRightState, seeGoalSingle, stopRotateState)
addTransition(stopRotateState, lambda wm: True, lookUpState)
addTransition(lookUpState, seeGoal, stopWalkingState)
addTransition(lookUpState, seeGoalRight, rotateLeftState)
addTransition(lookUpState, seeGoalLeft, rotateRightState)
addTransition(stopWalkingState, lambda wm: True, lookDownState)
findGoalFSM = createFSM("findGoalFSM")
addStates(findGoalFSM, stopWalkingState, lookUpState, lookDownState,
rotateLeftState, rotateRightState, stopRotateState)
setInitialState(findGoalFSM, rotateLeftState)
# Prints all the completed transitions
setPrintTransition(findGoalFSM, True)
