diff --git a/LectureReception.py b/LectureReception.py new file mode 100644 index 0000000..892d5e2 --- /dev/null +++ b/LectureReception.py @@ -0,0 +1,50 @@ +import struct +import serial + +ser = serial.Serial('/dev/ttyACM0' , 115200) + + +def lecture_Detection(): + 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 check_message(message, offset): + offset = offset + 2 # on saut '\xFF\xFF' + if(len(message) > 0 + offset): + taille_message = message[0+offset] + print("taille_message + " + str(taille_message)) + if(len(message) > taille_message + offset): + if(message[taille_message+offset] == 0): + print("message valide") + return message[0+offset:taille_message+offset] + return None + + + +if __name__ == "__main__": + data = b'' + while(1): + nb_read = ser.in_waiting + offset = 0 + valid_offset = 0 + if nb_read > 0: + data+= ser.read(nb_read) + while offset > -1: + offset = data.find(b'\xFF\xFF', offset) + if(offset > -1): + message_brut = data[offset:] + message = check_message(data, offset) + if message is not None: + print(message) + valid_offset = offset + offset = offset + 1 + data = data[valid_offset:] + \ No newline at end of file diff --git a/Messagerie.py b/Messagerie.py index 4c0854f..b95663f 100644 --- a/Messagerie.py +++ b/Messagerie.py @@ -90,6 +90,15 @@ def envoie_cde_set_position(x_mm, y_mm, z_rad): 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 @@ -107,12 +116,13 @@ if __name__ == "__main__": # 200. , 200.,) # msg_envoi_donnees(b'P', 0x22, traj) envoie_cde_set_position(0, 0, 0) - envoie_trajectoire(traj_actuelle) + # envoie_trajectoire(traj_actuelle) # envoie_cde_set_position(0, 0, 0) - # envoie_cde_PWM(000,0) + # envoie_cde_PWM(8000, 8000) # envoie_cde_vitesse_moteur(100, 100) + auto_test() # exit() print("carte\ttemps\tx\ty\torientation\tabscisse\tx_consigne\ty_consigne\tvit_roue_gauche\tvit_roue_droite\tetat_trajet") @@ -148,21 +158,21 @@ if __name__ == "__main__": 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 + # 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