From 16083fe33344af1438460a47eb8bd306084df921 Mon Sep 17 00:00:00 2001 From: Nico-Mansilla Date: Wed, 14 Aug 2024 13:04:15 -0400 Subject: [PATCH 1/6] init --- python-engine/src/main.py | 35 ----------------------------------- 1 file changed, 35 deletions(-) diff --git a/python-engine/src/main.py b/python-engine/src/main.py index 4df93da..d46c7b0 100644 --- a/python-engine/src/main.py +++ b/python-engine/src/main.py @@ -8,40 +8,5 @@ import math if __name__ == '__main__': - # Initialize principal components - world : World = World(6,6) - nav : Navigator = Navigator(world) - comms : CommandSender = CommandSender() - - is_blue = 0 - robot_id = 0 - time.sleep(1) - move : Move = Move( world, nav, comms, (robot_id, is_blue) ) - radio : Grsim = Grsim() - test_angle = math.pi/2 + math.pi/4 - radio.communicate_pos_robot(robot_id, 1 - is_blue, 1,1, dir = test_angle) - time.sleep(1) - robot : Robot = world.get_robot(is_blue, robot_id) - print("Robot pos: " , robot.x, robot.y ) - move.move_to_point((0.4,-1)) - - ''' - # Initialize grsim packets - radio = Grsim() - # Por alguna razon grsim no envia packetes sin haber recibido ninguno - radio.communicate_pos_robot(id=0, yellowteam=0, x = 1, y = 1.2) - world : World = World(6,6) - nav : Navigator = Navigator(world) - time.sleep(1) - # test path - robot : Robot = world.get_robot(1, 0) - test_path = nav.get_path((robot.x, robot.y), (1.5,-3)) - print("robot pos: ", robot.x, robot.y) - # follow the path - print("path to follow: ", test_path) - for point in test_path: - print("to ", point) - radio.communicate_pos_robot(id=0, yellowteam=1, x = point[0], y = point[1]) - time.sleep(0.2) ''' \ No newline at end of file From 656f44a6c6644772667545f59f73ccc5a551ee55 Mon Sep 17 00:00:00 2001 From: Nico-Mansilla Date: Wed, 14 Aug 2024 13:06:49 -0400 Subject: [PATCH 2/6] cambios de prueba --- python-engine/src/main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python-engine/src/main.py b/python-engine/src/main.py index d46c7b0..2a82dc6 100644 --- a/python-engine/src/main.py +++ b/python-engine/src/main.py @@ -9,4 +9,4 @@ if __name__ == '__main__': - ''' \ No newline at end of file + ##cambios de prueba \ No newline at end of file From 1662cbb011e0f00784882066330c466ba7b4e0dc Mon Sep 17 00:00:00 2001 From: Nico-Mansilla Date: Sat, 24 Aug 2024 22:37:49 -0400 Subject: [PATCH 3/6] Se agrega isteamyellow como constante --- python-engine/src/constants.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python-engine/src/constants.py b/python-engine/src/constants.py index 1154158..bc8786b 100644 --- a/python-engine/src/constants.py +++ b/python-engine/src/constants.py @@ -1,8 +1,10 @@ +#Radio SERIAL_PORT = '/dev/ttyUSB0' BAUDRATE = 115200 TIMEOUT = 1 COMMUNICATION_MODE = 1 # 0: Cancha, 1: Grsim +ISTEAMYELLOW = 1 # Grsim GRSIM_VISION_PORT = 10020 From 3c96adc0a00fbcc10ef3ac69bca229e1b84465de Mon Sep 17 00:00:00 2001 From: Nico-Mansilla Date: Sat, 24 Aug 2024 23:40:03 -0400 Subject: [PATCH 4/6] radio.py: se agregan metodos de envio y manejo de colas move.py: se actualica una funcion para concordar con el wrapper wrapper.py: se implementan paquetes acordes con grsim y radio.py main.py: se genera codigo de prueba --- python-engine/src/communications/radio.py | 196 +++++++++++--------- python-engine/src/communications/wrapper.py | 78 ++++++-- python-engine/src/main.py | 23 ++- python-engine/src/stp/move.py | 2 +- 4 files changed, 200 insertions(+), 99 deletions(-) diff --git a/python-engine/src/communications/radio.py b/python-engine/src/communications/radio.py index adc1be0..7b44365 100644 --- a/python-engine/src/communications/radio.py +++ b/python-engine/src/communications/radio.py @@ -3,6 +3,8 @@ import serial import constants import numpy as np +import pickle + class Radio: _instance = None @@ -15,8 +17,9 @@ def __new__(cls, *args, **kwargs): return cls._instance def __init__(self): - self.r_queue = {'r0': [], 'r1': [], 'r2': [], 'r3': [], 'r4': [], 'r5': []} - self.r_id = {'r0': 0, 'r1': 1, 'r2': 2, 'r3': 3, 'r4': 4, 'r5': 5} + self.robotQueue = {'r0': [], 'r1': [], 'r2': [], 'r3': [], 'r4': [], 'r5': []} + self.robotIds = {'r0': 0, 'r1': 1, 'r2': 2, 'r3': 3, 'r4': 4, 'r5': 5} + self.verbose = False # Format for the queue ''' r_queue = { @@ -31,89 +34,90 @@ def __init__(self): self.serial_port = serial.Serial(port= constants.SERIAL_PORT, baudrate=constants.BAUDRATE, timeout=constants.TIMEOUT) if not self.serial_port.is_open: raise ValueError("No se pudo abrir el puerto serial") + + + #print data + def send_data(self): + packet = b'' + for rIndex in self.robotQueue.keys(): + robot_packet = np.zeros(5, dtype=np.uint8) + if self.robotQueue[rIndex] != []: + # Byte 0 + """ + 3 bits para el ID del robot a la izquierda del byte + 3 bits para la fuerza del dribbler a la derecha del ID del robot + 1 bit para el kicker a la derecha de la fuerza del dribbler + 1 bit para el callback a la derecha del kicker + """ + robot_packet[0] = self.robotIds[rIndex] << 5 + robot_packet[0] = robot_packet[0] | (self.robotQueue[rIndex][0]["drb"] << 2) + robot_packet[0] = robot_packet[0] | (self.robotQueue[rIndex][0]["kick"] << 1) + robot_packet[0] = robot_packet[0] | self.robotQueue[rIndex][0]["cb"] - #print data every 1 second - def send_loop(self): - while True: - packet = b'' - for k in self.r_queue.keys(): - robot_packet = np.zeros(5, dtype=np.uint8) - if self.r_queue[k] != []: - # Byte 0 - """ - 3 bits para el ID del robot a la izquierda del byte - 3 bits para la fuerza del dribbler a la derecha del ID del robot - 1 bit para el kicker a la derecha de la fuerza del dribbler - 1 bit para el callback a la derecha del kicker - """ - robot_packet[0] = self.r_id[k] << 5 - robot_packet[0] = robot_packet[0] | (self.r_queue[k][0]["drb"] << 2) - robot_packet[0] = robot_packet[0] | (self.r_queue[k][0]["kick"] << 1) - robot_packet[0] = robot_packet[0] | self.r_queue[k][0]["cb"] - - # Byte 1 - """ - 1 bit para el signo de la velocia x del robot a la izquierda del byte - 7 bits para la velocidad x del robot - """ - if self.r_queue[k][0]["x"] < 0: - self.r_queue[k][0]["x"] = -self.r_queue[k][0]["x"] - robot_packet[1] = robot_packet[1] | (self.r_queue[k][0]["x"] & 0x7F) - else: - robot_packet[1] = 1 << 7 - robot_packet[1] = robot_packet[1] | (self.r_queue[k][0]["x"] & 0x7F) - # Byte 2 - """ - 1 bit para el signo de la velocia y del robot a la izquierda del byte - 7 bits para la velocidad (solo parte entera) y del robot - """ - if self.r_queue[k][0]["y"] < 0: - robot_packet[2] = 1 << 7 - self.r_queue[k][0]["y"] = -self.r_queue[k][0]["y"] - robot_packet[2] = robot_packet[2] | (self.r_queue[k][0]["y"] & 0x7F) - else: - robot_packet[2] = robot_packet[2] | (self.r_queue[k][0]["y"] & 0x7F) - - # Byte 3 - """ - 1 bit para el signo de la velocia rotacional del robot a la izquierda del byte - 7 bits para la velocidad rotacional del robot - """ - if self.r_queue[k][0]["r"] < 0: - robot_packet[3] = 1 << 7 - self.r_queue[k][0]["r"] = -self.r_queue[k][0]["r"] - robot_packet[3] = robot_packet[3] | (self.r_queue[k][0]["r"] & 0x7F) - else: - robot_packet[3] = robot_packet[3] | (self.r_queue[k][0]["r"] & 0x7F) - - # Byte 4 - """ - 2 bits de digitos significativos de la velocidad x del robot a la izquierda del byte - 2 bits de digitos significativos de la velocidad y del robot a la derecha de los digitos significativos de la velocidad x - 4 bits de digitos significativos de la velocidad rotacional del robot a la derecha de los digitos significativos de la velocidad y - """ - # Convertir los digitos significativos a enteros - robot_packet[4] = robot_packet[4] | ((self.r_queue[k][0]["x"] >> 7) << 6) - robot_packet[4] = robot_packet[4] | ((self.r_queue[k][0]["y"] >> 7) << 4) - robot_packet[4] = robot_packet[4] | ((self.r_queue[k][0]["r"] >> 7) & 0x0F) - # print (robot_packet[4]) in binary - robot_packet_bytes = bytearray(robot_packet.tobytes()) - self.r_queue[k].pop(0) + # Byte 1 + """ + 1 bit para el signo de la velocidad x del robot a la izquierda del byte + 7 bits para la velocidad x del robot + """ + if self.robotQueue[rIndex][0]["x"] < 0: + self.robotQueue[rIndex][0]["x"] = -self.robotQueue[rIndex][0]["x"] + robot_packet[1] = robot_packet[1] | (self.robotQueue[rIndex][0]["x"] & 0x7F) else: - # Si el robot no esta activo, enviar un paquete de ceros - robot_packet[0] = self.r_id[k] << 5 - robot_packet[1] = 0 - robot_packet[2] = 0 - robot_packet[3] = 0 - robot_packet[4] = 0 - robot_packet_bytes = bytearray(robot_packet.tobytes()) + robot_packet[1] = 1 << 7 + robot_packet[1] = robot_packet[1] | (self.robotQueue[rIndex][0]["x"] & 0x7F) + # Byte 2 + """ + 1 bit para el signo de la velocia y del robot a la izquierda del byte + 7 bits para la velocidad (solo parte entera) y del robot + """ + if self.robotQueue[rIndex][0]["y"] < 0: + robot_packet[2] = 1 << 7 + self.robotQueue[rIndex][0]["y"] = -self.robotQueue[rIndex][0]["y"] + robot_packet[2] = robot_packet[2] | (self.robotQueue[rIndex][0]["y"] & 0x7F) + else: + robot_packet[2] = robot_packet[2] | (self.robotQueue[rIndex][0]["y"] & 0x7F) - # Convertir el arreglo de bytes a un string - # Convert each number in the array to its corresponding Unicode character - # Encode the characters using UTF-8 - packet += robot_packet_bytes - # Enviar el string al puerto serial - self.serial_port.write(packet) + # Byte 3 + """ + 1 bit para el signo de la velocia rotacional del robot a la izquierda del byte + 7 bits para la velocidad rotacional del robot + """ + if self.robotQueue[rIndex][0]["r"] < 0: + robot_packet[3] = 1 << 7 + self.robotQueue[rIndex][0]["r"] = -self.robotQueue[rIndex][0]["r"] + robot_packet[3] = robot_packet[3] | (self.robotQueue[rIndex][0]["r"] & 0x7F) + else: + robot_packet[3] = robot_packet[3] | (self.robotQueue[rIndex][0]["r"] & 0x7F) + + # Byte 4 + """ + 2 bits de digitos significativos de la velocidad x del robot a la izquierda del byte + 2 bits de digitos significativos de la velocidad y del robot a la derecha de los digitos significativos de la velocidad x + 4 bits de digitos significativos de la velocidad rotacional del robot a la derecha de los digitos significativos de la velocidad y + """ + # Convertir los digitos significativos a enteros + robot_packet[4] = robot_packet[4] | ((self.robotQueue[rIndex][0]["x"] >> 7) << 6) + robot_packet[4] = robot_packet[4] | ((self.robotQueue[rIndex][0]["y"] >> 7) << 4) + robot_packet[4] = robot_packet[4] | ((self.robotQueue[rIndex][0]["r"] >> 7) & 0x0F) + # print (robot_packet[4]) in binary + robot_packet_bytes = bytearray(robot_packet.tobytes()) + self.robotQueue[rIndex].pop(0) + else: + # Si el robot no esta activo, enviar un paquete de ceros + robot_packet[0] = self.robotIds[rIndex] << 5 + robot_packet[1] = 0 + robot_packet[2] = 0 + robot_packet[3] = 0 + robot_packet[4] = 0 + robot_packet_bytes = bytearray(robot_packet.tobytes()) + + # Convertir el arreglo de bytes a un string + # Convert each number in the array to its corresponding Unicode character + # Encode the characters using UTF-8 + packet += robot_packet_bytes + # Enviar el string al puerto serial + self.serial_port.write(packet) + if self.verbose: print(chr(27) + "[2J") for i in range(0, len(packet), 5): @@ -121,8 +125,32 @@ def send_loop(self): for j in range(0, 5): print(format(packet[i+j], '08b'), end=' ') print() + + + + + #print data every 1 second + def send_loop(self)-> None: + while True: + self.send_data() time.sleep(.016) - def recive_message(self, x,y,r,drb,kick,cb, robot_id): + + def add_to_robot_queue(self, x,y,r,drb,kick,cb, robot_id)-> None: message = {"x": x, "y": y, "r": r, "drb": drb, "kick": kick, "cb": cb} - self.r_queue[robot_id].append(message) + self.robotQueue[robot_id].append(message) + + + def receive_data(self): + # Recibir datos del puerto serial + # NOTE: Aun no se aplica un uso definido para esta funcion + serialized_data = self.serial_port.read(64) + data = pickle.loads(serialized_data) + return data + + + def list_serial_ports(self)-> None: + # Listar los puertos seriales disponibles + ports = serial.tools.list_ports.comports() + for port in ports: + print(port.device) diff --git a/python-engine/src/communications/wrapper.py b/python-engine/src/communications/wrapper.py index 81d5c3a..8273cda 100644 --- a/python-engine/src/communications/wrapper.py +++ b/python-engine/src/communications/wrapper.py @@ -1,25 +1,79 @@ # Interfaz de la radio para comunicarse con sim y cancha . #Esto es para evitar la divergencia entre codigo simulador y cancha from communications.grsim import Grsim +from communications.radio import Radio from constants import * import math class CommandSender: def __init__(self) -> None: + self.data_packet = [[0] * 7 for _ in range(6)] # Lista de listas 5x6 para enviar a los robots + if COMMUNICATION_MODE == 1: self.grsim = Grsim() - - def send_robot_data(self, id = 0, is_blue = 0, - velangular = 0, kickspeedx = 0, kickspeedz = 0, - veltangent = 0, velnormal = 0, spinner = 0, - wheelsspeed = False) -> None: + self.kickspeedx = 0 + self.kickspeedz = 0 + self.veltangent = 0 + self.wheelsspeed = False + else: + self.radio = Radio() + + + # LLena el paquete de datos para enviar a los robots + def add_robot_data(self, id=0, velangular=0, veltangent=0, + velnormal=0, dribbler=0, kicker=0, + call_back=0) -> None: + if id < 0 or id > 5: + raise ValueError("Robot id out of range") + if COMMUNICATION_MODE == 1: - isteamyellow = 1 - is_blue - self.grsim.communicate_grsim(id = id,isteamyellow= isteamyellow, velangular= velangular, - kickspeedx=kickspeedx, kickspeedz=kickspeedz, veltangent=veltangent, - velnormal= velnormal,spinner= spinner, - wheelsspeed=wheelsspeed) - - + self.data_packet[id] = [id, dribbler, kicker, call_back, veltangent, velnormal, velangular] + else: + self.radio.add_to_robot_queue(id, dribbler, kicker, call_back, veltangent, velnormal, velangular) + + + # Envía el paquete de datos a los robots en cancha o simulador segun corresponda + def send_robot_packet(self) -> None: + if COMMUNICATION_MODE == 1: + for id in range(6): + self.grsim.communicate_grsim( + id=id, + isteamyellow=ISTEAMYELLOW, + velangular=self.data_packet[id][6], + veltangent=self.data_packet[id][4], + velnormal=self.data_packet[id][5], + spinner=self.data_packet[id][1], + kickspeedx=self.kickspeedx, + kickspeedz=self.kickspeedz, + wheelsspeed=self.wheelsspeed + ) + + else: + self.radio.send_data() + + self.data_packet = [[0] * 7 for _ in range(6)] # Reinicia el paquete de datos + + # Envía un paquete de datos a un solo robot + def send_solo_robot_packet(self, id=0, velangular=0, veltangent=0, + velnormal=0, dribbler=0, kicker=0, + call_back=0) -> None: + if id < 0 or id > 5: + raise ValueError("Robot id out of range") + + if COMMUNICATION_MODE == 1: + self.grsim.communicate_grsim( + id=id, + isteamyellow=ISTEAMYELLOW, + velangular=velangular, + veltangent=veltangent, + velnormal=velnormal, + spinner=dribbler, + kickspeedx=self.kickspeedx, + kickspeedz=self.kickspeedz, + wheelsspeed=self.wheelsspeed + ) + else: + self.radio.add_to_robot_queue(id, dribbler, kicker, call_back, veltangent, velnormal, velangular) + self.radio.send_data() diff --git a/python-engine/src/main.py b/python-engine/src/main.py index 2a82dc6..a9eb62f 100644 --- a/python-engine/src/main.py +++ b/python-engine/src/main.py @@ -7,6 +7,25 @@ from stp.move import Move import math +def main(): + # Inicializar los componentes necesarios + grsim = Grsim() + world = World() + navigator = Navigator() + command_sender = CommandSender() + + # Crear un robot y añadirlo al mundo + robot = Robot(id=1, team='yellow') + world.add_robot(robot) + + # Definir el punto al que queremos mover el robot + target_point = (0.5, 0.5) + + # Mover el robot al punto especificado + Move.move_to_point(robot, target_point, navigator, command_sender) + + # Esperar un tiempo para observar el movimiento + time.sleep(5) + if __name__ == '__main__': - - ##cambios de prueba \ No newline at end of file + main() \ No newline at end of file diff --git a/python-engine/src/stp/move.py b/python-engine/src/stp/move.py index 57d9ce3..22c3b76 100644 --- a/python-engine/src/stp/move.py +++ b/python-engine/src/stp/move.py @@ -26,7 +26,7 @@ def motion_to_point(self, robot : Robot, to_point : tuple[float, float]): # Nose porque con negativo funciona si alguien sabe explicar porfavor dir_vec = rotate_2d_vector(dir_vec, -robot.orientation) dir_vec = normalize_2d_vector(dir_vec) - self.comms.send_robot_data(id = self.robot[0], is_blue = self.robot[1], + self.comms.send_solo_robot_packet(id = self.robot[0], is_blue = self.robot[1], veltangent= dir_vec[0]*self.MOVE_SPEED, velnormal = dir_vec[1]*self.MOVE_SPEED ) From 77b9eeae5369f3c7d177b23f56746dc53c755965 Mon Sep 17 00:00:00 2001 From: Nico-Mansilla Date: Sat, 31 Aug 2024 18:07:54 -0400 Subject: [PATCH 5/6] agregado codigo de prueba al main --- python-engine/src/constants.py | 2 +- python-engine/src/main.py | 22 +++++++++------------- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/python-engine/src/constants.py b/python-engine/src/constants.py index bc8786b..a3d1a33 100644 --- a/python-engine/src/constants.py +++ b/python-engine/src/constants.py @@ -3,7 +3,7 @@ BAUDRATE = 115200 TIMEOUT = 1 -COMMUNICATION_MODE = 1 # 0: Cancha, 1: Grsim +COMMUNICATION_MODE = 0 # 0: Cancha, 1: Grsim ISTEAMYELLOW = 1 # Grsim diff --git a/python-engine/src/main.py b/python-engine/src/main.py index a9eb62f..5f58870 100644 --- a/python-engine/src/main.py +++ b/python-engine/src/main.py @@ -8,24 +8,20 @@ import math def main(): - # Inicializar los componentes necesarios - grsim = Grsim() - world = World() - navigator = Navigator() + # Codigo de prueba de wrapper y radio command_sender = CommandSender() - # Crear un robot y añadirlo al mundo - robot = Robot(id=1, team='yellow') - world.add_robot(robot) - - # Definir el punto al que queremos mover el robot - target_point = (0.5, 0.5) - - # Mover el robot al punto especificado - Move.move_to_point(robot, target_point, navigator, command_sender) + #Enviar mensaje de prueba a robot 0 por radio + command_sender.send_solo_robot_packet(id=0, veltangent=40) # Esperar un tiempo para observar el movimiento time.sleep(5) + + # Enviar mensaje de parada a robot 0 por radio + command_sender.send_solo_robot_packet(id=0, veltangent=0) + + + if __name__ == '__main__': main() \ No newline at end of file From 6b483b4924cf7dd523dea43e8c86945708b73e50 Mon Sep 17 00:00:00 2001 From: Robocupusm-Windows Date: Sat, 31 Aug 2024 19:41:16 -0400 Subject: [PATCH 6/6] arreglo de error funcional en radio.py --- python-engine/src/communications/radio.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/python-engine/src/communications/radio.py b/python-engine/src/communications/radio.py index 7b44365..e51115f 100644 --- a/python-engine/src/communications/radio.py +++ b/python-engine/src/communications/radio.py @@ -136,9 +136,10 @@ def send_loop(self)-> None: time.sleep(.016) - def add_to_robot_queue(self, x,y,r,drb,kick,cb, robot_id)-> None: + def add_to_robot_queue(self, robot_id, drb, kick, cb, x, y, r)-> None: message = {"x": x, "y": y, "r": r, "drb": drb, "kick": kick, "cb": cb} - self.robotQueue[robot_id].append(message) + id = "r" + str(robot_id) + self.robotQueue[id].append(message) def receive_data(self):