Ejemplo n.º 1
0
def stop_move(TH_goal_info,OUR_goal_info,robot_info,devide,ball_info,geometry,message):
	message.send.append(attacker.stop_posi(TH_goal_info,robot_info[devide.attacker],ball_info,0.5))

	target_position=Pose2D(-geometry.field_length/2,0,0)
	goalie_target_info=calculate.Calc(robot_info[devide.goalie].pose,target_position)
	goalie_target_degree=goalie_target_info.degree_calc() -  math.degrees(robot_info[devide.goalie].pose.theta) +180
	goalie_target_distance=goalie_target_info.distance_calc()
	goalie_target_degree=calculate.degree_improve(goalie_target_degree)
	if goalie_target_distance>=1:
		goalie_target_distance=1

	send_val=SEND()
	send_val.robot_id=robot_info[devide.goalie].robot_id
	send_val.distance=goalie_target_distance
	send_val.move_degree=math.radians(goalie_target_degree)
	send_val.direction=-robot_info[devide.goalie].pose.theta
	send_val.stop=False

	message.send.append(send_val)

	for offense_id in devide.offense:
		offense_ball_info=calculate.Calc(robot_info[offense_id].pose,ball_info.pose)
		offense_ball_degree=offense_ball_info.degree_calc() - math.degrees(robot_info[offense_id].pose.theta) +180
		offense_ball_degree=calculate.degree_improve(offense_ball_degree)

		send_val=SEND()
		send_val.robot_id=robot_info[offense_id].robot_id
		send_val.distance=0.0
		send_val.move_degree=0.0
		send_val.direction=-robot_info[offense_id].pose.theta
		send_val.stop=False
		send_val.spinner=False
		send_val.kickspeedx=0.0
		
		message.send.append(send_val)

	for defense_id in devide.defense:
		defense_ball_info=calculate.Calc(robot_info[defense_id].pose,ball_info.pose)
		defense_ball_degree=defense_ball_info.degree_calc() - math.degrees(robot_info[defense_id].pose.theta) +180
		defense_ball_degree=calculate.degree_improve(defense_ball_degree)

		send_val=SEND()
		send_val.robot_id=robot_info[defense_id].robot_id
		send_val.distance=0.0
		send_val.move_degree=0.0
		send_val.direction=math.radians(defense_ball_degree)
		send_val.stop=False
		send_val.spinner=False
		send_val.kickspeedx=0.0

		message.send.append(send_val)
Ejemplo n.º 2
0
def main(TH_goal_info, attacker, offense, ball_info, geometry):
    send_val = SEND()

    AT_TH_info = calculate.Calc(attacker.pose, TH_goal_info)
    AT_TH_distance = AT_TH_info.distance_calc()
    AT_TH_degree = AT_TH_info.degree_calc()
    AT_TH_degree = calculate.degree_improve(AT_TH_degree)

    AT_OFF_info = calculate.Calc(attacker.pose, offense.pose)
    AT_OFF_degree = AT_OFF_info.degree_calc() - math.degrees(
        offense.pose.theta)
    AT_OFF_distance = AT_OFF_info.distance_calc()
    AT_OFF_degree = calculate.degree_improve(AT_OFF_degree)

    target_position = Pose2D(
        attacker.pose.x + math.cos(attacker.pose.theta) * AT_TH_distance / 2,
        attacker.pose.y + math.sin(attacker.pose.theta) * AT_TH_distance / 2,
        0)
    target_robot_info = calculate.Calc(target_position, offense.pose)
    target_robot_distance = target_robot_info.distance_calc()
    target_robot_degree = target_robot_info.degree_calc() - math.degrees(
        offense.pose.theta)
    target_robot_degree = calculate.degree_improve(target_robot_degree)

    send_val.stop = False
    send_val.direction = math.radians(AT_OFF_degree)
    send_val.move_degree = math.radians(target_robot_degree)
    send_val.spinner = False
    send_val.kickspeedx = 0
    send_val.distance = target_robot_distance
    if send_val.distance >= 1:
        send_val.distance = 1
    send_val.robot_id = offense.robot_id

    return send_val
Ejemplo n.º 3
0
def main(our_goal_info, defense, ball_info, geometry):
    send_val = SEND()

    penalty_length = 0
    target_position = Pose2D()
    for line_field in geometry.line_field:
        if line_field.name == "LeftPenaltyStretch":
            penalty_length = line_field.p2_x
            target_position = Pose2D(penalty_length, ball_info.pose.y, 0)
            if defense.pose.y >= math.fabs(line_field.p1_y):
                target_position.y = math.fabs(line_field.p1_y)
            elif defense.pose.y <= -math.fabs(line_field.p1_y):
                target_position.y = -math.fabs(line_field.p1_y)

    robot_target_info = calculate.Calc(target_position, defense.pose)
    robot_target_degree = robot_target_info.degree_calc() - math.degrees(
        defense.pose.theta)

    robot_ball_info = calculate.Calc(defense.pose, ball_info.pose)
    robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
        defense.pose.theta) + 180

    robot_target_degree = calculate.degree_improve(robot_target_degree)
    robot_ball_degree = calculate.degree_improve(robot_ball_degree)
    #print(penalty_length)
    send_val.stop = False
    send_val.direction = -defense.pose.theta
    send_val.move_degree = math.radians(robot_target_degree)
    send_val.spinner = True
    send_val.kickspeedx = 0.0
    send_val.distance = math.fabs(math.radians(robot_ball_degree))
    send_val.robot_id = defense.robot_id

    return send_val
Ejemplo n.º 4
0
def halt():
    send_val = SEND()

    send_val.stop = True
    send_val.direction = 0
    send_val.move_degree = -1
    send_val.spinner = False
    send_val.kickspeedx = 0

    return send_val
Ejemplo n.º 5
0
def normal_position(attacker, offense, devide, ball_info, geometry, count):
    target_position = Pose2D()
    if len(devide.offense) == 4:
        if count == 1:
            target_position = Pose2D(0, 0, 0)
        elif count == 2:
            target_position = Pose2D(4.5, 3.5, 0)
        elif count == 3:
            target_position = Pose2D(4.5, -3.5, 0)
        elif count == 4:
            target_position = Pose2D(3.5, 0, 0)
    elif len(devide.offense) == 3:
        if count == 1:
            target_position = Pose2D(0, 0, 0)
        elif count == 2:
            target_position = Pose2D(3.5, 3.5, 0)
        elif count == 3:
            target_position = Pose2D(3.5, -3.5, 0)
    elif len(devide.offense) == 2:
        if count == 1:
            target_position = Pose2D(0, 0, 0)
        elif count == 2:
            target_position = Pose2D(3.5, 3.5, 0)
    elif len(devide.offense) == 1:
        if count == 1:
            target_position = Pose2D(2.5, 3, 0)

    target_robot_info = calculate.Calc(target_position, offense.pose)
    target_robot_degree = target_robot_info.degree_calc() - math.degrees(
        offense.pose.theta) + 180
    target_robot_distance = target_robot_info.distance_calc()
    target_robot_degree = calculate.degree_improve(target_robot_degree)

    robot_ball_info = calculate.Calc(offense.pose, ball_info.pose)
    robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
        offense.pose.theta) + 180
    robot_ball_distance = robot_ball_info.distance_calc()
    robot_ball_degree = calculate.degree_improve(robot_ball_degree)

    send_val = SEND()
    send_val.stop = False
    send_val.direction = math.radians(robot_ball_degree)
    send_val.move_degree = math.radians(target_robot_degree)
    send_val.spinner = False
    send_val.kickspeedx = 0.0
    send_val.distance = target_robot_distance
    if send_val.distance >= 1:
        send_val.distance = 1
    send_val.robot_id = offense.robot_id

    return send_val
Ejemplo n.º 6
0
def stop_posi(TH_goal_info, attacker, ball_info, DISTANCE):
    send_val = SEND()

    DISTANCE_SPACE = Pose2D(
        math.cos(attacker.pose.theta) * DISTANCE,
        math.sin(attacker.pose.theta) * DISTANCE, 0)

    target_posi = Pose2D(ball_info.pose.x - DISTANCE_SPACE.x,
                         ball_info.pose.y - DISTANCE_SPACE.y, 0)
    TH_goal_robot = calculate.Calc(TH_goal_info, attacker.pose)
    TH_goal_robot_degree = TH_goal_robot.degree_calc(
    ) - attacker.pose.theta * 180 / 3.14
    target_robot = calculate.Calc(target_posi, attacker.pose)
    target_robot_distance = target_robot.distance_calc()
    robot_ball_info = calculate.Calc(attacker.pose, target_posi)
    robot_ball_degree = robot_ball_info.degree_calc(
    ) - attacker.pose.theta * 180 / 3.14
    wrap_degree = robot_ball_info.wrap_calc(math.degrees(attacker.pose.theta))
    robot_ball_distance = robot_ball_info.distance_calc()

    if target_robot_distance >= 1:
        target_robot_distance = 1

    wrap_degree = calculate.degree_improve(wrap_degree)

    robot_ball_degree = calculate.degree_improve(robot_ball_degree)

    ball_detect = False
    if robot_ball_degree <= 10 and robot_ball_degree >= -10:
        ball_detect = True

    spinner = False
    if robot_ball_distance < 0.4 and ball_detect == True:
        spinner = True

    kick_pow_x = 0
    if TH_goal_robot_degree >= -10 and TH_goal_robot_degree <= 10:
        kick_pow_x = 0.0

    send_val.stop = False
    send_val.direction = math.radians(TH_goal_robot_degree)
    send_val.move_degree = math.radians(wrap_degree)
    send_val.spinner = True
    send_val.kickspeedx = kick_pow_x
    send_val.distance = target_robot_distance
    send_val.robot_id = attacker.robot_id

    return send_val
Ejemplo n.º 7
0
def main(our_goal_info,goalie,ball_info,geometry):
	send_val=SEND()

	OUR_goal_position_middle = Pose2D(-geometry.field_length/2,0,0)
	target_position=Pose2D(OUR_goal_position_middle.x+0.2,ball_info.pose.y,0)

	flag=False
	if goalie.pose.y>=geometry.goal_width/2:
		target_position.y=geometry.goal_width/2
		flag=True
	
	if goalie.pose.y<=-geometry.goal_width/2:
		target_position.y=-geometry.goal_width/2
		flag=True

	OUR_goal_robot=calculate.Calc(our_goal_info,goalie.pose)
	OUR_goal_robot_degree=OUR_goal_robot.degree_calc() - math.degrees(goalie.pose.theta) +180
	OUR_goal_robot_distance=OUR_goal_robot.distance_calc()

	robot_ball_info=calculate.Calc(goalie.pose,ball_info.pose)
	robot_ball_degree=robot_ball_info.degree_calc() - math.degrees(goalie.pose.theta) +180
	robot_ball_distance=robot_ball_info.distance_calc()

	robot_ball_degree = calculate.degree_improve(robot_ball_degree)
	OUR_goal_robot_degree = calculate.degree_improve(OUR_goal_robot_degree)

	target_info=calculate.Calc(target_position,goalie.pose)
	target_degree=target_info.degree_calc() - goalie.pose.theta*180/3.14 
	target_degree=calculate.degree_improve(target_degree)

	kick_pow_x=0.0
	#target_degree=

	send_val.stop=False
	send_val.direction=math.degrees(goalie.pose.theta)
	send_val.direction=calculate.degree_improve(send_val.direction)
	send_val.direction=math.radians(-send_val.direction)
	send_val.move_degree=math.radians(target_degree)
	send_val.spinner=True
	send_val.kickspeedx=OUR_goal_robot_distance
	send_val.distance=math.fabs(math.radians(robot_ball_degree))
	if flag==True:
		send_val.distance=0.1
	send_val.robot_id=goalie.robot_id

	return send_val
Ejemplo n.º 8
0
def move_straight(TH_goal_info, offense, ball_info):
    send_val = SEND()

    TH_robot_info = calculate.Calc(TH_goal_info, offense.pose)
    TH_robot_degree = TH_robot_info.degree_calc() - math.degrees(
        offense.pose.theta)
    TH_robot_distance = TH_robot_info.distance_calc()
    TH_robot_degree = calculate.degree_improve(TH_robot_degree)

    robot_ball_info = calculate.Calc(offense.pose, ball_info.pose)
    robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
        offense.pose.theta) + 180
    robot_ball_distance = robot_ball_info.distance_calc()
    robot_ball_degree = calculate.degree_improve(robot_ball_degree)

    ball_detect = False
    if robot_ball_degree <= 3 and robot_ball_degree >= -3:
        ball_detect = True

    spinner = False
    if robot_ball_distance < 0.12 and ball_detect == True:
        spinner = True

    if spinner == True and ball_detect == True:
        direction = TH_robot_degree
    else:
        direction = robot_ball_degree

    kick_pow_x = 0
    if TH_robot_degree >= -5 and TH_robot_degree <= 5:
        kick_pow_x = 10.0

    send_val.stop = False
    send_val.direction = math.radians(direction)
    send_val.move_degree = math.radians(robot_ball_degree)
    send_val.spinner = True
    send_val.kickspeedx = kick_pow_x
    send_val.distance = robot_ball_distance

    if send_val.distance >= 1:
        send_val.distance = 1
    send_val.robot_id = attacker.robot_id

    return send_val
Ejemplo n.º 9
0
def main(TH_goal_info, attacker, ball_info):
    send_val = SEND()

    TH_goal_robot = calculate.Calc(TH_goal_info, attacker.pose)
    TH_goal_robot_degree = TH_goal_robot.degree_calc(
    ) - attacker.pose.theta * 180 / 3.14

    robot_ball_info = calculate.Calc(attacker.pose, ball_info.pose)
    robot_ball_degree = robot_ball_info.degree_calc(
    ) - attacker.pose.theta * 180 / 3.14
    wrap_degree = robot_ball_info.wrap_calc(math.degrees(attacker.pose.theta))
    robot_ball_distance = robot_ball_info.distance_calc()

    calculate.degree_improve(wrap_degree)
    calculate.degree_improve(robot_ball_degree)

    ball_detect = False
    if robot_ball_degree <= 10 and robot_ball_degree >= -10:
        ball_detect = True

    spinner = False
    if robot_ball_distance < 0.4 and ball_detect == True:
        spinner = True

    kick_pow_x = 0
    if TH_goal_robot_degree >= -10 and TH_goal_robot_degree <= 10:
        kick_pow_x = 6.0

    send_val.direction = math.radians(TH_goal_robot_degree)
    send_val.move_degree = math.radians(wrap_degree)
    send_val.spinner = spinner
    send_val.distance = 0.85
    send_val.kickspeedx = kick_pow_x
    send_val.robot_id = attacker.robot_id
    send_val.stop = True
    return send_val
Ejemplo n.º 10
0
def pass_move(TH_goal_info, attacker, offense, ball_info, geometry, target,
              flag):
    send_val = SEND()

    TARGET_POSITION_X = 3.5
    TARGET_POSITION_Y = -3.5

    AT_TH_info = calculate.Calc(attacker.pose, TH_goal_info)
    AT_TH_distance = AT_TH_info.distance_calc()
    AT_TH_degree = AT_TH_info.degree_calc()

    AT_OFF_info = calculate.Calc(attacker.pose, offense.pose)
    AT_OFF_distance = AT_OFF_info.distance_calc()
    AT_OFF_degree = AT_OFF_info.degree_calc() - math.degrees(
        offense.pose.theta)

    robot_ball_info = calculate.Calc(offense.pose, ball_info.pose)
    robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
        offense.pose.theta) + 180
    robot_ball_degree = calculate.degree_improve(robot_ball_degree)
    robot_ball_distance = robot_ball_info.distance_calc()

    y = flag.slope * target.x + flag.intercept

    if flag.kick_flag == True:
        target_position = Pose2D(target.x, y, 0)
    else:
        target_position = Pose2D(target.x, target.y, 0)

    if flag.different_place == True:
        target_position = Pose2D(target.x, target.y, 0)

    target_position = position.line_limit(target_position, geometry)

    flag.arrive[flag.count_offense] = False
    if (target.x - 0.1) <= offense.pose.x and (target.x +
                                               0.1) >= offense.pose.x:
        if (target.y - 0.1) <= offense.pose.y and (target.y +
                                                   0.1) >= offense.pose.y:
            flag.arrive[flag.count_offense] = True

    target_robot_info = calculate.Calc(target_position, offense.pose)
    target_robot_distance = target_robot_info.distance_calc()
    target_robot_degree = target_robot_info.degree_calc() - math.degrees(
        offense.pose.theta)
    target_robot_degree = calculate.degree_improve(target_robot_degree)

    TH_robot_info = calculate.Calc(TH_goal_info, offense.pose)
    TH_robot_degree = TH_robot_info.degree_calc() - math.degrees(
        offense.pose.theta)
    TH_robot_distance = TH_robot_info.distance_calc()

    ball_detect = False
    if robot_ball_degree <= 10 and robot_ball_degree >= -10:
        ball_detect = True

    spinner = False
    if robot_ball_distance < 0.15 and ball_detect == True:
        spinner = True

    if spinner == True and ball_detect == True:
        direction = TH_robot_degree
    else:
        direction = robot_ball_degree

    kick_pow_x = 0
    if TH_robot_degree >= -5 and TH_robot_degree <= 5:
        kick_pow_x = 5.0

    if robot_ball_distance < 0.15:
        flag.kick_flag = False

    send_val.stop = False
    send_val.direction = math.radians(direction)
    send_val.move_degree = math.radians(target_robot_degree)
    send_val.spinner = spinner
    send_val.kickspeedx = kick_pow_x
    send_val.distance = target_robot_distance * 6
    if send_val.distance >= 1:
        send_val.distance = 1
    send_val.robot_id = offense.robot_id

    return send_val
Ejemplo n.º 11
0
def main(TH_goal_info,OUR_goal_info,robot_info,devide,ball_info,geometry,message):
	message.send.append(attacker.stop_posi(TH_goal_info,robot_info[devide.attacker],ball_info,0.12))

	target_position=Pose2D(-geometry.field_length/2,0,0)
	goalie_target_info=calculate.Calc(robot_info[devide.goalie].pose,target_position)
	goalie_target_degree=goalie_target_info.degree_calc() -  math.degrees(robot_info[devide.goalie].pose.theta) +180
	goalie_target_distance=goalie_target_info.distance_calc()
	goalie_target_degree=calculate.degree_improve(goalie_target_degree)
	if goalie_target_distance>=1:
		goalie_target_distance=1

	send_val=SEND()
	send_val.robot_id=robot_info[devide.goalie].robot_id
	send_val.distance=goalie_target_distance
	send_val.move_degree=math.radians(goalie_target_degree)
	send_val.direction=-robot_info[devide.goalie].pose.theta
	send_val.stop=False
	message.send.append(send_val)

	count=-1
	for offense_id in devide.offense:
		count=count+1
		target_position=calculate.kick_offense_posiiton_calc(devide.offense,geometry.field_width)
	
		offense_target_info=calculate.Calc(robot_info[offense_id].pose,target_position[count])
		offense_target_distance=offense_target_info.distance_calc()
		offense_target_degree=offense_target_info.degree_calc() -  math.degrees(robot_info[offense_id].pose.theta) +180
		offense_target_degree=calculate.degree_improve(offense_target_degree)
		if offense_target_distance>=1:
			offense_target_distance=1
		send_val=SEND()
		send_val.robot_id=robot_info[offense_id].robot_id
		send_val.distance=offense_target_distance
		send_val.move_degree=math.radians(offense_target_degree)
		send_val.stop=False
		send_val.direction=-robot_info[offense_id].pose.theta
		send_val.spinner=True
		send_val.kickspeedx=0.0

		message.send.append(send_val)

	count=-1
	for defense_id in devide.defense:
		count=count+1
		num=calculate.defense_position_calc(devide.defense,geometry.field_width)
		if len(num)>count:
			target_position=Pose2D(-4.5,num[count],0)
		defense_target_info=calculate.Calc(robot_info[defense_id].pose,target_position)
		defense_target_distance=defense_target_info.distance_calc()
		defense_target_degree=defense_target_info.degree_calc() - math.degrees(robot_info[defense_id].pose.theta) +180
		defense_target_degree=calculate.degree_improve(defense_target_degree)
		if defense_target_distance>=1:
			defense_target_distance=1
		send_val=SEND()
		send_val.robot_id=robot_info[defense_id].robot_id
		send_val.distance=defense_target_distance
		send_val.move_degree=math.radians(defense_target_degree)
		send_val.direction=-robot_info[defense_id].pose.theta
		send_val.stop=False
		send_val.spinner=True
		send_val.kickspeedx=0.0

		message.send.append(send_val)
Ejemplo n.º 12
0
def pass_move(TH_goal_info, attacker, ball_info, flag, target_position):
    send_val = SEND()

    TARGET_POSITION_X = 3.5
    TARGET_POSITION_Y = -3.5
    TH_robot_info = calculate.Calc(TH_goal_info, attacker.pose)
    TH_robot_degree = TH_robot_info.degree_calc() - math.degrees(
        attacker.pose.theta)
    TH_robot_distance = TH_robot_info.distance_calc()
    TH_robot_degree = calculate.degree_improve(TH_robot_degree)

    robot_ball_info = calculate.Calc(attacker.pose, ball_info.pose)
    robot_ball_degree = robot_ball_info.degree_calc() - math.degrees(
        attacker.pose.theta) + 180
    robot_ball_degree = calculate.degree_improve(robot_ball_degree)
    robot_ball_distance = robot_ball_info.distance_calc()

    #target_position=Pose2D(TARGET_POSITION_X, TARGET_POSITION_Y,0)
    target_robot_info = calculate.Calc(target_position, attacker.pose)
    target_robot_degree = target_robot_info.degree_calc() - math.degrees(
        attacker.pose.theta)
    target_robot_degree = calculate.degree_improve(target_robot_degree)
    target_robot_distance = target_robot_info.distance_calc()

    #print(target_robot_degree)
    ball_detect = False
    if robot_ball_degree <= 5 and robot_ball_degree >= -5:
        ball_detect = True

    spinner = False
    if ball_detect == True and robot_ball_distance < 0.12:
        spinner = True

    kick_pow_x = 0
    direction = robot_ball_degree
    if spinner == True and ball_detect == True:
        direction = target_robot_degree
        if target_robot_degree >= -5 and target_robot_degree <= 5 and not target_position.x == 100:
            kick_pow_x = 5
            flag.flag_temp = True

    if flag.flag_temp == True:
        if robot_ball_distance > 0.2:
            flag.flag_temp = False
            flag.kick_flag = True
            flag.robot_id_rem = attacker.robot_id

    if not attacker.robot_id == flag.robot_id_rem and robot_ball_distance < 0.3:
        flag.kick_flag = False

    send_val.stop = False
    send_val.direction = math.radians(direction)
    send_val.move_degree = math.radians(robot_ball_degree)
    send_val.spinner = spinner
    send_val.kickspeedx = kick_pow_x
    send_val.distance = robot_ball_distance
    send_val.robot_id = attacker.robot_id

    if send_val.distance >= 1:
        send_val.distance = 1

    return send_val