def main(): host = sys.argv[1] if len(sys.argv) > 1 else 'localhost' port = int(sys.argv[2]) if len(sys.argv) > 2 else 1024 if port == 1024: trainingDataFile = open('soccer_blue.lrn', 'w') elif port == 1025: trainingDataFile = open('soccer_yellow.lrn', 'w') sc = SoccerClient() sc.connect(host, port) #conjuntos fuzzy de distância da bola ballVeryClose = fuzzyLeftTrapezoid(50,90,0,1500) ballClose = fuzzyTrapezoid(80,150,250,400,0,1500) ballFar = fuzzyRightTrapezoid(370,500,0,1500) #conjuntos fuzzy do ângulo em relação à bola ballLeftBack = fuzzyLeftTrapezoid(-math.pi, -math.pi/2, -math.pi, math.pi) ballLeftFront = fuzzyLambda(-3*math.pi/4, -math.pi/2, 0, -math.pi, math.pi) ballForward = fuzzyLambda(-math.pi/4, 0, math.pi/4, -math.pi, math.pi) ballRightFront = fuzzyLambda(0, math.pi/2, 3*math.pi/4, -math.pi, math.pi) ballRightBack = fuzzyRightTrapezoid(math.pi/2, math.pi, -math.pi, math.pi) #conjuntos fuzzy do ângulo em relação ao gol targetLeftBack = fuzzyLeftTrapezoid(-math.pi, -math.pi/2, -math.pi, math.pi) targetLeftFront = fuzzyLambda(-3*math.pi/4, -math.pi/2, 0, -math.pi, math.pi) targetForward = fuzzyLambda(-math.pi/2, 0, math.pi/2, -math.pi, math.pi) targetRightFront = fuzzyLambda(0, math.pi/2, 3*math.pi/4, -math.pi, math.pi) targetRightBack = fuzzyRightTrapezoid(math.pi/2, math.pi, -math.pi, math.pi) #conjuntos fuzzy do ângulo do robô(saída) robotLeftBack = fuzzyLambda(- math.pi, -3*math.pi/4, -math.pi/2, -math.pi, math.pi) robotLeftFront = fuzzyLambda(-3*math.pi/4, -math.pi/2, 0, -math.pi, math.pi) robotForward = fuzzyLambda(-math.pi/2, 0, math.pi/2, -math.pi, math.pi) robotRightFront = fuzzyLambda(0, math.pi/2, 3*math.pi/4, -math.pi, math.pi) robotRightBack = fuzzyLambda(math.pi/2, 3*math.pi/4, math.pi, -math.pi, math.pi) rulesMatrix = numpy.zeros((3,5,5), dtype=object) # Make a 3 by 5 by 5 rules matrix #[distância, angulo bola, angulo gol] #very close rulesMatrix[0][0][0] = robotLeftBack rulesMatrix[0][0][1] = robotLeftFront rulesMatrix[0][0][2] = robotLeftFront rulesMatrix[0][0][3] = robotLeftFront rulesMatrix[0][0][4] = robotLeftFront rulesMatrix[0][1][0] = robotLeftBack rulesMatrix[0][1][1] = robotLeftFront rulesMatrix[0][1][2] = robotLeftBack rulesMatrix[0][1][3] = robotLeftBack rulesMatrix[0][1][4] = robotLeftBack rulesMatrix[0][2][0] = robotLeftBack rulesMatrix[0][2][1] = robotLeftFront rulesMatrix[0][2][2] = robotForward rulesMatrix[0][2][3] = robotRightFront rulesMatrix[0][2][4] = robotRightBack rulesMatrix[0][3][0] = robotRightBack rulesMatrix[0][3][1] = robotRightBack rulesMatrix[0][3][2] = robotRightBack rulesMatrix[0][3][3] = robotRightFront rulesMatrix[0][3][4] = robotRightBack rulesMatrix[0][4][0] = robotRightFront rulesMatrix[0][4][1] = robotRightFront rulesMatrix[0][4][2] = robotRightFront rulesMatrix[0][4][3] = robotRightFront rulesMatrix[0][4][4] = robotRightBack #close rulesMatrix[1][0][0] = robotLeftBack rulesMatrix[1][0][1] = robotLeftBack rulesMatrix[1][0][2] = robotLeftBack rulesMatrix[1][0][3] = robotLeftFront rulesMatrix[1][0][4] = robotLeftFront rulesMatrix[1][1][0] = robotForward rulesMatrix[1][1][1] = robotForward rulesMatrix[1][1][2] = robotLeftBack rulesMatrix[1][1][3] = robotLeftBack rulesMatrix[1][1][4] = robotLeftBack rulesMatrix[1][2][0] = robotRightFront rulesMatrix[1][2][1] = robotRightFront rulesMatrix[1][2][2] = robotForward rulesMatrix[1][2][3] = robotLeftFront rulesMatrix[1][2][4] = robotLeftFront rulesMatrix[1][3][0] = robotRightBack rulesMatrix[1][3][1] = robotRightBack rulesMatrix[1][3][2] = robotRightBack rulesMatrix[1][3][3] = robotForward rulesMatrix[1][3][4] = robotForward rulesMatrix[1][4][0] = robotRightBack rulesMatrix[1][4][1] = robotRightBack rulesMatrix[1][4][2] = robotRightFront rulesMatrix[1][4][3] = robotRightFront rulesMatrix[1][4][4] = robotRightFront #far rulesMatrix[2][0][0] = robotLeftBack rulesMatrix[2][0][1] = robotLeftBack rulesMatrix[2][0][2] = robotLeftBack rulesMatrix[2][0][3] = robotLeftBack rulesMatrix[2][0][4] = robotLeftBack rulesMatrix[2][1][0] = robotLeftFront rulesMatrix[2][1][1] = robotLeftFront rulesMatrix[2][1][2] = robotLeftFront rulesMatrix[2][1][3] = robotLeftFront rulesMatrix[2][1][4] = robotLeftFront rulesMatrix[2][2][0] = robotForward rulesMatrix[2][2][1] = robotForward rulesMatrix[2][2][2] = robotForward rulesMatrix[2][2][3] = robotForward rulesMatrix[2][2][4] = robotForward rulesMatrix[2][3][0] = robotRightFront rulesMatrix[2][3][1] = robotRightFront rulesMatrix[2][3][2] = robotRightFront rulesMatrix[2][3][3] = robotRightFront rulesMatrix[2][3][4] = robotRightFront rulesMatrix[2][4][0] = robotRightBack rulesMatrix[2][4][1] = robotRightBack rulesMatrix[2][4][2] = robotRightBack rulesMatrix[2][4][3] = robotRightBack rulesMatrix[2][4][4] = robotRightBack distanceFuzzySets = [ballVeryClose, ballClose, ballFar] ballFuzzySets = [ballLeftBack, ballLeftFront, ballForward, ballRightFront, ballRightBack] targetFuzzySets = [targetLeftBack, targetLeftFront, targetForward, targetRightFront, targetRightBack] robotFuzzySets = [robotLeftBack, robotLeftFront, robotForward, robotRightFront, robotRightBack] # Action loop while True: # Gets the angle between robot and ball ball_angle = sc.get_ball_angle() # Gets the angle between goal and robot target_angle = sc.get_target_angle() #gets the distance to the ball ball_distance = sc.get_ball_distance() inferedVector = [0]*NUMBER_OF_POINTS #laço para determinar a força de disparo de cada regra e calcular o conjunto de inferencia fuzzy for i in range(len(distanceFuzzySets)): for j in range(len(ballFuzzySets)): for k in range(len(targetFuzzySets)): mi0 = distanceFuzzySets[i].membership(ball_distance) if mi0 > 0: mi1 = ballFuzzySets[j].membership(ball_angle) #print(mi1) if mi1 > 0: mi2=targetFuzzySets[k].membership(target_angle) # print(mi2) if mi2 > 0: firingStrength = getFiringStrength(distanceFuzzySets[i], ball_distance, ballFuzzySets[j], ball_angle, targetFuzzySets[k], target_angle) inferedVector = fuzzyMaximum(inferedVector, cutOutputVector(rulesMatrix[i][j][k].set, firingStrength)) out = defuzzification(inferedVector) # converte ângulo do robô na força dos dois motores force_left = (math.cos(out) - math.sin(out))*0.55 force_right = (math.cos(out) + math.sin(out))*0.55 trainingDataFile.write(str(round(ball_distance,5))+" "+str(round(ball_angle,5))+" "+str(round(target_angle,5))+" "+str(round(force_left,5))+" "+str(round(force_right,5))+"\n") # Sends the action of robot to simulator sc.act(force_left, force_right) # Disconnects from match simulator (actually this line is never called) sc.disconnect() trainingDataFile.close()
def main(): host = sys.argv[1] if len(sys.argv) > 1 else 'localhost' port = int(sys.argv[2]) if len(sys.argv) > 2 else 1024 if port == 1024: trainingDataFile = open('soccer_blue.lrn', 'w') elif port == 1025: trainingDataFile = open('soccer_yellow.lrn', 'w') sc = SoccerClient() sc.connect(host, port) #conjuntos fuzzy de distância da bola ballVeryClose = fuzzyLeftTrapezoid(50, 90, 0, 1500) ballClose = fuzzyTrapezoid(80, 150, 250, 400, 0, 1500) ballFar = fuzzyRightTrapezoid(370, 500, 0, 1500) #conjuntos fuzzy do ângulo em relação à bola ballLeftBack = fuzzyLeftTrapezoid(-math.pi, -math.pi / 2, -math.pi, math.pi) ballLeftFront = fuzzyLambda(-3 * math.pi / 4, -math.pi / 2, 0, -math.pi, math.pi) ballForward = fuzzyLambda(-math.pi / 4, 0, math.pi / 4, -math.pi, math.pi) ballRightFront = fuzzyLambda(0, math.pi / 2, 3 * math.pi / 4, -math.pi, math.pi) ballRightBack = fuzzyRightTrapezoid(math.pi / 2, math.pi, -math.pi, math.pi) #conjuntos fuzzy do ângulo em relação ao gol targetLeftBack = fuzzyLeftTrapezoid(-math.pi, -math.pi / 2, -math.pi, math.pi) targetLeftFront = fuzzyLambda(-3 * math.pi / 4, -math.pi / 2, 0, -math.pi, math.pi) targetForward = fuzzyLambda(-math.pi / 2, 0, math.pi / 2, -math.pi, math.pi) targetRightFront = fuzzyLambda(0, math.pi / 2, 3 * math.pi / 4, -math.pi, math.pi) targetRightBack = fuzzyRightTrapezoid(math.pi / 2, math.pi, -math.pi, math.pi) #conjuntos fuzzy do ângulo do robô(saída) robotLeftBack = fuzzyLambda(-math.pi, -3 * math.pi / 4, -math.pi / 2, -math.pi, math.pi) robotLeftFront = fuzzyLambda(-3 * math.pi / 4, -math.pi / 2, 0, -math.pi, math.pi) robotForward = fuzzyLambda(-math.pi / 2, 0, math.pi / 2, -math.pi, math.pi) robotRightFront = fuzzyLambda(0, math.pi / 2, 3 * math.pi / 4, -math.pi, math.pi) robotRightBack = fuzzyLambda(math.pi / 2, 3 * math.pi / 4, math.pi, -math.pi, math.pi) rulesMatrix = numpy.zeros((3, 5, 5), dtype=object) # Make a 3 by 5 by 5 rules matrix #[distância, angulo bola, angulo gol] #very close rulesMatrix[0][0][0] = robotLeftBack rulesMatrix[0][0][1] = robotLeftFront rulesMatrix[0][0][2] = robotLeftFront rulesMatrix[0][0][3] = robotLeftFront rulesMatrix[0][0][4] = robotLeftFront rulesMatrix[0][1][0] = robotLeftBack rulesMatrix[0][1][1] = robotLeftFront rulesMatrix[0][1][2] = robotLeftBack rulesMatrix[0][1][3] = robotLeftBack rulesMatrix[0][1][4] = robotLeftBack rulesMatrix[0][2][0] = robotLeftBack rulesMatrix[0][2][1] = robotLeftFront rulesMatrix[0][2][2] = robotForward rulesMatrix[0][2][3] = robotRightFront rulesMatrix[0][2][4] = robotRightBack rulesMatrix[0][3][0] = robotRightBack rulesMatrix[0][3][1] = robotRightBack rulesMatrix[0][3][2] = robotRightBack rulesMatrix[0][3][3] = robotRightFront rulesMatrix[0][3][4] = robotRightBack rulesMatrix[0][4][0] = robotRightFront rulesMatrix[0][4][1] = robotRightFront rulesMatrix[0][4][2] = robotRightFront rulesMatrix[0][4][3] = robotRightFront rulesMatrix[0][4][4] = robotRightBack #close rulesMatrix[1][0][0] = robotLeftBack rulesMatrix[1][0][1] = robotLeftBack rulesMatrix[1][0][2] = robotLeftBack rulesMatrix[1][0][3] = robotLeftFront rulesMatrix[1][0][4] = robotLeftFront rulesMatrix[1][1][0] = robotForward rulesMatrix[1][1][1] = robotForward rulesMatrix[1][1][2] = robotLeftBack rulesMatrix[1][1][3] = robotLeftBack rulesMatrix[1][1][4] = robotLeftBack rulesMatrix[1][2][0] = robotRightFront rulesMatrix[1][2][1] = robotRightFront rulesMatrix[1][2][2] = robotForward rulesMatrix[1][2][3] = robotLeftFront rulesMatrix[1][2][4] = robotLeftFront rulesMatrix[1][3][0] = robotRightBack rulesMatrix[1][3][1] = robotRightBack rulesMatrix[1][3][2] = robotRightBack rulesMatrix[1][3][3] = robotForward rulesMatrix[1][3][4] = robotForward rulesMatrix[1][4][0] = robotRightBack rulesMatrix[1][4][1] = robotRightBack rulesMatrix[1][4][2] = robotRightFront rulesMatrix[1][4][3] = robotRightFront rulesMatrix[1][4][4] = robotRightFront #far rulesMatrix[2][0][0] = robotLeftBack rulesMatrix[2][0][1] = robotLeftBack rulesMatrix[2][0][2] = robotLeftBack rulesMatrix[2][0][3] = robotLeftBack rulesMatrix[2][0][4] = robotLeftBack rulesMatrix[2][1][0] = robotLeftFront rulesMatrix[2][1][1] = robotLeftFront rulesMatrix[2][1][2] = robotLeftFront rulesMatrix[2][1][3] = robotLeftFront rulesMatrix[2][1][4] = robotLeftFront rulesMatrix[2][2][0] = robotForward rulesMatrix[2][2][1] = robotForward rulesMatrix[2][2][2] = robotForward rulesMatrix[2][2][3] = robotForward rulesMatrix[2][2][4] = robotForward rulesMatrix[2][3][0] = robotRightFront rulesMatrix[2][3][1] = robotRightFront rulesMatrix[2][3][2] = robotRightFront rulesMatrix[2][3][3] = robotRightFront rulesMatrix[2][3][4] = robotRightFront rulesMatrix[2][4][0] = robotRightBack rulesMatrix[2][4][1] = robotRightBack rulesMatrix[2][4][2] = robotRightBack rulesMatrix[2][4][3] = robotRightBack rulesMatrix[2][4][4] = robotRightBack distanceFuzzySets = [ballVeryClose, ballClose, ballFar] ballFuzzySets = [ ballLeftBack, ballLeftFront, ballForward, ballRightFront, ballRightBack ] targetFuzzySets = [ targetLeftBack, targetLeftFront, targetForward, targetRightFront, targetRightBack ] robotFuzzySets = [ robotLeftBack, robotLeftFront, robotForward, robotRightFront, robotRightBack ] # Action loop while True: # Gets the angle between robot and ball ball_angle = sc.get_ball_angle() # Gets the angle between goal and robot target_angle = sc.get_target_angle() #gets the distance to the ball ball_distance = sc.get_ball_distance() inferedVector = [0] * NUMBER_OF_POINTS #laço para determinar a força de disparo de cada regra e calcular o conjunto de inferencia fuzzy for i in range(len(distanceFuzzySets)): for j in range(len(ballFuzzySets)): for k in range(len(targetFuzzySets)): mi0 = distanceFuzzySets[i].membership(ball_distance) if mi0 > 0: mi1 = ballFuzzySets[j].membership(ball_angle) #print(mi1) if mi1 > 0: mi2 = targetFuzzySets[k].membership(target_angle) # print(mi2) if mi2 > 0: firingStrength = getFiringStrength( distanceFuzzySets[i], ball_distance, ballFuzzySets[j], ball_angle, targetFuzzySets[k], target_angle) inferedVector = fuzzyMaximum( inferedVector, cutOutputVector(rulesMatrix[i][j][k].set, firingStrength)) out = defuzzification(inferedVector) # converte ângulo do robô na força dos dois motores force_left = (math.cos(out) - math.sin(out)) * 0.55 force_right = (math.cos(out) + math.sin(out)) * 0.55 trainingDataFile.write( str(round(ball_distance, 5)) + " " + str(round(ball_angle, 5)) + " " + str(round(target_angle, 5)) + " " + str(round(force_left, 5)) + " " + str(round(force_right, 5)) + "\n") # Sends the action of robot to simulator sc.act(force_left, force_right) # Disconnects from match simulator (actually this line is never called) sc.disconnect() trainingDataFile.close()
} if __name__ == '__main__': host = sys.argv[1] if len(sys.argv) > 1 else 'localhost' port = int(sys.argv[2]) if len(sys.argv) > 2 else 1024 sc = SoccerClient() sc.connect(host, port) # if you need only one calculation you do not need the while while True: # set input values my_input["ball_angle"] = math.degrees(sc.get_ball_angle()) my_input["target_angle"] = math.degrees(sc.get_target_angle()) my_input["robot_spin"] = math.degrees(sc.get_spin()) my_input["ball_distance"] = sc.get_ball_distance() my_input["obstacle_distance"] = sc.get_obstacle_distance() my_input["target_distance"] = sc.get_target_distance() my_input["obstacle_angle"] = math.degrees(sc.get_obstacle_angle()) # calculate system_direction.calculate(my_input, my_output) # now use outputs #angle = my_output["angle"] #angle = 0.0 print "---------------------------------------------------------------------------------" print "Ball_angle: ", my_input["ball_angle"], "target_angle ", my_input["target_angle"], "spin: " , my_input["robot_spin"] print "Ball_distance", my_input["ball_distance"], "obstacle_distance", my_input["obstacle_distance"], my_input["obstacle_angle"] print "---------------------------------------------------------------------------------"