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)