import struct import serial import socket from time import sleep from time import time import socket def send_msg(message : bytes): message_binaire = struct.pack('BBB{}sB'.format(message.__len__()), 0xFF, 0xFF, message.__len__() + 1, message, 0x00) print(message_binaire) 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 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') 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])>12: int_value = struct.unpack('fff',list_message[1][1:13]) return (int_value[0],int_value[1],int_value[2]) 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') 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): msg_envoi_donnees(b'P', 0x22, traj) 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_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) 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)) 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)) ser = serial.Serial('/dev/ttyACM0' , 115200) traj1 = struct.pack('bffff', 0, 0.0, 0.0, 100. , 0.) traj2 = struct.pack('bffff', 0, 100.0, 0.0, 200. , 0.) traj_actuelle = traj1 # traj = struct.pack('bffffffff', 2, # 0.0, 0.0, # 50. , 0., # 150. , 200., # 200. , 200.,) # msg_envoi_donnees(b'P', 0x22, traj) envoie_cde_set_position(0, 0, 0) envoie_trajectoire(traj_actuelle) # envoie_cde_set_position(0, 0, 0) # envoie_cde_PWM(000,0) # envoie_cde_vitesse_moteur(100, 100) # exit() print("carte\ttemps\tx\ty\torientation\tabscisse\tx_consigne\ty_consigne\tvit_roue_gauche\tvit_roue_droite\tetat_trajet") while True: pos = None données_propulsion= None données_propulsion = lecture_propulsion() #abscisse = lecture_abscisse() #pos_consigne = lecture_pos_consigne() # if pos is not None and abscisse is not None and pos_consigne is not None : # print(">pos:{:.2f}:{:.2f}:{:.2f}:{:.2f}:{:.2f}". # format(pos[0],pos[1], abscisse, pos_consigne[0], pos_consigne[1] )) if données_propulsion != None : x, y = données_propulsion[0],données_propulsion[1] orientation = données_propulsion[2] abscisse = données_propulsion[3] x_consigne, y_consigne = données_propulsion[4],données_propulsion[5] vit_roue_gauche, vit_roue_droite = données_propulsion[6], données_propulsion[7] etat_trajet = données_propulsion[8] log_variable("x_pos", x) log_variable("y_pos", y) log_variable("Orientation", orientation) log_variable("abscisse", abscisse) log_variable("x_consigne", x_consigne) log_variable("y_consigne", y_consigne) log_variable("vit_roue_gauche", vit_roue_gauche) log_variable("vit_roue_droite", vit_roue_droite) log_variable("etat_trajet", etat_trajet) print("propulsion\t{:.3f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{}\t".format( time(), x, y, orientation, abscisse, x_consigne, y_consigne, vit_roue_gauche, vit_roue_droite, etat_trajet)) if etat_trajet == 1: if etat_trajet_flag == 0: etat_trajet_flag = 1 if traj_actuelle == traj1: traj2 = struct.pack('bffff', 0, x, y, 0. , 0.) print(traj2) traj_actuelle = traj2 envoie_trajectoire(traj_actuelle) else: traj1 = struct.pack('bffff', 0, x, y, 100. , 0.) print(traj1) traj_actuelle = traj1 envoie_trajectoire(traj_actuelle) else: etat_trajet_flag = 0 else: # print("propulsion=None") pass #abscisse = lecture_abscisse() # if abscisse != None: # print(">abscisse:{:.2f}".format(abscisse)) sleep(0.01)