import struct import serial.tools import serial.tools.list_ports import serial import socket from time import sleep import socket def send_msg(message : bytes): message_binaire = struct.pack('BBB{}sB'.format(message.__len__()), 0xFF, 0xFF, message.__len__() + 1, message, 0x00) ser.write(message_binaire) def msg_envoi_donnees(carte_id: bytes, registre : int, charge_utile : bytes): send_msg(struct.pack('ccBB{}s'.format(charge_utile.__len__()), b'r', carte_id, registre, charge_utile.__len__(), charge_utile)) def msg_demande_donnees(carte_id: bytes, registre : int, charge_utile : bytes): pass def check_message(message, offset): offset = offset + 3 # on saut '\xFF\xFF + id' if(len(message) > 0 + offset): taille_message = message[0+offset] print("taille message:", taille_message) if(len(message) > taille_message + offset): if(message[taille_message+offset] == 0): return message[offset-1:taille_message+offset] # Renvoi les message lus. # Les deux octets de début \xFF \xFF ne sont pas contenu dans le message # octet 0 : id_carte émettrice # octet 1 : Nombre d'octets reçus def lire_message(ser, old_data): data = old_data nb_read = ser.in_waiting offset = 0 valid_offset = 0 messages = [] if nb_read > 0: data += ser.read(nb_read) while offset > -1: offset = data.find(b'\xFF\xFF', offset) if(offset > -1): message = check_message(data, offset) if message is not None: messages.append(message) valid_offset = offset offset = offset + 1 data = data[valid_offset:] return messages, data def led_on(): msg_envoi_donnees(b'D', 0x00, b'\x06') def led_off(): msg_envoi_donnees(b'D', 0x00, b'\x36') def lecture_pos(): ser.write(b'\xFF\xFF\x05dP\x00\x0c\x00') sleep(0.01) messages, data = lire_message(ser, b'') for message in messages: print(message[0]) if message[0] == b'P'[0]: return struct.unpack('fff',message[2:14]) def lecture_abscisse(): ser.write(b'\xFF\xFF\x05dP\x8C\x04\x00') while ser.in_waiting == 0: pass data = ser.read(ser.in_waiting) list_message = data.split(b"\xff\xffP") if len(list_message)>1: if len(list_message[1])>4: int_value = struct.unpack('f',list_message[1][1:5]) return int_value[0] def lecture_pos_consigne(): ser.write(b'\xFF\xFF\x05dP\x00\x08\x00') while ser.in_waiting == 0: pass data = ser.read(ser.in_waiting) list_message = data.split(b"\xff\xffP") if len(list_message)>1: if len(list_message[1])>12: int_value = struct.unpack('ff',list_message[1][1:9]) return (int_value[0],int_value[1]) def lecture_propulsion(): ser.write(b'\xFF\xFF\x05dP\x80\x21\x00') sleep(0.01) nb_read = ser.in_waiting data = ser.read(nb_read) print("nb_read={}".format(nb_read)) list_message = data.split(b"\xff\xffP") print("nb_read={}, list_message={}".format(nb_read, len(list_message))) if len(list_message)>1: if len(list_message[1])>33: struct_value = struct.unpack('ffffffffB',list_message[1][1:34]) return (struct_value[0],struct_value[1],struct_value[2], struct_value[3], struct_value[4], struct_value[5], struct_value[6], struct_value[7], struct_value[8]) def envoie_trajectoire(traj): print(ser.read(ser.in_waiting)) msg_envoi_donnees(b'P', 0x22, traj) sleep(0.1) print("REP: envoie_trajectoire", ser.read(ser.in_waiting)) def envoie_cde_PWM(pwn_gauche, pwm_droit): commande_PWM = struct.pack('=Bhh',1, pwn_gauche, pwm_droit) msg_envoi_donnees(b'P', 0x0D, commande_PWM) def envoie_cde_vitesse_moteur(moteur_gauche_mm_s, moteur_droit_mm_s): commande_PWM = struct.pack('=Bhhff', 2, 0, 0, moteur_gauche_mm_s, moteur_droit_mm_s) msg_envoi_donnees(b'P', 0x0D, commande_PWM) def envoie_cde_vitesse_robot(avance_mm_s, rotation_rad_s): commande_PWM = struct.pack('=Bhhffff', 2, 0, 0, 0, 0, avance_mm_s, rotation_rad_s) msg_envoi_donnees(b'P', 0x0D, commande_PWM) def envoie_cde_vitesse_robot_2(avance_mm_s, rotation_rad_s): commande_mode = struct.pack('=B', 2) commande_vit_robot = struct.pack('=ff', avance_mm_s, rotation_rad_s) msg_envoi_donnees(b'P', 0x0D, commande_mode) msg_envoi_donnees(b'P', 0x1A, commande_vit_robot) def envoie_cde_config_trajet(vitesse_mm_s, acceleration_mm_ss): commande_config_trajet = struct.pack('ff', vitesse_mm_s, acceleration_mm_ss) print(commande_config_trajet) print(ser.read(ser.in_waiting)) msg_envoi_donnees(b'P', 0x5A, commande_config_trajet) sleep(0.1) print("REP: envoie_cde_config_trajet", ser.read(ser.in_waiting)) def envoie_cde_set_position(x_mm, y_mm, z_rad): commande_set_pos = struct.pack('fff', x_mm, y_mm, z_rad) msg_envoi_donnees(b'P', 0x00, commande_set_pos) def envoie_cde_inv_traj(): cde_inv_traj = struct.pack('B', 1) msg_envoi_donnees(b'P', 0x62, cde_inv_traj) sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) def log_variable(nom_variable, valeur): sock.sendto(nom_variable.encode() + ":".encode() + str(valeur).encode(), ("localhost", 47269)) def auto_test(): envoie_cde_vitesse_moteur(100, 0) sleep(3) envoie_cde_vitesse_moteur(0, 100) sleep(3) envoie_cde_vitesse_moteur(100, 100) sleep(3) envoie_cde_PWM(0, 0) if __name__ == "__main__": # sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP # sock.sendto(bytes(">c:1", "utf-8"), ("127.0.0.1", 47269)) forbidden_comport_device = ["/dev/ttyS0", "/dev/ttyS1", "/dev/ttyS2", "/dev/ttyS3"] for comport in serial.tools.list_ports.comports(): if comport.device in forbidden_comport_device: continue ser = serial.Serial(comport.device , 115200) print(comport.device) break print(ser.read(ser.in_waiting)) #msg_envoi_donnees(b'J', 0, b"\x01\x09AADAGGAAD") msg_envoi_donnees(b'J', 0, b"\x01\x04GDGD") # while True: # sleep(0.5) # print(str(ser.read(ser.in_waiting)))