def rush_goal():
print "rush goal"
mutex.acquire()
desiredPoint = param.AWAY_GOAL
while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.4)) or \
(team1_robot_state.pos_y_est > (desiredPoint.y + 0.3) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.3)):
if team1_robot_state.pos_x_est < ball.x:
print "break"
break
targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), desiredPoint)
radian180 = param.degreeToRadian(180)
radian360 = param.degreeToRadian(360)
radian5 = param.degreeToRadian(5)
anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180
angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL)
omega = angular_command.omega
if(anglediff <= radian5 and anglediff >= -radian5):
omega = 0
command = MotionSkills.go_to_point(team1_robot_state, desiredPoint)
velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est)
time.sleep(robot_update_delay)
# kick.kick()
velchange.goXYOmega(0,0,0)
def go_to_center():
mutex.acquire()
desiredPoint = param.CENTER
while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.05) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.05)) or \
(team1_robot_state.pos_y_est > (desiredPoint.y + 0.05) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.05)):
targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), desiredPoint)
radian180 = param.degreeToRadian(180)
radian360 = param.degreeToRadian(360)
radian5 = param.degreeToRadian(7)
anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180
angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL)
omega = angular_command.omega
if(anglediff <= radian5 and anglediff >= -radian5):
omega = 0
command = MotionSkills.go_to_point(team1_robot_state, desiredPoint)
velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega , team1_robot_state.pos_theta_est)
time.sleep(robot_update_delay)
velchange.goXYOmega(0,0,0)
def go_to_point_behind_ball():
desiredPoint = MotionSkills.getPointBehindBall(ball, param.AWAY_GOAL)
while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.13) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.13)) or \
(team1_robot_state.pos_y_est > (desiredPoint.y + 0.13) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.13)):
robot_point = Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est)
desiredPoint = MotionSkills.getPointBehindBall(ball, param.AWAY_GOAL)
targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), param.AWAY_GOAL)
radian180 = param.degreeToRadian(180)
radian360 = param.degreeToRadian(360)
radian5 = param.degreeToRadian(5)
anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180
command = MotionSkills.go_to_point(team1_robot_state, desiredPoint)
angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL)
omega = angular_command.omega
if(anglediff <= radian5 and anglediff >= -radian5):
omega = 0
velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est)
time.sleep(robot_update_delay)
angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL)
velchange.goXYOmega(0,0,angular_command.omega)
time.sleep(angular_command.runTime)
velchange.goXYOmega(0,0,0)
def defend_goal():
# vel_x = 0
# if float(ball_y) < 85 and float(ball_y) > -75:
# if state.pos_y_est > param.pixelToMeter(float(ball_y)):
# vel_x = -0.7
# else:
# vel_x = 0.7
# velchange.goXYOmega(0,vel_x,0)
# else:
# velchange.goXYOmega(0,0,0)
desiredPoint = Point.Point(param.HOME_GOAL.x - 0.42 , ball.y)
# keep robot within the bounds of the goal
if desiredPoint.y > param.HOME_GOAL.y + 0.3:
desiredPoint.y = param.HOME_GOAL.y + 0.3
elif desiredPoint.y < param.HOME_GOAL.y - 0.3:
desiredPoint.y = param.HOME_GOAL.y - 0.3
# move to the desiredPoint
while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.13) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.13)) or \
(team1_robot_state.pos_y_est > (desiredPoint.y + 0.13) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.13)):
desiredPoint = Point.Point(param.HOME_GOAL.x - 0.42 , ball.y)
# keep robot within the bounds of the goal
if desiredPoint.y > param.HOME_GOAL.y + 0.3:
desiredPoint.y = param.HOME_GOAL.y + 0.3
elif desiredPoint.y < param.HOME_GOAL.y - 0.3:
desiredPoint.y = param.HOME_GOAL.y - 0.3
targetAngle = MotionSkills.angleBetweenPoints(Point.Point(team1_robot_state.pos_x_est, team1_robot_state.pos_y_est), param.AWAY_GOAL)
radian180 = param.degreeToRadian(180)
radian360 = param.degreeToRadian(360)
radian5 = param.degreeToRadian(7)
anglediff = (team1_robot_state.pos_theta_est - targetAngle + radian180) % radian360 - radian180
angular_command = MotionSkills.go_to_angle(team1_robot_state, param.AWAY_GOAL)
omega = angular_command.omega
if(anglediff <= radian5 and anglediff >= -radian5):
omega = 0
command = MotionSkills.go_to_point(team1_robot_state, desiredPoint)
velchange.goXYOmegaTheta(command.vel_x, command.vel_y, omega, team1_robot_state.pos_theta_est)
time.sleep(goalie_update_delay)
velchange.goXYOmega(0,0,0)
def receive(threadName, *args):
# Prepare our context and publisher
context = zmq.Context()
subscriber = context.socket(zmq.SUB)
subscriber.connect("tcp://%s:%s" % (ip, port))
subscriber.setsockopt(zmq.SUBSCRIBE, b"A")
while True:
address, contents = subscriber.recv_multipart()
contentArray = contents.split()
home_robot_angle = contentArray[0]
home_robot_x = contentArray[1]
home_robot_y = contentArray[2]
away_robot_angle = contentArray[3]
away_robot_x = contentArray[4]
away_robot_y = contentArray[5]
ball_x = float(contentArray[6])
ball_y = float(contentArray[7])
if ball_y > 210:
ball_y = 210.0
elif ball_y < -210:
ball_y = -210.0
ball.x = param.pixelToMeter(ball_x)
ball.y = param.pixelToMeter(ball_y)
team1_robot_state.pos_theta_est = param.degreeToRadian(float(home_robot_angle))
team1_robot_state.pos_x_est = param.pixelToMeter(float(home_robot_x))
team1_robot_state.pos_y_est = param.pixelToMeter(float(home_robot_y))
team2_robot_state.pos_theta_est = param.degreeToRadian(float(away_robot_angle))
team2_robot_state.pos_x_est = param.pixelToMeter(float(away_robot_x))
team2_robot_state.pos_y_est = param.pixelToMeter(float(away_robot_y))
try:
mutex.release()
except:
pass
# We never get here but clean up anyhow
subscriber.close()
context.term()
def receive(threadName, *args):
# Prepare our context and publisher
context = zmq.Context()
subscriber = context.socket(zmq.SUB)
subscriber.connect("tcp://%s:%s" % (ip, port))
subscriber.setsockopt(zmq.SUBSCRIBE, b"A")
prev_home_robot_angle = 0.0
prev_home_robot_x = 0.0
prev_home_robot_y = 0.0
prev_ball_x = 0.0
prev_ball_y = 0.0
while True:
address, contents = subscriber.recv_multipart()
contentArray = contents.split()
# home robot
home_robot_angle = contentArray[0]
home_robot_x = ((1-a) * float(contentArray[1])) + (a * prev_home_robot_x)
home_robot_y = ((1-a) * float(contentArray[2])) + (a * prev_home_robot_y)
# away robot
# away_robot_angle = contentArray[3]
# away_robot_x = contentArray[4]
# away_robot_y = contentArray[5]
# ball
ball_x = ((1-a) * param.pixelToMeter(float(contentArray[6]))) + (a * prev_ball_x)
ball_y = ((1-a) * param.pixelToMeter(float(contentArray[7]))) + (a * prev_ball_y)
# set state
team1_robot_state.pos_theta_est = param.degreeToRadian(float(home_robot_angle))
team1_robot_state.pos_x_est = param.pixelToMeter(float(home_robot_x))
team1_robot_state.pos_y_est = param.pixelToMeter(float(home_robot_y))
# team2_robot_state.pos_theta_est = param.degreeToRadian(float(away_robot_angle))
# team2_robot_state.pos_x_est = param.pixelToMeter(float(away_robot_x))
# team2_robot_state.pos_y_est = param.pixelToMeter(float(away_robot_y))
ball.x = ball_x
ball.y = ball_y
#print ball_x, ball_y
# set previous values for Low Pass Filter
prev_home_robot_angle = float(home_robot_angle)
prev_home_robot_x = float(home_robot_x)
prev_home_robot_y = float(home_robot_y)
prev_ball_x = float(ball_x)
prev_ball_y = float(ball_y)
try:
mutex.release()
except:
pass
# We never get here but clean up anyhow
subscriber.close()
context.term()
