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)
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)
#create states 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)
shakeHeadState = createState("shakeHeadState", lambda wm: headTurning(wm)) shakeHeadState2 = createState("shakeHeadState2", lambda wm: headTurning(wm)) setTopCameraState = createState("setTopCameraState", lambda : setCamera("top")) 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)
from api.pubapi import (startWalking, turnHead, setCamera, kick, say, setLED, 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
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)
# Import the FSM functions from fsm.functions import (createState, createFSM, addTransition, addStates, setInitialState) # Import primitive robot behaviors from api.pubapi import (sit, stand, rest, shutdown) # Create states sitState = createState("sitState", sit) standState = createState("standState", stand) restState = createState("restState", rest) shutdownState = createState("shutdownState", lambda: shutdown("Final state reached")) # Add transitions between states addTransition(standState, 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("myFSM") addStates(myFSM, standState, sitState, restState, shutdownState) # Set the initial state to standState setInitialState(myFSM, standState)
say, setLED, 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)
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: 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(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)
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)
rotateRightState = createState("rotateRightState", lambda: setWalkVelocity(0,0, -0.15)) archLeftState = createState("archLeftState", lambda: setWalkVelocity(0.65, 0, 0.15)) 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)
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)
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)
lambda: setCamera("bottom")) setTopCameraState2 = createState("setTopCameraState2", 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)
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)
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)
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)
# Import the FSM functions from fsm.functions import ( createState, createFSM, addTransition, addStates, setInitialState) # Import primitive robot behaviors from api.pubapi import (sit, stand, rest, shutdown) # Create states sitState = createState("sitState", sit) standState = createState("standState", stand) restState = createState("restState", rest) shutdownState = createState("shutdownState", lambda : shutdown("Final state reached")) # Add transitions between states addTransition(standState, 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("myFSM") addStates(myFSM, standState, sitState, restState, shutdownState) # Set the initial state to standState setInitialState(myFSM, standState)