def goal_scored():
if(ball.x < -param.pixelToMeter(310)):
return True
if(ball.x > param.pixelToMeter(335)):
return True
return False
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()
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()
# print contentArray
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])
# keep the robot in vision
if ball_y > 170:
ball_y = 170.0
elif ball_y < -170:
ball_y = -170.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 go_prepare_spin():
mutex.acquire()
desiredPoint = Point.Point(param.pixelToMeter(90), 0)
while (team1_robot_state.pos_x_est > (desiredPoint.x + 0.1) or team1_robot_state.pos_x_est < (desiredPoint.x - 0.1)) or \
(team1_robot_state.pos_y_est > (desiredPoint.y + 0.1) or team1_robot_state.pos_y_est < (desiredPoint.y - 0.1)):
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
velchange.goXYOmegaTheta(command.vel_x, command.vel_y, 2.0, 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 one_on_one():
mutex.acquire()
state = "defend"
start = default_timer()
ball_prev_x = ball.x
ball_prev_y = ball.y
while True:
if state == "defend":
print "state: defend"
if ball.x < 0:
# transition
state = "score"
else:
# save state
ball_prev_x = ball.x
ball_prev_y = ball.y
# restart timer
start = default_timer()
# transition
state = "defend_goal"
elif state == "defend_goal":
print "state: defend_goal"
if ball.x < 0:
# transition
state = "score"
continue
if abs(ball.x - ball_prev_x) < 0.02 and abs(ball.y - ball_prev_y) < 0.02:
if default_timer() - start > 0.5:
# transition
state = "score"
else:
defend_goal()
else:
start = default_timer()
ball_prev_x = ball.x
ball_prev_y = ball.y
defend_goal()
elif state == "score":
print "state: score"
# check if a goal was scored
if(ball.x < -param.pixelToMeter(330)):
print "goal scored", ball.x
# transition
state = "victory"
continue
go_to_point_behind_ball()
# check if a goal was scored
if(ball.x < -param.pixelToMeter(330)):
print "goal scored", ball.x
# transition
state = "victory"
continue
rush_goal()
# check if a goal was scored
if(ball.x < -param.pixelToMeter(330)):
print "goal scored", ball.x
# transition
state = "victory"
else:
#transition
state = "defend"
elif state == "victory":
print "state: victory"
go_home_spin()
break
