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