from api.pubapi import (sit, stand, rest, say, shutdown, communicate, stopWalking,
resetSubFSM, setCamera, setLED, turnHead, setWalkVelocity)
# Import functions we've written
from functions import (closeToObstacle, closeToFeet, rotateTime, walkDelay,
obstacleToLeft, obstacleToRight, headDelay)
# Import FSM
# Currently none
# Create state
#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)
lambda: setWalkVelocity(0, 0, 0.15))
rotateRightState = createState("rotateRightState",
lambda: setWalkVelocity(0, 0, -0.15))
archLeftState = createState("archLeftState",
lambda: setWalkVelocity(0.65, 0, 0.15))
archRightState = createState("archRightState",
lambda: setWalkVelocity(0.65, 0, -0.15))
walkToBallState = createState("walkToBallState",
lambda: setWalkVelocity(0.6, 0, 0))
watchBallState = createState("watchBallState", lambda wm: lookAtBall2(wm))
stopWalkingState = createState("stopWalkingState", stopWalking)
stopWalkingState2 = createState("stopWalkingState2", stopWalking)
setBottomCamState = createState("setBottomCamState",
lambda: setCamera("bottom"))
setBottomLedState = createState("setBottomLedState",
lambda: setLED("eyes", 1, 0, 0))
kickRightState = createState("kickRightState", lambda: kick("right"))
kickLeftState = createState("kickLeftState", lambda: kick("left"))
# FSM for following the ball
goToBallFSM = createFSM("goToBallFSM")
addStates(goToBallFSM, rotateLeftState, rotateRightState, walkToBallState,
watchBallState, stopWalkingState, stopWalkingState2,
setBottomLedState, setBottomCamState, kickRightState, kickLeftState,
archRightState, archLeftState)
setInitialState(goToBallFSM, watchBallState)
# Transitions
# Import functions we've written
from functions import (averageLook, lookAtBall, largestBall, positiveYaw,
negativeYaw, zeroYaw, switchCamera, entryTime, currentTime, cameraDelay,
closeToFeet)
#create states
rotateLeftState = createState("rotateLeftState", lambda : setWalkVelocity(0,0, 0.15))
rotateRightState = createState("rotateRightState", lambda : setWalkVelocity(0,0, -0.15))
walkToBallState = createState("walkToBallState", lambda : setWalkVelocity(1, 0, 0))
watchBallState = createState("watchBallState", lambda wm : lookAtBall(wm))
stopWalkingState = createState("stopWalkingState", stopWalking)
stopWalkingState2 = createState("stopWalkingState2", stopWalking)
setBottomCamState = createState("setBottomCamState", lambda : setCamera("bottom"))
setBottomLedState = createState("setBottomLedState", lambda : setLED("eyes", 1,0,0))
# FSM for following the ball
followBallFSM = createFSM("followBallFSM")
addStates(followBallFSM, rotateLeftState, rotateRightState, walkToBallState,
watchBallState, stopWalkingState, stopWalkingState2, setBottomLedState,
setBottomCamState)
setInitialState(followBallFSM, watchBallState)
# Transitions
addTransition(watchBallState, positiveYaw, rotateLeftState)
addTransition(watchBallState, negativeYaw, rotateRightState)
# Import functions we've written
from functions import (seeBall, noSeeBall, shakeHeadTime, largestBall,
averageLook, lookAtBall, rotateTime, entryTime, currentTime,
cameraDelay, headTurning, lookAtBall2)
#Create States
shakeHeadState = createState("shakeHeadState", lambda wm: headTurning(wm))
shakeHeadState2 = createState("shakeHeadState2", lambda wm: headTurning(wm))
setTopCameraState = createState("setTopCameraState", lambda : setCamera("top"))
setBottomCameraState = createState("setBottomCameraState", lambda : setCamera("bottom"))
setTopCameraState2 = createState("setTopCameraState2", lambda : setCamera("top"))
lookAtBallState = createState("lookAtBallState", lambda wm : lookAtBall2(wm))
rotateState = createState("rotateState", lambda : setWalkVelocity(0, 0, 0.5))
bottomLedState = createState("bottomLedState", lambda : setLED("eyes", 1, 0, 0)) # Red
topLedState = createState("topLedState", lambda : setLED("eyes", 0, 1, 0)) # Green
topLedState2 = createState("topLedState2", lambda : setLED("eyes", 0, 1, 0)) # Green
stopWalkState = createState("stopWalkState", stopWalking)
# FSM for searching for the ball
lookBallFSM = createFSM("lookBallFSM")
addStates(lookBallFSM, shakeHeadState, stopWalkState,
setTopCameraState, setTopCameraState2,
setBottomCameraState, bottomLedState,
topLedState, lookAtBallState, rotateState,
shakeHeadState2,topLedState2)
setInitialState(lookBallFSM, shakeHeadState)
averageLook, lookAtBall, rotateTime, entryTime,
currentTime, cameraDelay, headTurning)
#Create States
shakeHeadState = createState("shakeHeadState", lambda wm: headTurning(wm))
shakeHeadState2 = createState("shakeHeadState2", lambda wm: headTurning(wm))
setTopCameraState = createState("setTopCameraState", lambda: setCamera("top"))
setBottomCameraState = createState("setBottomCameraState",
lambda: setCamera("bottom"))
setTopCameraState2 = createState("setTopCameraState2",
lambda: setCamera("top"))
lookAtBallState = createState("lookAtBallState", lambda wm: lookAtBall(wm))
rotateState = createState("rotateState", lambda: setWalkVelocity(0, 0, 0.5))
bottomLedState = createState("bottomLedState",
lambda: setLED("eyes", 1, 0, 0)) # Red
topLedState = createState("topLedState",
lambda: setLED("eyes", 0, 1, 0)) # Green
topLedState2 = createState("topLedState2",
lambda: setLED("eyes", 0, 1, 0)) # Green
stopWalkState = createState("stopWalkState", stopWalking)
# FSM for searching for the ball
lookBallFSM = createFSM("lookBallFSM")
addStates(lookBallFSM, shakeHeadState, stopWalkState, setTopCameraState,
setTopCameraState2, setBottomCameraState, bottomLedState,
topLedState, lookAtBallState, rotateState, shakeHeadState2,
topLedState2)
setInitialState(lookBallFSM, shakeHeadState)
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)
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)
from functions import (detectTouch, seeGoal, seeGoalLeft, seeGoalRight,
seeGoalSingle, lookForGoal, seeBall, oneStep, leftFoot,
rightFoot, betweenFeet)
# Create states
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, 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)
say, setLED, setWalkVelocity, stopWalking)
# Import functions we've written
from functions import (detectTouch, seeGoal, seeGoalLeft, seeGoalRight,
seeGoalSingle, lookForGoal, seeBall, oneStep, leftFoot, rightFoot,
betweenFeet, seeNao)
# Create states
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)
