diff --git a/Messagerie.py b/Messagerie.py index ccc08f1..46b09e8 100644 --- a/Messagerie.py +++ b/Messagerie.py @@ -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) \ No newline at end of file + #abscisse = lecture_abscisse() + # if abscisse != None: + # print(">abscisse:{:.2f}".format(abscisse)) + + sleep(0.05) \ No newline at end of file