def doLeftEyeObjects():
goal = world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
circle = world_objects.getObjPtr(core.WO_CENTER_CIRCLE)
if goal.seen:
ledsC.allLeftEye(0, 0, 1)
elif circle.seen:
ledsC.allLeftEye(1, 0, 0)
else:
ledsC.allLeftEye(0, 0, 0)
def doLeftEyeGoal():
goal = world_objects.getObjPtr(core.WO_UNKNOWN_GOAL)
soloPost = world_objects.getObjPtr(core.WO_UNKNOWN_GOALPOST)
if soloPost.seen:
ledsC.allTopLeftEye(0, 0, 1)
else:
ledsC.allTopLeftEye(0, 0, 0)
if goal.seen:
ledsC.allBottomLeftEye(0, 0, 1)
else:
ledsC.allBottomLeftEye(0, 0, 0)
def run(self):
commands.setHeadPanTilt(0.0, -10.0, 1.0)
robot = world_objects.getObjPtr(core.WO_TEAM5)
self.checkLocalized(robot)
if self.localized:
print("Localized")
self.postSignal("localized")
return
else:
print("Lost")
self.postSignal("lost")
return
def doPlayingEarLights():
"""Set ears for playing dependent on role."""
if (robot_state.role_ == core.CHASER):
ledsC.frontRightEar(1)
else:
ledsC.frontRightEar(0)
# I'd like not to send this every frame, rather
# only send when seen changes
if (world_objects.getObjPtr(core.WO_BALL).seen):
ledsC.frontLeftEar(1)
else:
ledsC.frontLeftEar(0)
# default to off
ledsC.backLeftEar(0)
ledsC.backRightEar(0)
# left arm bumper
if (processed_sonar.bump_left_):
ledsC.backLeftEar(1)
if (processed_sonar.bump_right_):
ledsC.backRightEar(1)
def setup(self):
ball = mem_objects.world_objects[core.WO_BALL]
robot = world_objects.getObjPtr(core.WO_TEAM5)
localized = False
poseListX = []
poseListY = []
poseListTh = []
pose_index = 0
blocker = Blocker()
lookStraight = MoveHead(0.0, -10.0, 2.5)
moveHeadLeft = MoveHead(85.0, -10.0, 2.5)
moveHeadRight = MoveHead(-85.0, -10.0, 5.0)
reset = Reset()
rdy = GetReady()
moveBtwBall = MoveBtwBall(localized, poseListX, poseListY, poseListTh,
pose_index)
checkLoc = CheckIfLocalized(localized, poseListX, poseListY,
poseListTh, pose_index)
blocks = {
"left": BlockLeft(),
"right": BlockRight(),
"center": BlockCenter(),
"miss": DontBlock()
}
locState = {"lost": moveHeadLeft, "localized": blocker}
self.add_transition(rdy, C, checkLoc)
for state in locState:
s = locState[state]
self.add_transition(checkLoc, S(state), s)
self.add_transition(moveBtwBall, S(state), s)
for name in blocks:
b = blocks[name]
self.add_transition(blocker, S(name), b, T(4.0), reset, T(3.0),
blocker)
self.add_transition(blocker, S("moveBall"), moveBtwBall)
self.add_transition(moveHeadLeft, C, moveHeadRight, C, checkLoc)
def __init__(self, localized, poseListX, poseListY, poseListTh,
pose_index):
super(MoveBtwBall, self).__init__()
self.goalieStartX = 0.0
self.goalieStartY = 0.0
self.localized = localized
self.initialized = False
self.radius = 300.0
self.roboDesiredX = 0.0
self.roboDesiredY = 0.0
self.roboDesiredTh = 0.0
self.localized = localized
self.poseListX = poseListX
self.poseListY = poseListY
self.pose_index = pose_index
self.k_x = (0.005, 0.0, 0.0)
self.k_y = (0.0005, 0.0, 0.0)
self.k_t = (0.7, 0.01, 0.1)
self.dir = 1.0
# integrals and previouse values for the PID controllers
self.x_int = 0.0
self.x_prev = 0.0
self.y_int = 0.0
self.y_prev = 0.0
self.theta_int = 0.0
self.theta_prev = 0.0
# Time for PID control
self.time_last = time.clock()
self.time_current = time.clock()
# Objects
self.gb_line_obj = mem_objects.world_objects[core.WO_OWN_PENALTY]
self.gb_line_seg = None
self.robot = world_objects.getObjPtr(core.WO_TEAM5)
self.ball = mem_objects.world_objects[core.WO_BALL]