diff --git a/Messagerie.py b/Messagerie.py index 7209e04..4c0854f 100644 --- a/Messagerie.py +++ b/Messagerie.py @@ -4,6 +4,9 @@ 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) @@ -57,50 +60,65 @@ def lecture_pos_consigne(): return (int_value[0],int_value[1]) def lecture_propulsion(): - ser.write(b'\xFF\xFF\x05dP\x80\x18\x00') + 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])>25: - struct_value = struct.unpack('ffffff',list_message[1][1:25]) - return (struct_value[0],struct_value[1],struct_value[2], struct_value[3], struct_value[4], struct_value[5]) + 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) + 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) + 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)) + # 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) - # traj = struct.pack('bffff', 0, 0.0, 0.0, 300. , 0.) - traj = struct.pack('bffffffff', 2, - 0.0, 0.0, - 50. , 0., - 150. , 200., - 200. , 200.,) + 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) - commande_PWM = struct.pack('=Bhh',1, 000, 000) - print(commande_PWM) - msg_envoi_donnees(b'P', 0x0D, commande_PWM) - - envoie_cde_PWM(000,0) - exit() + # 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= None données_propulsion = lecture_propulsion() #abscisse = lecture_abscisse() #pos_consigne = lecture_pos_consigne() @@ -108,13 +126,46 @@ if __name__ == "__main__": # 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] - print("propulsion\t{:.3f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t".format(time(), x, y, orientation, abscisse, x_consigne, y_consigne)) + 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 @@ -123,4 +174,4 @@ if __name__ == "__main__": # if abscisse != None: # print(">abscisse:{:.2f}".format(abscisse)) - sleep(0.05) \ No newline at end of file + sleep(0.01) \ No newline at end of file