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)
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)