Enchainement trajectoire + Envoi des données vers téléplot

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,16 +60,16 @@ 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:
+        if len(list_message[1])>33:
-                struct_value = struct.unpack('ffffff',list_message[1][1:25])
+                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])
+                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)
@@ -75,32 +78,47 @@ 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)
 
-if __name__ == "__main__":
+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)
 
-    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP
+def envoie_cde_set_position(x_mm, y_mm, z_rad):
-    sock.sendto(bytes(">c:1", "utf-8"), ("127.0.0.1", 47269))
+    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)
-    # traj = struct.pack('bffff', 0, 0.0, 0.0, 300. , 0.)
+    traj1 = struct.pack('bffff', 0, 0.0, 0.0, 100. , 0.)
-    traj = struct.pack('bffffffff', 2, 
+    traj2 = struct.pack('bffff', 0, 100.0, 0.0, 200. , 0.)
-                       0.0, 0.0, 
+    traj_actuelle = traj1
-                       50. , 0.,
+    # traj = struct.pack('bffffffff', 2, 
-                       150. , 200.,
+    #                    0.0, 0.0, 
-                       200. , 200.,)
+    #                    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)
+    # envoie_cde_set_position(0, 0, 0)
-    print(commande_PWM)
-    msg_envoi_donnees(b'P', 0x0D, commande_PWM)
-
-    envoie_cde_PWM(000,0)
-    exit()
 
+    # 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()
@@ -114,7 +132,40 @@ if __name__ == "__main__":
             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)
+        sleep(0.01)