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()