# Import primitive robot behaviors
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)
rotateLeftState = createState("rotateLeftState",
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)
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown, startWalking,
turnHead, setCamera, say, setLED, setWalkVelocity,
stopWalking)
# Import functions we've written
from functions import (seeBall, noSeeBall, shakeHeadTime, largestBall,
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
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown,
startWalking, turnHead, setCamera,
say, setLED, setWalkVelocity, stopWalking)
# 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,
startWalking, turnHead, setCamera, resetSubFSM,
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)