class Kernel(object):
state_receiver = None
command_sender = None
debug_sender = None
def loop(self):
self.state_receiver = StateReceiver()
self.state_receiver.create_socket()
self.command_sender = CommandSender()
self.command_sender.create_socket()
self.debug_sender = DebugSender()
self.debug_sender.create_socket()
while(True):
state = self.state_receiver.receive_state()
self.command_sender.send_command(self.__build_command())
self.debug_sender.send_debug(self.__build_debug(state))
def __build_command(self):
command = Command()
command.wheels_commands.append(WheelsCommand(10, -10))
command.wheels_commands.append(WheelsCommand(10, -10))
command.wheels_commands.append(WheelsCommand(10, -10))
return command
def __build_debug(self, state):
debug = Debug()
debug.clean()
for robot in state.team_yellow:
debug.step_points.append(Point(robot.x + 10, robot.y + 10))
debug.final_poses.append(Pose(state.ball.x + 10, state.ball.y + 10, 10))
class Kernel(object):
state_receiver = None
command_sender = None
debug_sender = None
def __init__(self):
self.state_receiver = StateReceiver()
self.state_receiver.create_socket()
self.command_sender = CommandSender()
self.command_sender.create_socket()
self.debug_sender = DebugSender()
self.debug_sender.create_socket()
def __build_command(self, rvel):
command = Command()
command.wheels_commands.append(WheelsCommand(rvel[0][0], rvel[0][1]))
command.wheels_commands.append(WheelsCommand(rvel[1][0], rvel[1][1]))
command.wheels_commands.append(WheelsCommand(rvel[2][0], rvel[2][1]))
return command
def __build_debug(self, robots_points):
debug = Debug()
debug.clean()
for robot_points in robots_points:
points = robot_points["step_points"]
end_pose = robot_points["end_pose"]
for point in points:
debug.step_points.append(Point(point[0], point[1]))
debug.final_poses.append(
Pose(end_pose[0], end_pose[1] + end_pose[2]))
""" Nome da função : receive_state
Intenção da função : receber um state do simulador vss-sdk
Pré-requisitos : simulador rodando
Efeitos colaterais : recebe o state atual
Parâmetros : nenhum
Retorno : state : State
"""
def receive_state(self):
return self.state_receiver.receive_state()
""" Nome da função : send_command
Intenção da função : enviar um comando ao simulador vss-sdk
Pré-requisitos : simulador rodando
Efeitos colaterais : envia comandos a um dos times do simulador
os times movem-se com a velocidade especificada
Parâmetros : rvel : List([vel1, vel2]*3)
Retorno : nenhum
"""
def send_command(self, rvel):
self.command_sender.send_command(self.__build_command(rvel))
def send_debug(self, robots_points):
self.debug_sender.send_debug(self.__build_debug(robots_points))
class Kernel():
state_receiver = None
command_sender = None
debug_sender = None
robots_pid = [Robot(i) for i in range(6)]
def __init__(self, two_teams):
self.two_teams = two_teams
def loop(self):
self.count = 0
self.state_receiver = StateReceiver()
self.state_receiver.create_socket()
self.command_sender_yellow = CommandSender()
self.command_sender_yellow.create_socket(port=5556)
self.command_sender_blue = CommandSender()
self.command_sender_blue.create_socket(port=5557)
self.debug_sender = DebugSender()
self.debug_sender.create_socket()
while True:
state = self.state_receiver.receive_state()
state_data = build_for_noplan(state)
udp_sender.sendto(bytes(json.dumps(state_data), 'utf-8'),
dest_sender)
data, addr = udp_receiver.recvfrom(1024)
self.command_sender_yellow.send_command(
self.__build_command(data, state_data, 'yellow'))
if self.two_teams:
self.command_sender_blue.send_command(
self.__build_command(data, state_data, 'blue'))
# self.debug_sender.send_debug(self.__build_debug(state))
def __build_command(self, data, state_data, team_color):
command = Command()
command.clean()
commands_obj = json.loads(data.decode('utf-8'))
commands_obj.sort(key=lambda x: x[0], reverse=False)
last_angles = [0, 0, 0, 0, 0, 0]
allowed_ids = [0, 1, 2] if team_color == 'yellow' else [3, 4, 5]
output = []
for obj, r_pid in zip(commands_obj, self.robots_pid):
# print(state_data)
if (obj[0] not in allowed_ids):
continue
sp_lin = state_data['detection']['robots_{}'.format(team_color)][
obj[0] % 3]['linear_speed']
ang = math.degrees(state_data['detection']['robots_{}'.format(
team_color)][obj[0] % 3]['orientation'])
delta_angle = (ang - last_angles[obj[0]])
if delta_angle > 180:
delta_angle -= 360
elif delta_angle < -180:
delta_angle += 360
last_angles[obj[0]] = ang
print(obj)
sp_ang = delta_angle / r_pid.dt
r_pid.set_target(obj[2], obj[3])
wheel_right, wheel_left = r_pid.speed_to_power(sp_lin, sp_ang)
lin = wheel_right
ang = wheel_left
# print('wheels_power: ', wheel_right, wheel_left)
command.commands.append(WheelsCommand(wheel_right, wheel_left))
# Partindo do principio que havera no maximo 3 robos em cada time
mock_commands_left = 3 - len(command.commands)
for i in range(mock_commands_left):
command.commands.append(WheelsCommand(0, 0))
print(len(command.commands))
return command