diff --git a/hote_python/Messagerie.py b/hote_python/Messagerie.py new file mode 100644 index 0000000..7581c9e --- /dev/null +++ b/hote_python/Messagerie.py @@ -0,0 +1,177 @@ +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))) + diff --git a/hote_python/requirements.txt b/hote_python/requirements.txt new file mode 100644 index 0000000..d0dfd58 --- /dev/null +++ b/hote_python/requirements.txt @@ -0,0 +1 @@ +pyserial==3.5