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

Trajectoire droite semble fonctionnelle, bezier plante en milieur de trajectoire

Browse Source
This commit is contained in:
Samuel 2026-04-17 22:45:10 +02:00
parent cfed6d27b9
commit aedd4f5e82
1 changed files with 86 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
Messagerie.py
View File

@ -1,6 +1,8 @@
import struct
import serial
import socket
from time import sleep
from time import time
def send_msg(message : bytes):
@ -10,21 +12,100 @@ def send_msg(message : bytes):
def msg_envoi_donnees(carte_id: bytes, registre : int, charge_utile : bytes):
send_msg(struct.pack('ccBB{}s'.format(charge_utile.__len__()), b'r', carte_id, registre, charge_utile.__len__(), charge_utile))
def msg_demande_donnees(carte_id: bytes, registre : int, charge_utile : bytes):
pass
def led_on():
msg_envoi_donnees(b'D', 0x00, b'\x06')
def led_off():
msg_envoi_donnees(b'D', 0x00, b'\x36')
def lecture_pos():
ser.write(b'\xFF\xFF\x05dP\x00\x0c\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])>12:
int_value = struct.unpack('fff',list_message[1][1:13])
return (int_value[0],int_value[1],int_value[2])
def lecture_abscisse():
ser.write(b'\xFF\xFF\x05dP\x8C\x04\x00')
while ser.in_waiting == 0:
pass
data = ser.read(ser.in_waiting)
list_message = data.split(b"\xff\xffP")
if len(list_message)>1:
if len(list_message[1])>4:
int_value = struct.unpack('f',list_message[1][1:5])
return int_value[0]
def lecture_pos_consigne():
ser.write(b'\xFF\xFF\x05dP\x00\x08\x00')
while ser.in_waiting == 0:
pass
data = ser.read(ser.in_waiting)
list_message = data.split(b"\xff\xffP")
if len(list_message)>1:
if len(list_message[1])>12:
int_value = struct.unpack('ff',list_message[1][1:9])
return (int_value[0],int_value[1])
def lecture_propulsion():
ser.write(b'\xFF\xFF\x05dP\x80\x18\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 __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, 100. , 0.)
traj = struct.pack('bffff', 0, 0.0, 0.0, 500. , 0.)
# traj = struct.pack('bffffffff', 2,
# 0.0, 0.0,
# 150. , 0.,
# 50. , 200.,
# 200. , 200.,)
msg_envoi_donnees(b'P', 0x22, traj)
while True:
pos = None
#données_propulsion= None
données_propulsion = lecture_propulsion()
#abscisse = lecture_abscisse()
#pos_consigne = lecture_pos_consigne()
# 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))
else:
# print("propulsion=None")
pass
led_off()
sleep(0.5)
led_on()
sleep(0.5)
#abscisse = lecture_abscisse()
# if abscisse != None:
# print(">abscisse:{:.2f}".format(abscisse))
sleep(0.05)