diff --git a/python-engine/src/communications/radio.py b/python-engine/src/communications/radio.py index adc1be0..e51115f 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,33 @@ 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, robot_id, drb, kick, cb, x, y, r)-> None: message = {"x": x, "y": y, "r": r, "drb": drb, "kick": kick, "cb": cb} - self.r_queue[robot_id].append(message) + id = "r" + str(robot_id) + self.robotQueue[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/constants.py b/python-engine/src/constants.py index 1154158..a3d1a33 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 +COMMUNICATION_MODE = 0 # 0: Cancha, 1: Grsim +ISTEAMYELLOW = 1 # Grsim GRSIM_VISION_PORT = 10020 diff --git a/python-engine/src/main.py b/python-engine/src/main.py index f35e26d..771ac97 100644 --- a/python-engine/src/main.py +++ b/python-engine/src/main.py @@ -32,4 +32,5 @@ motion.move_linear([0,0], delta_time) - #radio.communicate_grsim(0, 0, veltangent = 5) \ No newline at end of file + #radio.communicate_grsim(0, 0, veltangent = 5) + 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 )