# Import the FSM functions
from fsm.functions import (createState, createFSM, addTransition,
addStates, setInitialState, readWM, writeWM, setPrintTransition)
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown,
startWalking, turnHead, setCamera, resetSubFSM,
say, setLED, setWalkVelocity, stopWalking)
# 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)
addStates, setInitialState, readWM, writeWM, setPrintTransition)
# Import primitive robot behaviors
from api.pubapi import (
startWalking, turnHead, setCamera,
say, setLED, setWalkVelocity, stopWalking)
# 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)
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)
def uTurnTime(wm):
if currentTime(wm) - entryTime(wm) >= 17:
return True
else:
return False
## CREATE STATES ##
sitState = createState("sitState", sit)
standState = createState("standState", stand)
restState = createState("restState", rest)
walkState = createState("walkState", startWalking)
stopWalkState = createState("stopWalkState", stopWalking)
stopWalk2State = createState("stopWalk2State", stopWalking)
rotateState = createState("rotateState", lambda: setWalkVelocity(0, 0, 0.5))
uTurnState = createState("uTurnState", lambda: setWalkVelocity(1, 0, 0.2))
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
# 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)
# Import primitive robot behaviors
from api.pubapi import (startWalking, turnHead, setCamera, kick, say, setLED,
setWalkVelocity, stopWalking)
# Import functions we've written
from functions import (averageLook, lookAtBall, largestBall, positiveYaw,
negativeYaw, zeroYaw, switchCamera, entryTime,
currentTime, cameraDelay, closeToFeet, leftFoot,
rightFoot, farNegativeYaw, farPositiveYaw, lookAtBall2,
seeGoal)
#create states
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))
# 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
lookBallFSM = createFSM("lookBallFSM")
addStates(lookBallFSM, shakeHeadState, stopWalkState, setTopCameraState,
setTopCameraState2, setBottomCameraState, bottomLedState,
topLedState, lookAtBallState, rotateState, shakeHeadState2,
topLedState2)
# 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)
# Import primitive robot behaviors
from api.pubapi import (
startWalking, turnHead, setCamera, kick,
say, setLED, setWalkVelocity, stopWalking)
# Import functions we've written
from functions import (averageLook, lookAtBall, largestBall, positiveYaw,
negativeYaw, zeroYaw, switchCamera, entryTime, currentTime, cameraDelay,
closeToFeet, leftFoot, rightFoot, farNegativeYaw, farPositiveYaw, lookAtBall2,
seeGoal)
#create states
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
say, setLED, setWalkVelocity, stopWalking)
# Import functions we've written
from functions import (detectTouch)
# 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)
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)
setPrintTransition)
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown, 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)
# 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))
setInitialState, readWM, writeWM,
setPrintTransition)
# Import primitive robot behaviors
from api.pubapi import (startWalking, turnHead, setCamera, say, setLED,
setWalkVelocity, stopWalking)
# 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")
from fsm.functions import (createState, createFSM, addTransition,
addStates, setInitialState, readWM, writeWM, setPrintTransition)
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown,
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)
def uTurnTime(wm):
if currentTime(wm) - entryTime(wm) >= 17:
return True
else:
return False
## CREATE STATES ##
sitState = createState("sitState", sit)
standState = createState("standState", stand)
restState = createState("restState", rest)
walkState = createState("walkState", startWalking)
stopWalkState = createState("stopWalkState", stopWalking)
stopWalk2State = createState("stopWalk2State", stopWalking)
rotateState = createState("rotateState", lambda: setWalkVelocity(0, 0, 0.5))
uTurnState = createState("uTurnState", lambda: setWalkVelocity(1, 0, 0.2))
shutdownState = createState("shutdownState",
lambda: shutdown("Final state reached"))
# 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
setInitialState, readWM, writeWM,
setPrintTransition)
# Import primitive robot behaviors
from api.pubapi import (sit, stand, rest, say, shutdown, startWalking,
turnHead, setCamera, resetSubFSM, say, setLED,
setWalkVelocity, stopWalking)
# 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)
