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) addTransition(rotateLeftState, lambda wm: True, watchBallState) addTransition(archLeftState, zeroYaw, walkToBallState) addTransition(archLeftState, lambda wm: True, watchBallState)
# 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 talking sayLetsWalkState = createState("sayLetsWalkState", lambda: say("Hej Yousif!")) sayRotateState = createState("sayRotateState", lambda: say("spinn!")) sayUTurnState = createState("sayUTurnState", lambda: say("u turn!")) sayGoodbyeState = createState("sayGoodbyeState", lambda: say("Goodbye World!")) # 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,
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)
restState = createState("restState", rest) standState = createState("standState", stand) shutdownState = createState("shutdownState", lambda: shutdown("Final state reached")) stopWalkState = createState("stopWalkState", stopWalking) stopWalkState2 = createState("stopWalkState2", stopWalking) resetKickBallState = createState("resetKickBallState", lambda: resetSubFSM(kickBallFSM)) 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, 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)
# Import functions we've written from functions import (detectTouch, seeGoal, seeGoalLeft, seeGoalRight, seeGoalSingle) # Create states rotateLeftState = createState("rotateLeftState", lambda: setWalkVelocity(0, 0, 0.7)) rotateRightState = createState("rotateRightState", lambda: setWalkVelocity(0,0, -0.7)) lookUpState = createState("lookUpState", lambda wm: lookUp(wm)) lookDownState = createState("lookDownState", lambda: turnHead(0, 0)) 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)
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")) 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)
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) # 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)
resetgoToBallState = createState("resetgoToBallState", lambda: resetSubFSM(goToBallFSM)) # States for communication robot = "david" sendSitStatus = createState("sendSitStatus" , lambda: communicate(robot, "Sit")) # The main FSM # 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)
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)
stopWalkState2 = createState("stopWalkState2", stopWalking) stopWalkState3 = createState("stopWalkState3", stopWalking) stopWalkState4 = createState("stopWalkState4", stopWalking) resetKickBallState = createState("resetKickBallState", lambda : resetSubFSM(kickBallFSM)) resetLookBallState = createState("resetLookBallState", lambda : resetSubFSM(lookBallFSM)) resetgoToBallState = createState("resetgoToBallState", lambda : resetSubFSM(goToBallFSM)) resetFindGoalState = createState("resetFindGoalState", lambda : resetSubFSM(findGoalFSM)) setBottomCameraState = createState("setBottomCameraState", lambda : setCamera("bottom")) bottomLedState = createState("bottomLedState", lambda : setLED("eyes", 1, 0, 0)) # Red lookDownState = createState("lookDownState", lambda : turnHead(0,0.3149)) # The main FSM # Transitions for the mainFSM addTransition(waitSittingState, detectTouch, standState) addTransition(standState, lambda wm: True, lookBallFSM) addTransition(lookBallFSM, seeBall, resetgoToBallState) addTransition(resetgoToBallState, lambda wm: True, goToBallFSM) addTransition(goToBallFSM, noSeeBall, resetgoToBallState) addTransition(goToBallFSM, closeToFeet, stopWalkState3) addTransition(stopWalkState3, lambda wm: True, resetFindGoalState) addTransition(resetFindGoalState, lambda wm: True, findGoalFSM) addTransition(findGoalFSM, seeGoal, stopWalkState4) addTransition(stopWalkState4, lambda wm: True, setBottomCameraState) addTransition(setBottomCameraState, lambda wm: True, bottomLedState) addTransition(bottomLedState, lambda wm: True, lookDownState)
sayLetsWalkState = createState("sayLetsWalkState", lambda: say("Hej Yousif!")) sayRotateState = createState("sayRotateState", lambda: say("spinn!")) sayUTurnState = createState("sayUTurnState", lambda: say("u turn!")) sayGoodbyeState = createState("sayGoodbyeState", lambda: say("Goodbye World!")) # 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,
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(rotateLeftState, lambda wm: True, watchBallState) addTransition(archLeftState, zeroYaw, walkToBallState) addTransition(archLeftState, lambda wm: True, watchBallState)
# 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)
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)
setTopLedState = createState("setTopLedState", lambda: setLED("eyes", 0.0, 1.0, 0.0)) # Green lookDownState = createState("lookDownState", lambda: turnHead(0,0.3149)) # Create communcationsstates robot = "piff" sendStandStatus = createState("sendStandStatus" , lambda: communicate(robot, "Stand")) sendFindBallStatus = createState("sendFindBallStatus", lambda: communicate(robot, "Find ball")) # Add transitions addTransition(waitSittingState , detectTouch, sendStandStatus) addTransition(sendStandStatus , lambda wm: True, standState) addTransition(standState, lambda wm: True, lookBallFSM) addTransition(lookBallFSM, seeBall, resetgoToBallState) addTransition(resetgoToBallState, lambda wm: True, goToBallFSM) addTransition(goToBallFSM, noSeeBall, resetLookBallState) addTransition(goToBallFSM, closeToFeet, stopWalkingState) addTransition(stopWalkingState, lambda wm: True, resetFindNaoState) addTransition(resetFindNaoState, lambda wm: True, findNaoFSM) addTransition(findNaoFSM, seeNao, stopWalkingState2) addTransition(stopWalkingState2, lambda wm: True, setBottomCamState)
# Create communcationsstates sendStandStatus = createState("sendStandStatus" , 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
circleLeftState = createState("circleLeftState", lambda: setWalkVelocity(0, 1, -0.45)) circleRightState = createState("circleRightState", lambda: setWalkVelocity(0, 1, 0.45)) lookUpState = createState("lookUpState", lambda wm: lookUp(wm)) stopWalkingState = createState("stopWalkingState", stopWalking) stopRotateState = createState("stopRotateState", stopWalking) setTopCameraState = createState("setTopCameraState", lambda: setCamera("top")) topLedState = createState("topLedState", lambda: setLED("eyes", 0, 1, 0)) # Green lookStraightState = createState("lookStraightState", lambda: turnHead(0.25, 0)) standState = createState("standState", stopWalking) leftStepState = createState("leftStepState", lambda: setWalkVelocity(0, 0.5 ,0)) 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,
seeGoalSingle) # Create states rotateLeftState = createState("rotateLeftState", lambda: setWalkVelocity(0, 0, 0.7)) rotateRightState = createState("rotateRightState", lambda: setWalkVelocity(0, 0, -0.7)) lookUpState = createState("lookUpState", lambda wm: lookUp(wm)) lookDownState = createState("lookDownState", lambda: turnHead(0, 0)) 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)
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: 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) # repeat after first 90 degrees addTransition(rotateState, rotateTime, stopWalkState) addTransition(stopWalkState, lambda wm: True, shakeHeadState)
sendStandStatus = createState("sendStandStatus", lambda: communicate(robot, "Stand")) sendSitStatus = createState("sendSitStatus", lambda: communicate(robot, "Sit")) sendKickStatus = createState("sendKickStatus", lambda: communicate(robot, "Kick")) # 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")
# 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) # repeat after first 90 degrees addTransition(rotateState, rotateTime, stopWalkState)
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)
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")) stopWalkState = createState("stopWalkState", stopWalking) stopWalkState2 = createState("stopWalkState2", stopWalking) resetKickBallState = createState("resetKickBallState", lambda: resetSubFSM(kickBallFSM)) 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, 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)
# Create states 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")) 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)
# Import FSM 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)
# Create states 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")) 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)
# create states waitSittingState = createState("waitSittingState", lambda : None) 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)
resetLookBallState = createState("resetLookBallState", lambda: resetSubFSM(lookBallFSM)) resetgoToBallState = createState("resetgoToBallState", lambda: resetSubFSM(goToBallFSM)) # States for communication robot = "david" sendSitStatus = createState("sendSitStatus", lambda: communicate(robot, "Sit")) # The main FSM # 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)
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(walkToBallState, lambda wm: True, watchBallState) addTransition(setBottomCamState, cameraDelay, setBottomLedState) addTransition(setBottomLedState, lambda wm: True, walkToBallState)
lookUpState = createState("lookUpState", lambda wm: lookUp(wm)) stopWalkingState = createState("stopWalkingState", stopWalking) stopRotateState = createState("stopRotateState", stopWalking) setTopCameraState = createState("setTopCameraState", lambda: setCamera("top")) topLedState = createState("topLedState", lambda: setLED("eyes", 0, 1, 0)) # Green lookStraightState = createState("lookStraightState", lambda: turnHead(0, 0)) standState = createState("standState", stopWalking) leftStepState = createState("leftStepState", lambda: setWalkVelocity(0, 0.5, 0)) 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, seeGoal, stopWalkingState) addTransition(circleLeftState, seeGoalLeft, circleRightState) addTransition(circleLeftState, seeGoalSingle, stopRotateState) addTransition(circleRightState, seeGoal, stopWalkingState)
#resetLookBallState = createState("resetLookBallState", lambda : resetSubFSM(lookBallFSM)) setBottomCamState = createState("setBottomCamState", lambda: setCamera("bottom")) setBottomLedState = createState("setBottomLedState", lambda: setLED("eyes", 1.0, 0.0, 0.0)) # Red walkState = createState("walkState", lambda: setWalkVelocity(0.6, 0, 0)) rotateState = createState("rotateState", lambda: setWalkVelocity(0, 0, -0.4)) stopWalkState = createState("stopWalkState", stopWalking) leftHeadState = createState("leftHeadState", lambda: turnHead(0.5, 0.2549)) rightHeadState = createState("rightHeadState", lambda: turnHead(-0.5, 0.2549)) lookDownState = createState("lookDownState", lambda: turnHead(0, 0.0549)) #0.1149 ok lookDownState2 = createState("lookDownState2", lambda: turnHead(0, 0.0549)) lookDownMoreState = createState("lookDownMoreState", lambda: turnHead(0, 0.2549)) # Add transitions addTransition(setBottomCamState, lambda wm: True, setBottomLedState) addTransition(setBottomLedState, lambda wm: True, lookDownState2) addTransition(lookDownState2, lambda wm: True, walkState) 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)
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)
stopWalkState2 = createState("stopWalkState2", stopWalking) stopWalkState3 = createState("stopWalkState3", stopWalking) stopWalkState4 = createState("stopWalkState4", stopWalking) resetKickBallState = createState("resetKickBallState", lambda: resetSubFSM(kickBallFSM)) resetLookBallState = createState("resetLookBallState", lambda: resetSubFSM(lookBallFSM)) resetgoToBallState = createState("resetgoToBallState", lambda: resetSubFSM(goToBallFSM)) resetFindGoalState = createState("resetFindGoalState", lambda: resetSubFSM(findGoalFSM)) setBottomCameraState = createState("setBottomCameraState", lambda: setCamera("bottom")) bottomLedState = createState("bottomLedState", lambda: setLED("eyes", 1, 0, 0)) # Red lookDownState = createState("lookDownState", lambda: turnHead(0,0.3149)) # The main FSM # Transitions for the mainFSM addTransition(waitSittingState, waitForStand, standState) addTransition(standState, lambda wm: True, lookBallFSM) addTransition(lookBallFSM, seeBall, resetgoToBallState) addTransition(resetgoToBallState, lambda wm: True, goToBallFSM) addTransition(goToBallFSM, noSeeBall, resetgoToBallState) 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)
waitSittingState = createState("waitSittingState", lambda: None) waitStandingState = createState("waitStandingState", lambda: None) sitState = createState("sitState", sit) restState = createState("restState", rest) standState = createState("standState", stand) hangHeadState = createState("hangHeadState", hMHang) shutdownState = createState("shutdownState", 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,
sitState = createState("sitState", sit) restState = createState("restState", rest) 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