def goToDesiredPos(self):
self.time_current = time.clock()
globX = self.robot.loc.x
globY = self.robot.loc.y
globTh = self.robot.orientation
dt = self.time_current - self.time_last
x = self.roboDesiredX
y = self.roboDesiredY
theta = self.roboDesiredTh
bearing = self.ball.bearing
self.calc_integral(dt, x, y, theta)
elevation = core.RAD_T_DEG * self.ball.visionElevation
commands.setHeadPanTilt(self.ball.bearing, -elevation, 1.5)
if dt == 0:
x_cont = self.k_x[0] * (globX - x) + self.k_x[1] * self.x_int
y_cont = self.k_y[0] * (globY - y) + self.k_y[1] * self.y_int
theta_cont = self.k_t[0] * bearing + self.k_t[1] * self.theta_int
else:
x_cont = self.k_x[0] * (globX - x) + self.k_x[
1] * self.x_int + self.k_x[2] * (globX - self.x_prev) / dt
y_cont = self.k_y[0] * (globY - y) + self.k_y[
1] * self.y_int + self.k_y[2] * (globY - self.y_prev) / dt
theta_cont = self.k_t[0] * bearing + self.k_t[
1] * self.theta_int + self.k_t[2] * (bearing -
self.theta_prev) / dt
print("p_cont: %f, i_cont: %f, d_cont: %f" %
(self.k_x[0] *
(globX - x), self.k_x[1] * self.x_int, self.k_x[2] *
(globX - self.x_prev) / dt))
if abs(bearing) >= 0.3:
# Control only the heading of the robot and not the velocity
commands.setWalkVelocity(0.0, 0.0, 0.4 * np.sign(bearing))
else:
# Control both heading and velocity
if self.gb_line_obj.seen:
if globX < 1800.0:
self.gb_line_seg = self.gb_line_obj.lineLoc
gb_line_cent = self.gb_line_seg.getCenter()
rel_robot = core.Point2D(0.0, 0.0)
gb_dist_thresh = 200.0
gb_robo_dist = self.gb_line_seg.getDistanceTo(rel_robot)
# print("Line seen. Center at [%f, %f], robot at [%f, %f] dist to closest point is: %f" % (gb_line_cent.x,gb_line_cent.y,self.robot.loc.x,self.robot.loc.y,gb_robo_dist))
if gb_robo_dist < gb_dist_thresh:
# Too close in either x or y
x_cont = 0.0
# else:
# print("Line not seen.\n")
commands.setWalkVelocity(x_cont, y_cont, theta_cont)
print("Controls: [%f, %f, %f]\n" % (x_cont, y_cont, theta_cont))
self.x_prev = globX - self.x_prev
self.y_prev = globY - self.y_prev
self.theta_prev = bearing
self.time_last = self.time_current
return
def run(self):
# if not ball.seen, move the head back to the center
commands.setWalkVelocity(0.0, 0.0, 0.0)
ball = mem_objects.world_objects[core.WO_BALL]
line_obj = mem_objects.world_objects[core.WO_OWN_PENALTY]
line = line_obj.lineLoc
if not ball.seen:
commands.setHeadPan(0.0, 0.2)
else:
commands.setHeadPan(ball.bearing, 0.2)
# print("Ball distance: %f Ball bearing: %f\n" % (ball.distance,ball.bearing))
v_mag = math.sqrt(ball.relVel.x * ball.relVel.x +
ball.relVel.y * ball.relVel.y)
x_ball = ball.distance * math.cos(ball.bearing) / 10.0
y_ball = ball.distance * math.sin(ball.bearing) / 10.0
delta_t = 2.0
v_thresh = -(x_ball / delta_t)
# print("X: %f, Y: %f, VThresh: %f, VMag: %f, Vx: %f, Vy: %f" % (x_ball, y_ball, v_thresh, v_mag, ball.relVel.x, ball.relVel.y))
if ball.seen and ball.relVel.x < 0.0:
if ball.relVel.x > v_thresh:
"""do nothing"""
pass
else:
# ball moving away from the robot, reset to regular position
y_intercept = -ball.relVel.y * x_ball / ball.relVel.x + y_ball
print(
"Current Vx: %f, Current Vy: %f, x ball: %f, y ball: %f" %
(ball.relVel.x, ball.relVel.y, x_ball, y_ball))
print("Y intercept: %f" % y_intercept)
if y_intercept > 25.0 or y_intercept < -25.0:
rel_robot = core.Point2D(0.0, 0.0)
distance = line.getDistanceTo(rel_robot)
if line_obj.seen and distance < 100.0:
x_cont = 0.0
else:
x_cont = 0.2
commands.setWalkVelocity(x_cont,
0.4 * np.sign(y_intercept), 0.0)
return
elif y_intercept > 18.0:
choice = "left"
elif y_intercept < -18.0:
choice = "right"
elif y_intercept <= 18.0 and y_intercept >= -18.0:
choice = "center"
self.postSignal(choice)
return
def run(self):
# Localization information
robot = world_objects.getObjPtr(robot_state.WO_SELF)
center = core.Point2D(0, 0)
angleToCenter = robot.loc.getBearingTo(center, robot.orientation)
distanceToCenter = robot.loc.getDistanceTo(center)
# Walk Commands to reach center
xVel = 0.0
yVel = 0.0
tVel = 0.0
if not self.omniWalk:
if abs(angleToCenter) > 15 * core.DEG_T_RAD:
tVel = 0.5 * (angleToCenter / abs(angleToCenter))
# else:
# yVel = 0.2 * (angleToCenter / abs(angleToCenter))
if abs(angleToCenter) > 30 * core.DEG_T_RAD:
xVel = 0
elif distanceToCenter > 100:
xVel = max(min((distanceToCenter + 100) / 300.0, 0.5),
-0.5)
if distanceToCenter < 600 and tVel > 0.0:
xVel = 0.0
elif distanceToCenter < 200:
relCenter = center.globalToRelative(
robot.loc, robot.orientation)
xVel = max(min((relCenter.x / 300.0), 0.5), -0.5)
tVel = 0.0
elif distanceToCenter < 100:
xVel = 0
tVel = 0
else:
relCenter = center.globalToRelative(robot.loc,
robot.orientation)
xVel = max(min((relCenter.x + 0) / 300.0, 0.5), -0.5)
yVel = max(min((relCenter.y) / 200.0, 0.5), -0.5)
if self.omniWalk and distanceToCenter > 800:
self.changeCt += 1
elif self.omniWalk:
self.changeCt = max(self.changeCt - 1, 0)
if not self.omniWalk and distanceToCenter < 600:
self.changeCt += 1
elif not self.omniWalk:
self.changeCt = max(self.changeCt - 1, 0)
xVel = 0.6 * xVel + self.prevX * 0.4
yVel = 0.3 * yVel + self.prevY * 0.7
tVel = 0.3 * tVel + self.prevT * 0.7
commands.setWalkVelocity(xVel, yVel, tVel)
self.prevX = xVel
self.prevY = yVel
self.prevT = tVel
print "Position {0}, {1}, {2} and velocity: {3}, {4}, {5}".format(
robot.loc.x, robot.loc.y, robot.orientation, xVel, yVel, tVel)
# Update how long its been since we saw a beacon
seen = False
for b in self.beacons:
beacon = world_objects.getObjPtr(b)
if beacon.seen:
seen = True
print self.names[
self.beacons.index(b)] + " {0} and {1}".format(
beacon.visionDistance, beacon.visionBearing)
break
# If we haven't seen a beacon in a while we should stop and rotate
if seen:
self.lastObsCount = max(self.lastObsCount - 2, 0)
else:
self.lastObsCount += 1
if self.lastObsCount > 150: # 5 seconds without seeing a beacon
self.lastObsCount = 0
self.finish()
# move head
commands.setHeadPan(self.direction * 50 * core.DEG_T_RAD, 0.5)
if abs(core.joint_values[core.HeadYaw]) > 45 * core.DEG_T_RAD:
if core.joint_values[core.HeadYaw] > 0:
self.direction = -1
else:
self.direction = 1
if self.changeCt > 10:
self.changeCt = 0
self.prevX = 0.0
self.prevY = 0.0
self.prevT = 0.0
if distanceToCenter < 100:
self.doneCt += 1
else:
self.doneCt = max(self.doneCt - 1, 0)
if self.doneCt > 5:
self.finish()
def calculateSlopeFromPointAngle(pt, angle):
pt2 = pt + core.Point2D(100, angle, core.POLAR)
return calculateSlope(pt, pt2)
def ready(self):
rel_robot = core.Point2D(0.0, 0.0)
distance = self.obj.lineLoc.getDistanceTo(rel_robot)
print("LineDistance: %f" % (distance))
return (self.obj.seen and (distance < self.dist))
