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)
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(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)
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) addTransition(rotateLeftState, lambda wm: True, watchBallState) addTransition(archLeftState, zeroYaw, walkToBallState) addTransition(archLeftState, lambda wm: True, watchBallState) addTransition(walkToBallState, closeToFeet, stopWalkingState) addTransition(walkToBallState, switchCamera, setBottomCamState) addTransition(walkToBallState, lambda wm: True, watchBallState) addTransition(setBottomCamState, cameraDelay, setBottomLedState) addTransition(setBottomLedState, lambda wm: True, watchBallState) # Prints out completed transitions setPrintTransition(goToBallFSM, True)
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(walkToBallState, lambda wm: True, watchBallState) addTransition(setBottomCamState, cameraDelay, setBottomLedState) addTransition(setBottomLedState, lambda wm: True, walkToBallState) # Prints out completed transitions setPrintTransition(followBallFSM, True)
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) addTransition(rotateLeftState, lambda wm: True, watchBallState) addTransition(archLeftState, zeroYaw, walkToBallState) addTransition(archLeftState, lambda wm: True, watchBallState) addTransition(walkToBallState, closeToFeet, stopWalkingState) addTransition(walkToBallState, switchCamera, setBottomCamState) addTransition(walkToBallState, lambda wm: True, watchBallState) addTransition(setBottomCamState, cameraDelay, setBottomLedState) addTransition(setBottomLedState, lambda wm: True, watchBallState) # Prints out completed transitions setPrintTransition(goToBallFSM, 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)
# 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) # repeat after first 90 degrees addTransition(rotateState, rotateTime, stopWalkState) addTransition(stopWalkState, lambda wm: True, shakeHeadState) # if ball is lost from sight addTransition(lookAtBallState, noSeeBall, setTopCameraState2) addTransition(setTopCameraState2, cameraDelay, topLedState2) addTransition(topLedState2, seeBall, lookAtBallState) addTransition(topLedState2, noSeeBall, shakeHeadState) # Prints out completed transitions setPrintTransition(lookBallFSM, 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)
# 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)
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)
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(walkToBallState, lambda wm: True, watchBallState) addTransition(setBottomCamState, cameraDelay, setBottomLedState) addTransition(setBottomLedState, lambda wm: True, walkToBallState) # Prints out completed transitions setPrintTransition(followBallFSM, 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)
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)
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) # repeat after first 90 degrees addTransition(rotateState, rotateTime, stopWalkState) addTransition(stopWalkState, lambda wm: True, shakeHeadState) # if ball is lost from sight addTransition(lookAtBallState, noSeeBall, setTopCameraState2) addTransition(setTopCameraState2, cameraDelay, topLedState2) addTransition(topLedState2, seeBall, lookAtBallState) addTransition(topLedState2, noSeeBall, shakeHeadState) # Prints out completed transitions setPrintTransition(lookBallFSM, 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)
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)