Découpage en message OK

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:]
+            

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 == 1:
-                if etat_trajet_flag == 0:
+            #     if etat_trajet_flag == 0:
-                    etat_trajet_flag = 1
+            #         etat_trajet_flag = 1
-                    if traj_actuelle == traj1:
+            #         if traj_actuelle == traj1:
-                        traj2 = struct.pack('bffff', 0, x, y, 0. , 0.)
+            #             traj2 = struct.pack('bffff', 0, x, y, 0. , 0.)
-                        print(traj2)
+            #             print(traj2)
-                        traj_actuelle = traj2
+            #             traj_actuelle = traj2
-                        envoie_trajectoire(traj_actuelle)
+            #             envoie_trajectoire(traj_actuelle)
-                    else:
+            #         else:
-                        traj1 = struct.pack('bffff', 0, x, y, 100. , 0.)
+            #             traj1 = struct.pack('bffff', 0, x, y, 100. , 0.)
-                        print(traj1)
+            #             print(traj1)
-                        traj_actuelle = traj1
+            #             traj_actuelle = traj1
-                        envoie_trajectoire(traj_actuelle)
+            #             envoie_trajectoire(traj_actuelle)
-            else:
+            # else:
-                 etat_trajet_flag = 0
+            #      etat_trajet_flag = 0