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