setWalkVelocity, stopWalking)
# 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)
# Create states for waiting for touch
waitSittingState = createState("waitSittingState", lambda: None)
waitStandingState = createState("waitStandingState", lambda: None)
# Create states for function calls
# Add transitions according to the state diagram
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, sayLetsWalkState)
addTransition(sayLetsWalkState, lambda wm: True, walkState)
addTransition(walkState, walkTime, stopWalkState)
addTransition(stopWalkState, lambda wm: True, rotateState)
addTransition(rotateState, rotateTime, uTurnState)
addTransition(uTurnState, uTurnTime, stopWalk2State)
addTransition(stopWalk2State, 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, waitStandingState, standState,
sayLetsWalkState, sayGoodbyeState, sitState, restState,
shutdownState, walkState, stopWalkState, rotateState, uTurnState,
sayRotateState, sayUTurnState, stopWalk2State)
setPrintTransition(myFSM, True)
# Set the initial state to waitSittingState
setInitialState(myFSM, waitSittingState)
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)
# The main FSM
# Transitions for the mainFSM
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, lookBallFSM)
addTransition(lookBallFSM, seeBall, kickBallFSM)
addTransition(kickBallFSM, noSeeBall, resetKickBallState)
addTransition(resetKickBallState, lambda wm: True, stopWalkState)
addTransition(stopWalkState, lambda wm: True, resetLookBallState)
addTransition(resetLookBallState, lambda wm: True, lookBallFSM)
addTransition(kickBallFSM, detectTouch, stopWalkState2)
addTransition(lookBallFSM, detectTouch, stopWalkState2)
addTransition(stopWalkState2, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, standState, sitState, restState,
shutdownState, lookBallFSM, kickBallFSM, stopWalkState,
stopWalkState2, resetLookBallState, resetKickBallState)
setInitialState(mainFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(mainFSM, 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)
standState = createState("standState", stand)
shutdownState = createState("shutdownState",
lambda : shutdown("Final state reached"))
stopWalkState = createState("stopWalkState", stopWalking)
circleLeftState = createState("circleLeftState", lambda: setWalkVelocity(0, 1, -0.45))
circleRightState = createState("circleRightState", lambda: setWalkVelocity(0, 1, 0.45))
# The main FSM
# Transitions for the mainFSM
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, circleLeftState)
addTransition(circleLeftState, detectTouch, stopWalkState)
addTransition(stopWalkState, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, standState, sitState,
restState, shutdownState, circleLeftState, circleRightState,
stopWalkState)
setInitialState(mainFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(mainFSM, True)
archRightState = createState("archRightState", lambda: setWalkVelocity(0.65, 0, -0.15))
walkToBallState = createState("walkToBallState", lambda: setWalkVelocity(0.6, 0, 0))
watchBallState = createState("watchBallState", lambda wm: lookAtBall2(wm))
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
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)
lambda: communicate("puff", "Stand"))
sendSitStatus = createState("sendSitStatus",
lambda: communicate("puff", "Sit"))
# States for talking
sayShutdownState = createState("sayShutdownState", lambda: say("Shutting Down"))
# Add transitions according to the state diagram
addTransition(waitSittingState, detectTouch, sendStandStatus)
addTransition(sendStandStatus, lambda wm: True, waitSittingState2)
addTransition(waitSittingState2, touchDelay, sendSitStatus)
addTransition(sendSitStatus, lambda wm: True, waitSittingState3)
addTransition(waitSittingState3, touchDelay, sayShutdownState)
addTransition(sayShutdownState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
# Create the FSM and add the states created above
mainFSM = createFSM("mainFSM")
addStates(mainFSM , waitSittingState, waitSittingState2, waitSittingState3,
sendStandStatus, sendSitStatus,
restState, shutdownState, sayShutdownState)
# Set the initial state to waitSittingState
setInitialState(mainFSM, waitSittingState)
setPrintTransition(mainFSM, True)
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)
# Create states for talking
sayHelloState = createState("sayHelloState", lambda: say("Hello World"))
sayGoodbyeState = createState("sayGoodbyeState", lambda: say("Goodbye World!"))
# Create states for waiting for touch
waitSittingState = createState("waitSittingState", lambda: None)
waitStandingState = createState("waitStandingState", lambda: None)
# Create communcationsstates
# Add transitions according to the state diagram
addTransition(waitSittingState, waitForStand, standState)
addTransition(standState, lambda wm: True, sayHelloState)
addTransition(sayHelloState, lambda wm: True, waitStandingState)
addTransition(waitStandingState, waitForSit, sayGoodbyeState)
addTransition(sayGoodbyeState, 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
secondFSM = createFSM("secondFSM")
addStates(secondFSM, waitSittingState, waitStandingState, standState,
sayHelloState, sayGoodbyeState, sitState, restState, shutdownState)
# Set the initial state to waitSittingState
setInitialState(secondFSM, waitSittingState)
setPrintTransition(secondFSM, True)
# Create states for function calls
# Add transitions according to the state diagram
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, sayLetsWalkState)
addTransition(sayLetsWalkState, lambda wm: True, walkState)
addTransition(walkState, walkTime, stopWalkState)
addTransition(stopWalkState, lambda wm: True, rotateState)
addTransition(rotateState, rotateTime, uTurnState)
addTransition(uTurnState, uTurnTime, stopWalk2State)
addTransition(stopWalk2State, 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, waitStandingState, standState,
sayLetsWalkState, sayGoodbyeState, sitState, restState,
shutdownState, walkState, stopWalkState, rotateState,
uTurnState, sayRotateState, sayUTurnState,
stopWalk2State)
setPrintTransition(myFSM, True)
# Set the initial state to waitSittingState
setInitialState(myFSM, waitSittingState)
# Create states
waitSittingState = createState("waitSittingState", lambda: None)
standState = createState("standState", stand)
sitState = createState("sitState", sit)
restState = createState("restState", rest)
stopWalkState = createState("stopWalkState", stopWalking)
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
# Add transitions
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, walkObstacleFSM)
addTransition(walkObstacleFSM, detectTouch, stopWalkState)
addTransition(stopWalkState, 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
mainFSM = createFSM("mainFSM")
addStates(mainFSM, walkObstacleFSM, waitSittingState, standState,
stopWalkState, sitState, restState, shutdownState)
# Set the initial state to waitSittingState
setInitialState(mainFSM, waitSittingState)
setPrintTransition(mainFSM, True)
resetLookBallState = createState("resetLookBallState", lambda: resetSubFSM(lookBallFSM))
# The main FSM
# Transitions for the mainFSM
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, lookBallFSM)
addTransition(lookBallFSM, seeBall, followBallFSM)
addTransition(followBallFSM, noSeeBall, resetFollowBallState)
addTransition(resetFollowBallState, lambda wm: True, stopWalkState)
addTransition(stopWalkState, lambda wm: True, lookBallFSM)
addTransition(followBallFSM, detectTouch, sitState)
addTransition(lookBallFSM, detectTouch, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, standState, sitState,
restState, shutdownState, lookBallFSM, followBallFSM,
stopWalkState, resetLookBallState, resetFollowBallState)
setInitialState(mainFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(mainFSM, True)
lambda: setWalkVelocity(0.6, 0, 0))
watchBallState = createState("watchBallState", lambda wm: lookAtBall2(wm))
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
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(waitSittingState, waitForStand, standState)
addTransition(standState, waitForFindBall, lookBallFSM)
addTransition(lookBallFSM, seeBall, resetLookBallState)
addTransition(resetLookBallState, lambda wm: True, goToBallFSM)
addTransition(goToBallFSM, noSeeBall, resetgoToBallState)
addTransition(resetgoToBallState, lambda wm: True, lookBallFSM)
addTransition(goToBallFSM, closeToFeet, stopWalkState)
addTransition(stopWalkState, lambda wm: True, sendSitStatus)
addTransition(goToBallFSM, detectTouch, stopWalkState2)
addTransition(lookBallFSM, detectTouch, stopWalkState2)
addTransition(stopWalkState2, lambda wm: True, sendSitStatus)
addTransition(sendSitStatus, 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,
stopWalkState, stopWalkState2, sendSitStatus,
resetLookBallState, resetgoToBallState)
setInitialState(secondFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(secondFSM, True)
from followBallFSM import (followBallFSM, rotateLeftState, rotateRightState,
walkToBallState, watchBallState, stopWalkingState,
stopWalkingState2, followBallFSM)
def detectTouch(wm):
return readWM(wm, "tactile", "middle")
waitSittingState = createState("waitSittingState", lambda: None)
sitState = createState("sitState", sit)
restState = createState("restState", rest)
standState = createState("standState", stand)
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, sitState, restState, standState,
shutdownState, followBallFSM)
setInitialState(mainFSM, waitSittingState)
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, followBallFSM)
addTransition(followBallFSM, detectTouch, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
setPrintTransition(mainFSM, True)
addTransition(stopWalkingState2, lambda wm: True, setBottomCamState)
addTransition(setBottomCamState, lambda wm: True, setBottomLedState)
addTransition(setBottomLedState, lambda wm: True, lookDownState)
addTransition(lookDownState, seeBall, kickBallFSM)
addTransition(lookDownState, noSeeBall, stopWalkingState3)
addTransition(stopWalkingState3, lambda wm: True, resetLookBallState)
addTransition(resetLookBallState, lambda wm: True, lookBallFSM)
addTransition(kickBallFSM, oneKick, sendFindBallStatus)
addTransition(sendFindBallStatus, lambda wm: True, stopWalkingState4)
addTransition(stopWalkingState4, waitForSit, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
# Create the FSM and add the states created above
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, setBottomCamState, setBottomLedState,
sendStandStatus, sendFindBallStatus, resetLookBallState, resetgoToBallState, resetFindNaoState,
sitState, standState, lookBallFSM, goToBallFSM, findNaoFSM, kickBallFSM,
restState, shutdownState, stopWalkingState, stopWalkingState2, stopWalkingState3,
stopWalkingState4, setTopCamState, setTopLedState, lookDownState)
# Set the initial state to waitSittingState
setInitialState(mainFSM, waitSittingState)
setPrintTransition(mainFSM, 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)
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)
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)
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)
lambda: setCamera("top"))
lookAtBallState = createState("lookAtBallState", lambda wm: lookAtBall(wm))
rotateState = createState("rotateState", lambda: setWalkVelocity(0, 0, 0.5))
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)
# States for talking
sayShutdownState = createState("sayShutdownState",
lambda: say("Shutting Down"))
# Add transitions according to the state diagram
addTransition(waitSittingState, detectTouch, sendStandStatus)
addTransition(sendStandStatus, lambda wm: True, waitSittingState2)
addTransition(waitSittingState2, touchDelay, sendKickStatus)
addTransition(sendKickStatus, lambda wm: True, waitSittingState3)
addTransition(waitSittingState3, touchDelay, sendSitStatus)
addTransition(sendSitStatus, lambda wm: True, waitSittingState4)
addTransition(waitSittingState4, touchDelay, sayShutdownState)
addTransition(sayShutdownState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
# Create the FSM and add the states created above
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, waitSittingState2, waitSittingState3,
waitSittingState4, sendStandStatus, sendSitStatus, sendKickStatus,
restState, shutdownState, sayShutdownState)
# Set the initial state to waitSittingState
setInitialState(mainFSM, waitSittingState)
setPrintTransition(mainFSM, True)
setBottomCameraState = createState("setBottomCameraState", lambda : setCamera("bottom"))
setTopCameraState2 = createState("setTopCameraState2", lambda : setCamera("top"))
lookAtBallState = createState("lookAtBallState", lambda wm : lookAtBall2(wm))
rotateState = createState("rotateState", lambda : setWalkVelocity(0, 0, 0.5))
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)
restState = createState("restState", rest)
shutdownState = createState("shutdownState",
lambda : shutdown("Final state reached"))
# Create states for talking
sayHelloState = createState("sayHelloState", lambda: say("Hello World"))
sayGoodbyeState = createState("sayGoodbyeState", lambda: say("Goodbye World!"))
# Create states for waiting for touch
waitSittingState = createState("waitSittingState", lambda : None)
waitStandingState = createState("waitStandingState", lambda : None)
# Add transitions according to the state diagram
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, sayHelloState)
addTransition(sayHelloState, lambda wm: True, waitStandingState)
addTransition(waitStandingState, detectTouch, sayGoodbyeState)
addTransition(sayGoodbyeState, 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, waitStandingState, standState,
sayHelloState, sayGoodbyeState, sitState, restState, shutdownState)
# Set the initial state to waitSittingState
setInitialState(myFSM, waitSittingState)
# The main FSM
# Transitions for the mainFSM
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, lookBallFSM)
addTransition(lookBallFSM, seeBall, kickBallFSM)
addTransition(kickBallFSM, noSeeBall, resetKickBallState)
addTransition(resetKickBallState, lambda wm: True, stopWalkState)
addTransition(stopWalkState, lambda wm: True, resetLookBallState)
addTransition(resetLookBallState, lambda wm: True, lookBallFSM)
addTransition(kickBallFSM, detectTouch, stopWalkState2)
addTransition(lookBallFSM, detectTouch, stopWalkState2)
addTransition(stopWalkState2, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, standState, sitState,
restState, shutdownState, lookBallFSM, kickBallFSM,
stopWalkState, stopWalkState2, resetLookBallState, resetKickBallState)
setInitialState(mainFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(mainFSM, True)
shutdownState = createState("shutdownState",
lambda : shutdown("Final state reached"))
stopWalkState = createState("stopWalkState", stopWalking)
lookAtGoalState = createState("lookAtGoalState", stopWalking)
# The main FSM
# Transitions for the mainFSM
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, findGoalFSM)
addTransition(findGoalFSM, seeGoal, lookAtGoalState)
addTransition(findGoalFSM, detectTouch, stopWalkState)
addTransition(lookAtGoalState, detectTouch, stopWalkState)
addTransition(stopWalkState, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, standState, sitState,
restState, shutdownState, findGoalFSM, lookAtGoalState,
stopWalkState)
setInitialState(mainFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(mainFSM, True)
from walkObstacleFSM import (walkObstacleFSM)
# Create states
waitSittingState = createState("waitSittingState", lambda: None)
standState = createState("standState", stand)
sitState = createState("sitState", sit)
restState = createState("restState", rest)
stopWalkState = createState("stopWalkState", stopWalking)
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
# Add transitions
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, walkObstacleFSM)
addTransition(walkObstacleFSM, detectTouch, stopWalkState)
addTransition(stopWalkState, 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
mainFSM = createFSM("mainFSM")
addStates(mainFSM, walkObstacleFSM, waitSittingState, standState,
stopWalkState, sitState, restState, shutdownState)
# Set the initial state to waitSittingState
setInitialState(mainFSM, waitSittingState)
setPrintTransition(mainFSM, True)
lambda: setWalkVelocity(0, 0, -0.15))
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)
sitState = createState("sitState", sit)
restState = createState("restState", rest)
standState = createState("standState", stand)
shakeHeadState = createState("shakeHeadState", hMShake)
shakeHead2State = createState("shakeHead2State", hMShake)
hangHeadState = createState("hangHeadState", hMHang)
shutdownState = createState("shutdownState",
lambda : shutdown("Final state reached"))
# Add transitions
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, shakeHeadState )
addTransition(shakeHeadState, lambda wm: True, hangHeadState )
addTransition(hangHeadState, lambda wm: True, shakeHead2State )
addTransition(shakeHead2State, 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,
shakeHeadState, shakeHead2State,
hangHeadState)
setPrintTransition(myFSM, True)
# Set the initial state to waitSittingState
setInitialState(myFSM, waitSittingState)
standState = createState("standState", stand)
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
stopWalkState = createState("stopWalkState", stopWalking)
lookAtGoalState = createState("lookAtGoalState", stopWalking)
# The main FSM
# Transitions for the mainFSM
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, findGoalFSM)
addTransition(findGoalFSM, seeGoal, lookAtGoalState)
addTransition(findGoalFSM, detectTouch, stopWalkState)
addTransition(lookAtGoalState, detectTouch, stopWalkState)
addTransition(stopWalkState, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, standState, sitState, restState,
shutdownState, findGoalFSM, lookAtGoalState, stopWalkState)
setInitialState(mainFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(mainFSM, True)
rotateLeftState = createState("rotateLeftState", lambda : setWalkVelocity(0,0, 0.15))
rotateRightState = createState("rotateRightState", lambda : setWalkVelocity(0,0, -0.15))
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)
# Transitions for the mainFSM
addTransition(waitSittingState, waitForStand, standState)
addTransition(standState, waitForFindBall, lookBallFSM)
addTransition(lookBallFSM, seeBall, resetLookBallState)
addTransition(resetLookBallState, lambda wm: True, goToBallFSM)
addTransition(goToBallFSM, noSeeBall, resetgoToBallState)
addTransition(resetgoToBallState, lambda wm: True, lookBallFSM)
addTransition(goToBallFSM, closeToFeet, stopWalkState)
addTransition(stopWalkState, lambda wm: True, sendSitStatus)
addTransition(goToBallFSM, detectTouch, stopWalkState2)
addTransition(lookBallFSM, detectTouch, stopWalkState2)
addTransition(stopWalkState2, lambda wm: True, sendSitStatus)
addTransition(sendSitStatus, 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, stopWalkState, stopWalkState2,
sendSitStatus, resetLookBallState, resetgoToBallState)
setInitialState(secondFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(secondFSM, 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(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)
startWalking, turnHead, setCamera,
say, setLED, setWalkVelocity, stopWalking)
from followBallFSM import (followBallFSM, rotateLeftState,
rotateRightState, walkToBallState, watchBallState,
stopWalkingState, stopWalkingState2, followBallFSM)
def detectTouch(wm):
return readWM(wm, "tactile", "middle")
waitSittingState = createState("waitSittingState", lambda: None)
sitState = createState("sitState", sit)
restState = createState("restState", rest)
standState = createState("standState", stand)
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, sitState, restState, standState,
shutdownState, followBallFSM)
setInitialState(mainFSM, waitSittingState)
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, followBallFSM)
addTransition(followBallFSM, detectTouch, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
setPrintTransition(mainFSM, True)
# 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 : 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)
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
stopWalkState = createState("stopWalkState", stopWalking)
circleLeftState = createState("circleLeftState",
lambda: setWalkVelocity(0, 1, -0.45))
circleRightState = createState("circleRightState",
lambda: setWalkVelocity(0, 1, 0.45))
# The main FSM
# Transitions for the mainFSM
addTransition(waitSittingState, detectTouch, standState)
addTransition(standState, lambda wm: True, circleLeftState)
addTransition(circleLeftState, detectTouch, stopWalkState)
addTransition(stopWalkState, lambda wm: True, sitState)
addTransition(sitState, lambda wm: True, restState)
addTransition(restState, lambda wm: True, shutdownState)
mainFSM = createFSM("mainFSM")
addStates(mainFSM, waitSittingState, standState, sitState, restState,
shutdownState, circleLeftState, circleRightState, stopWalkState)
setInitialState(mainFSM, waitSittingState)
# Prints all the completed transitions
setPrintTransition(mainFSM, True)
