111 lines
4.0 KiB
Python
111 lines
4.0 KiB
Python
import struct
|
|
import serial
|
|
import socket
|
|
from time import sleep
|
|
from time import time
|
|
|
|
|
|
def send_msg(message : bytes):
|
|
message_binaire = struct.pack('BBB{}sB'.format(message.__len__()), 0xFF, 0xFF, message.__len__() + 1, message, 0x00)
|
|
print(message_binaire)
|
|
ser.write(message_binaire)
|
|
|
|
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, 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
|
|
|
|
#abscisse = lecture_abscisse()
|
|
# if abscisse != None:
|
|
# print(">abscisse:{:.2f}".format(abscisse))
|
|
|
|
sleep(0.05) |