Keuronde/CerveauPy2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Samuel 2026-04-18 18:05:49 +02:00
parent 8fd6c07fc6
commit cd142def2f
1 changed files with 77 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
Messagerie.py
View File

@ -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)
sleep(0.01)