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