Hệ thống điều hướng tàu tự động

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH ĐỒ ÁN TỐT NGHIỆP NGÀNH KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA HỆ THỐNG ĐIỀU HƯỚNG TÀU TỰ ĐỘNG GVHD: PGS.TS NGUYỄN MINH TÂM SVTH: NGUYỄN THUẬN HẢI HUỲNH NHẬT QUANG SKL009259 Tp.Hồ Chí Minh, tháng 8/2022 TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP HỆ THỐNG ĐIỀU HƯỚNG TÀU TỰ ĐỘNG SINH VIÊN THỰC HIỆN TÊN: NGUYỄN THUẬN HẢI MSSV: 17151073 TÊN: HUỲNH NHẬT QUANG MSSV: 17151118 KHÓA: 2017 CHUYÊN NGÀNH: KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA GVHD : PGS.TS NGUYỄN MINH TÂM Tháng 8, Năm 2022, Thành Phố Hồ Chí Minh TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP HỆ THỐNG ĐIỀU HƯỚNG TÀU TỰ ĐỘNG SINH VIÊN THỰC HIỆN TÊN: NGUYỄN THUẬN HẢI MSSV: 17151073 TÊN: HUỲNH NHẬT QUANG MSSV: 17151118 KHÓA: 2017-2022 CHUYÊN NGÀNH: KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA GVHD: PGS.TS NGUYỄN MINH TÂM Tháng 8, Năm 2022, Thành Phố Hồ Chí Minh CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc Lập-Tự Do-Hạnh Phúc -Thành phố Hồ Chí Minh, tháng năm 2022 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Nguyễn Thuận Hải MSSV: 17151073 Họ tên sinh viên 2: Huỳnh Nhật Quang MSSV: 17151118 Chuyên ngành: Điều khiển Tự động hóa Lớp: 17151CL1B Giáo viên hướng dẫn: PGS.TS Nguyễn Minh Tâm Ngày nhận đề tài: Tên dề tài: HỆ THỐNG ĐIỀU HƯỚNG TÀU TỰ ĐỘNG Các số liệu, tài liệu ban đầu: - Máy tính cá nhân Atomic Pi, vi điều khiển Atmega328p cách sử dụng phần mềm Pycharm IDE Open CPN - Điều khiển động vận hành mơ hình hệ thống - Tài liệu tính toán, thiết kế phần mềm cho hệ thống điều hướng tàu tự động Nội dung thực đề tài: - Nghiên cứu tài liệu mơ hình, hệ thống thực tế điều hướng tàu tự động - Tính tốn, thiết kế thi cơng phần cứng - Tính tốn, lập trình điều khiển cho hệ thống tàu bám theo sát hành trình đặt - Liên kết vi điều khiển phần mềm để giám sát thông qua chuẩn UART Giao tiếp, truyền liệu mơ hình hệ thống vệ tinh GPS i - Liên kết giao tiếp phần mềm thông số thu từ vệ tinh để tính tốn đường cho chuyến hải trình Sản phẩm: - Mơ hình mô điều hướng tàu tự động TRƯỞNG NGÀNH GIẢNG VIÊN HƯỚNG DẪN ii CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc Lập-Tự Do-Hạnh Phúc -Thành phố Hồ Chí Minh, tháng năm 2022 PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Tên sinh viên: Nguyễn Thuận Hải MSSV: 17151073 Tên sinh viên: Huỳnh Nhật Quang MSSV: 17151118 Chuyên ngành: Kỹ thuật điều khiển tự động hóa Tên đồ án: Hệ thống điều hướng tàu tự động Giáo viên hướng dẫn: PGS.TS Nguyễn Minh Tâm NHẬN XÉT Về nội dung đề tài & khối lượng thực hiện: Ưu điểm: Khuyết điểm: Chấp nhận bảo vệ đồ án? (Chấp nhận từ chối?) Đánh giá chung: (Rất tốt, tốt, trung bình, yếu) Điểm số:……………….(bằng chữ: ) Thành phố Hồ Chí Minh , ngày tháng năm Hội đồng tham dự (kí ghi rõ họ tên) Giáo viên hướng dẫn (kí ghi rõ họ tên) iii TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỘC LẬP - TỰ DO - HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN o0o Tp HCM, ngày 20 tháng năm 2022 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Nguyễn Thuận Hải MSSV: 17151073 Họ tên sinh viên 2: Huỳnh Nhật Quang MSSV: 17151118 Tên đề tài: Hệ thống điều hướng tàu tự động Tuần/ngày Nội dung Lựa chọn đề tài thực (Thiết kế thi công mô hình tàu tự hành) Tìm hiểu tổng quan đề tài Lựa chọn thiết bị sử dụng 3,4 Thiết kế phần cứng Nghiên cứu chuẩn truyền thông NMEA 2000 NMEA 0183 Lập trình điều khiển 6, 7, 8, 9, 10 11, 12, 13 14 Thiết kế ứng dụng để giao tiếp phần cứng phần mềm Kiểm tra vận hành mơ hình Viết báo cáo chương 4,5 iv 15,16 Kiểm tra, sửa chữa báo cáo v TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN-ĐIỆN TỬ ĐỘC LẬP - TỰ DO - HẠNH PHÚC BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN o0o Tp HCM, ngày tháng năm 2022 LỜI CAM ĐOAN Nhóm xin cam kết đề tài nhóm tự thực dựa vào số tài liệu trước khơng chép từ tài liệu hay cơng trình có trước Nhóm thực đề tài Nguyễn Thuận Hải Huỳnh Nhật Quang vi LỜI CẢM ƠN Sau nhiều năm không ngừng nghiên cứu học hỏi, sau chặng đường dài, nhóm đúc kết đưa báo cáo tốt nghiệp chuyên ngành công nghệ kỹ thuật điều khiển tự động hóa mơn tự động điều khiển với đề tài “Hệ thống điều hướng tàu tự động” Có thể nói kết q trình cố gắng nhóm thực giúp đỡ, động viên khích lệ thầy cơ, bạn bè người thân Đầu tiên nhóm xin gửi lời cảm ơn biết ơn sâu sắc thầy, cô, giảng viên trường, tất thầy cô khoa điện - điện tử đặc biệt hết lời cảm ơn chân thành quý thầy cô giảng viên môn tự động điều khiển trường Đại học Sư Phạm Kỹ Thuật TPHCM hướng dẫn dìu dắt nhóm sinh viên từ kiến thức, tảng đến kiến thức chun mơn, phục vụ cho mục đích hồn thành luận văn Lời cảm ơn thứ hai nhóm xin gửi đến giảng viên hướng dẫn nhóm phó giáo sư, tiến sĩ Nguyễn Minh Tâm, thầy người giúp đỡ nhóm nhiều từ bắt đầu lúc hoàn thiện luận văn Thầy bảo, dẫn dắt lỗi sai nhóm q trình làm đề tài để đưa hướng tốt Mặc dù với vai trị trưởng khoa vơ bận rộn, thầy ln xếp thời gian để giải đáp thắc mắc lúc nhóm gặp khó khăn để nhóm đưa hướng giải kịp thời Thầy tạo điều kiện tốt để hỗ trợ cho nhóm trang thiết bị mơi trường làm việc để nhóm hồn thành đồ án Lời cuối cùng, xin cảm ơn gia đình bạn bè bên cạnh ủng hộ suốt năm đại học, tạo điều kiện thuận lợi để hoàn thành tốt luận văn tốt nghiệp Một lần nữa, nhóm xin chân thành cảm ơn! Nhóm thực đề tài Nguyễn Thuận Hải Huỳnh Nhật Quang vii y=330) condition = rows3 = ('3', 'W2', 'Đang Lái', crr_time, 'Warning') rows.append(rows3) wrapper1 = LabelFrame(ws, height=100, width=800, bg='#ccffcc') wrapper2 = LabelFrame(ws, height=200, width=350) wrapper3 = LabelFrame(ws, height=200, width=240) wrapper4 = LabelFrame(ws, height=200, width=240) wrapper5 = LabelFrame(ws, height=100, width=800) wrapper1.pack() wrapper5.place(x=0, y=300) wrapper2.place(x=0, y=100) wrapper3.place(x=350, y=100) wrapper4.place(x=590, y=100) def on_click(text): global var11 var11 = text def Window(): Window = tk.Toplevel() canvas = tk.Canvas(Window, height=300, width=500) canvas.pack() # Search Section var = IntVar() OPTIONS = ["Serial", "TCP/IP", "UDP", "GPSD"] timkiem = StringVar(Window) timkiem.set(OPTIONS[0]) # default value w = OptionMenu(Window, timkiem, *OPTIONS) w.config(width=12, height=2, font='Times 14 bold') w.place(x=0, y=10) ab1 = tk.Entry(Window, font='Times 12 bold', width=8) ab1.place(x=150, y=80) ab2 = tk.Entry(Window, font='Times 12 bold', width=8) ab2.place(x=150, y=110) ab3 = tk.Entry(Window, font='Times 12 bold', width=8) ab3.place(x=150, y=140) ab4 = tk.Entry(Window, font='Times 12 bold', width=8) ab4.place(x=150, y=170) ab5 = tk.Entry(Window, font='Times 12 bold', width=8) ab5.place(x=150, y=200) tk.Label(Window, text='Địa Chỉ:', font='Times 12 bold', fg='black').place(x=70, y=80) tk.Label(Window, text='Port:', font='Times 12 bold', fg='black').place(x=70, y=110) tk.Label(Window, text='Baurate:', font='Times 12 bold', fg='black').place(x=70, y=140) tk.Label(Window, text='Timeout:', font='Times 12 bold', fg='black').place(x=70, y=170) tk.Label(Window, text='Comment:', font='Times 12 bold', fg='black').place(x=70, y=200) but = tk.Button(Window, text="Xác Nhận", font='Times 17 bold', command=lambda: Window.destroy()) but.place(x=200, y=250) def New_Window1(): global feedback Window1 = tk.Toplevel() canvas = tk.Canvas(Window1, height=300, width=400) canvas.pack() ab1 = tk.Text(Window1, font='Times 12 bold', height=1, width=8) ab1.place(x=190, y=80) ab2 = tk.Text(Window1, font='Times 12 bold', height=1, width=8) ab2.place(x=190, y=110) ab3 = tk.Text(Window1, font='Times 12 bold', height=1, width=8) ab3.place(x=190, y=140) ab4 = tk.Text(Window1, font='Times 12 bold', height=1, width=8) ab4.place(x=190, y=170) ab5 = tk.Text(Window1, font='Times 12 bold', height=1, width=8) ab5.place(x=190, y=200) ab1.delete(0.0, 'end') ab1.insert(0.0, str(5)) ab2.delete(0.0, 'end') ab2.insert(0.0, str(40)) ab3.delete(0.0, 'end') ab3.insert(0.0, str(0.01)) ab4.delete(0.0, 'end') ab4.insert(0.0, str(3)) ab5.delete(0.0, 'end') ab5.insert(0.0, str(feedback)) tk.Label(Window1, text='Điện áp (V):', font='Times 12 bold', fg='black').place(x=70, y=80) tk.Label(Window1, text='Nhiệt Độ (*C):', font='Times 12 bold', fg='black').place(x=70, y=110) tk.Label(Window1, text='Dòng Điện(A):', font='Times 12 bold', fg='black').place(x=70, y=140) tk.Label(Window1, text='Tốc độ(rrm):', font='Times 12 bold', fg='black').place(x=70, y=170) tk.Label(Window1, text='Ví Trí(Xung):', font='Times 12 bold', fg='black').place(x=70, y=200) but = tk.Button(Window1, text="Xác Nhận", font='Times 17 bold', command=lambda: Window1.destroy()) but.place(x=100, y=250) but1 = tk.Button(Window1, text="Tunning", font='Times 17 bold') but1.place(x=230, y=250) def Window1(): global rows newWindow = Toplevel(ws) newWindow.title("Danh sách Cảnh Báo") cols1 = ('ID', 'Lỗi', 'Ghi Chú', 'Thời Gian', 'Cảnh Báo') trv1 = ttk.Treeview(newWindow, columns=cols1, show="headings", height=15) trv1.pack() trv1.heading(0, text="ID", anchor=CENTER) trv1.heading(1, text='Lỗi', anchor=CENTER) trv1.heading(2, text='Ghi Chú', anchor=CENTER) trv1.heading(3, text='Thời Gian', anchor=CENTER) trv1.heading(4, text='Cảnh Báo', anchor=CENTER) trv1.column(0, anchor=CENTER) trv1.column(1, anchor=CENTER) trv1.column(2, anchor=CENTER) trv1.column(3, anchor=CENTER) trv1.column(4, anchor=CENTER) vsb = ttk.Scrollbar(newWindow, orient="horizontal", command=trv1.xview) vsb.place(x=0, y=320, width=800) for contact in rows: trv1.insert('', tk.END, values=contact) # frame1 Giới Thiệu Label(ws, image=MC).place(x=20, y=10) Label(ws, image=MC1).place(x=700, y=10) left = Label(wrapper1, text="HỆ THỐNG LÁI TÀU TỰ ĐỘNG", font="Times 20 bold") left.place(x=200, y=20) # Frame2: Hiện Thị tk.Label(ws, text='Vĩ độ:', font='Times 12 bold').place(x=50, y=140) tk.Label(ws, text='Kinh độ:', font='Times 12 bold', fg='black').place(x=50, y=170) Long_des = tk.Entry(ws, font='Times 12 bold', width=8) Long_des.place(x=120, y=170) Long1_des = tk.Entry(ws, font='Times 12 bold', width=8) Long1_des.place(x=230, y=170) Lat_des = tk.Entry(ws, font='Times 12 bold', width=8) Lat_des.place(x=120, y=140) Lat1_des = tk.Entry(ws, font='Times 12 bold', width=8) Lat1_des.place(x=230, y=140) tk.Label(ws, text='Vị trí tại:', font='Times 12 bold', fg='black').place(x=150, y=110) tk.Label(ws, text='Vị trí đích đến:', font='Times 12 bold', fg='black').place(x=400, y=110) tk.Label(ws, text='Trạng Thái:', font='Times 12 bold', fg='black').place(x=650, y=110) tk.Label(ws, text='Thời Gian:', font='Times 12 bold', fg='black').place(x=50, y=210) tk.Label(ws, text='Độ', font='Times 12 bold', fg='black').place(x=200, y=140) tk.Label(ws, text='Phút', font='Times 12 bold', fg='black').place(x=310, y=140) tk.Label(ws, text='Độ', font='Times 12 bold', fg='black').place(x=200, y=170) tk.Label(ws, text='Phút', font='Times 12 bold', fg='black').place(x=310, y=170) tk.Label(ws, text='Độ', font='Times 12 bold', fg='black').place(x=450, y=140) tk.Label(ws, text='Phút', font='Times 12 bold', fg='black').place(x=550, y=140) tk.Label(ws, text='Độ', font='Times 12 bold', fg='black').place(x=450, y=170) tk.Label(ws, text='Phút', font='Times 12 bold', fg='black').place(x=550, y=170) tk.Label(ws, text='Góc Đặt:', font='Times 12 bold', fg='black').place(x=600, y=140) tk.Label(ws, text='Góc HT:', font='Times 12 bold', fg='black').place(x=600, y=170) tk.Label(ws, text='Khoảng Cách:', font='Times 12 bold', fg='black').place(x=600, y=200) tk.Label(ws, text='Per Out:', font='Times 12 bold', fg='black').place(x=600, y=230) tk.Label(ws, text='Speed:', font='Times 12 bold', fg='black').place(x=600, y=260) txt3 = tk.Text(ws, width=8, height=1, font='Times 12 bold', wrap=tk.WORD) txt3.place(x=380, y=140) txt4 = tk.Text(ws, width=8, height=1, font='Times 12 bold', wrap=tk.WORD) txt4.place(x=380, y=170) txt10 = tk.Text(ws, width=8, height=1, font='Times 12 bold', wrap=tk.WORD) txt10.place(x=480, y=140) txt11 = tk.Text(ws, width=8, height=1, font='Times 12 bold', wrap=tk.WORD) txt11.place(x=480, y=170) txt = tk.Text(ws, width=9, height=1, font='Times 12 bold', wrap=tk.WORD) txt.place(x=700, y=170) txt1 = tk.Text(ws, width=9, height=1, font='Times 12 bold', wrap=tk.WORD) txt1.place(x=700, y=140) txt9 = tk.Text(ws, width=9, height=1, font='Times 12 bold', wrap=tk.WORD) txt9.place(x=700, y=200) txt2 = tk.Text(ws, width=9, height=1, font='Times 12 bold', wrap=tk.WORD) txt2.place(x=700, y=230) txt7 = tk.Text(ws, width=9, height=1, font='Times 12 bold', wrap=tk.WORD) txt7.place(x=700, y=260) txt6 = tk.Text(ws, width=20, height=1, font='Times 12 bold', wrap=tk.WORD) txt6.place(x=380, y=210) txt12 = tk.Text(ws, width=20, height=1, font='Times 12 bold', wrap=tk.WORD) txt12.place(x=130, y=210) button = tk.Button(ws, text="Cấu Hình", font='Times 16 bold', bg='White', fg='Black', command=lambda: Window()) button.place(x=330, y=320) button1 = tk.Button(ws, text="Cảnh Báo", font='Times 16 bold', bg='White', fg='Black', command=lambda: Window1()) button1.place(x=450, y=320) button2 = tk.Button(ws, text="Động Cơ", font='Times 16 bold', bg='White', fg='Black', command=lambda: New_Window1()) button2.place(x=570, y=320) button4 = tk.Button(ws, text="Tự Lái", image=off, bd=0, command=lambda: switch()) button4.place(x=10, y=320) button5 = tk.Button(ws, text="Dừng", font='Times 16 bold', bg='red', fg='Black', command=lambda: close()) button5.place(x=700, y=320) def close(): global condition, heading, abc, angle_m, per_out, is_on condition = heading = abc = angle_m = per_out = if is_on: button4.config(image=off) is_on = False tk.Label(ws, text='Đang dừng ', font='Times 12 bold', fg='black').place(x=250, y=300) def set(): # ép kiểu cho số nhập kiểu nhập kiểu tring mà tính tốn lại float global az, bz, cz, dz, fz, condition, a, b, c, zz, zz1 try: a = az b = bz c = cz ccv = Lat_des.get() ddv = Long_des.get() ccv1 = Lat1_des.get() ddv1 = Long1_des.get() if(ccv != '' and ccv1 != ''): dz = float(ccv)*100 + float(ccv1) zz = float(ccv) + float(ccv1)/60 if(ccv != '' and ccv1 != ''): fz = float(ddv)*100 + float(ddv1) zz1 = float(ddv) + float(ddv1)/60 except AttributeError: pass except ValueError: pass except TypeError: pass def send(data1, port=50000, addr='239.192.1.100'): """send(data[, port[, addr]]) - multicasts a UDP datagram.""" # Create the socket sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) # UDP sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) sock.bind((addr, 0)) # Send the data sock.sendto(bytes(data1, "ascii"), (addr, port)) def recv(port=50000, addr="239.192.1.100", buf_size=1024): """recv([port[, addr[,buf_size]]]) - waits for a datagram and returns the data.""" # Create the socket s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Set some options to make it multicast-friendly s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) try: s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) except AttributeError: pass # Some systems don't support SO_REUSEPORT s.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 2) s.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1) # Bind to the port s.bind(('', port)) # Set some more multicast options intf = socket.gethostbyname(socket.gethostname()) s.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(intf)) s.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, socket.inet_aton(addr) + socket.inet_aton(intf)) # Receive the data, then unregister multicast receive membership, then close the port data, sender_addr = s.recvfrom(buf_size) s.setsockopt(socket.IPPROTO_IP, socket.IP_DROP_MEMBERSHIP, socket.inet_aton(addr) + socket.inet_aton('0.0.0.0')) s.close() return data try: gps1 = serial.Serial(gps, baudrate=9600) except serial.SerialException as e: dt = datetime.datetime.now() crr_time = dt.strftime("%H:%M:%S") rows1 = ('1', 'E1', 'Lỗi GPS', crr_time, 'Error') rows.append(rows1) pass try: compass1 = serial.Serial(compass, baudrate=4800) except serial.SerialException as e: dt = datetime.datetime.now() crr_time = dt.strftime("%H:%M:%S") rows2 = ('2', 'E2', 'Lỗi Hướng', crr_time, 'Error') rows.append(rows2) pass def data_send(): global lat, long, condition, heading, angle_m, lat_c, long_c, zz, zz1, xxx, setpoint if(condition == 1): val = random.randint(-17, 10) heading = angle_m + val if(heading < 90): lat = lat + xxx if (heading > 90): lat = lat - xxx long = D1*lat + D2 msg3 = str(val * -1) setpoint = float(msg3) adr.write(msg3.encode()) msg = pynmea2.GGA('GP', 'GGA', ('035832.410', str(lat), 'N', str(long), 'E', '1', '04', '2.6', '100.00', 'M', '-33.9', 'M', '', '0000')) msg = str(msg) msg2 = pynmea2.parse(msg) lat_c = msg2.latitude long_c = msg2.longitude msg1 = pynmea2.HDM('II', 'HDM', (str(heading), 'M')) msg1 = str(msg1) send(msg, 5004, '127.0.0.1') send(msg1, 5004, '127.0.0.1') txt3.delete(0.0, 'end') txt3.insert(0.0, str((int(lat/100)))) txt10.delete(0.0, 'end') txt10.insert(0.0, str((int(lat)%100))) txt4.delete(0.0, 'end') txt4.insert(0.0, str((int(long/100)))) txt11.delete(0.0, 'end') txt11.insert(0.0, str(int(long%100))) def read_gps(): global longitute_r, latitute_r, tam2, i, dz, fz, heading, string2, string, data try: string = gps1.readline() except NameError: pass except serial.SerialException: pass try: data = compass1.readline() except NameError: pass except serial.SerialException: pass gps_out = str(string) compass_out = str(data) send(compass_out, 5004, '192.168.1.65') send(gps_out, 5004, '192.168.1.65') try: string2 = string.decode("utf-8") data3 = data.decode("utf-8") msg = pynmea2.parse(string2) nmea = string2.split(',') data3 = data3.split(',') if(data3[0] == '$IIHDM'): tam2 = float(data3[1]) if((nmea[0] == '$GNRMC') or (nmea[0] == '$GPGGA')): latitute_r = msg.latitude longitute_r = msg.longitude txt3.delete(0.0, 'end') txt3.insert(0.0, ('%02d°' %(msg.latitude))) txt10.delete(0.0, 'end') txt10.insert(0.0, ('%02d′' %(msg.latitude_minutes))) txt4.delete(0.0, 'end') txt4.insert(0.0, ('%02d°' %(msg.longitude))) txt11.delete(0.0, 'end') txt11.insert(0.0, ('%02d′' %(msg.longitude_minutes))) if(nmea[0] == '$GNVTG'): txt7.delete(0.0, 'end') txt7.insert(0.0, str(msg.spd_over_grnd_kmph) + " Km/h") if (nmea[0] == '$ECWPL'): z = float(msg.lat) x = float(msg.lon) c = int(z/100) d = int(z % 100) c1 = int(x/100) d1 = int(x % 100) dz = c + d/60 fz = c1 + d1/60 except pynmea2.ParseError: pass except AttributeError: pass except UnicodeError: pass string2.replace(string2, " ") def output(a, b): global per_out, out_an, f f=b-a if ((f > 45) and (f < 180)): f = 45 if ((f < -45) and (f > -180)): f = -45 per_out = (f / 45) * 100 str1 = "N49 p" str2 = str(f) str4 = " v100 a100 O G3" str3 = ''.join([str1, str2, str4]) def get_bearing(lat1, long1, lat2, long2): dLon = (long2 - long1) x = math.cos(math.radians(lat2)) * math.sin(math.radians(dLon)) y = math.cos(math.radians(lat1)) * math.sin(math.radians(lat2)) math.sin(math.radians(lat1)) * \ math.cos(math.radians(lat2)) * math.cos(math.radians(dLon)) brng = numpy.arctan2(x, y) brng = numpy.degrees(brng) brng = brng + return brng def get_distance(lat1, lat2, long1, long2): coords_1 = (lat1, long1) coords_2 = (lat2, long2) azzz = geopy.distance.geodesic(coords_1, coords_2).km return azzz def check(e, f, g, h): global xxx if(abs(g - e) < 0.03): tk.Label(ws, text='ĐÃ ĐẾN NƠI', font='Times 12 bold', fg='red').place(x=600, y=200) xxx = else: xxx = 0.06 def solve(lat1, long1, lat2, long2): global D1, D2 a1 = lat1 b1 = c1 = long1 a2 = lat2 b2 = c2 = long2 D = a1 * b2 - a2 * b1 Dx = c1 * b2 - c2 * b1 Dy = c2 * a1 - c1 * a2 D1 = Dx/D D2 = Dy/D schedule.every(4).seconds.do(data_send) while True: abc = adr.readline().decode('utf-8') abc = abc.split(',') tg = float(abc[0]) feedback = tg * 180 / 3400 ws.update() schedule.run_pending() check(lat, long, dz, fz) solve(lat, long, dz, fz) dt = datetime.datetime.now() crr_time = dt.strftime("%H:%M:%S") if ((tam2 > 0) and (tam2 < 180)): heading = tam2 if ((tam2 < 360) and (tam2 > 180)): heading = tam2 - 360 heading = round(heading, 2) if (condition == 1): set() angle_m = get_bearing(lat_c, long_c, zz, zz1) angle_m = round(angle_m, 2) if (abs(b1 - heading) > 2): output(heading, angle_m) txt.delete(0.0, 'end') txt.insert(0.0, str(heading) + " Độ") txt2.delete(0.0, 'end') txt2.insert(0.0, str(per_out) + "%") txt1.delete(0.0, 'end') txt1.insert(0.0, str(angle_m) + " Độ") txt9.delete(0.0, 'end') txt9.insert(0.0, str(55) + " Km") txt6.delete(0.0, 'end') txt6.insert(0.0, str(' ' + crr_time)) txt12.delete(0.0, 'end') txt12.insert(0.0, ' ' + str(dt.day) + "/" + str(dt.month) + "/" + str(dt.year)) b1 = heading Phụ lục 2: Chương trình driver động #include // thu vien doc encoder #include // thu vien pid #include "pidautotuner.h"// thu vien tuning #include // eeprom luu bo nho // Change these two numbers to the pins connected to your encoder // Best Performance: both pins have interrupt capability // Good Performance: only the first pin has interrupt capability // Low Performance: neither pin has interrupt capability Encoder myEnc(3, 4);// khoi tao encoder // avoid using pins with LEDs attached String readString; //This while store the user input data int User_Input = 0; // This while convert input string into integer int REV = 0; // Set point REQUIRED ENCODER VALUE long newPosition=0; double kp = 0, ki = 0, kd = 0, a, b, c; // modify for optimal performance double input = 0, output = 0, setpoint = 0; PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); void setup() { Serial.begin(9600); pinMode(8, OUTPUT);//khoi tao chan pinMode(9, OUTPUT); pinMode(10, OUTPUT); a = EEPROM.read(0);//doc gia tri tu eeprom b = EEPROM.read(5); c = EEPROM.read(2); kp = a; ki = b/100; kd = c/100; Serial.println(kp); Serial.println(ki); Serial.println(kd); //PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); REV = 0; TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise myPID.SetTunings(kp, ki, kd, P_ON_E); myPID.SetMode(AUTOMATIC); myPID.SetSampleTime(1.5); // refresh rate of PID controller myPID.SetOutputLimits(-100, 100); // this is the MAX PWM value to move motor, here change in value reflect change in speed of motor } void loop() { digitalWrite(8, HIGH);// enable h-bridge while (Serial.available()) { //Check if the serial data is available delay(3); // a small delay readString = Serial.readString(); // storing input data } if (readString.length() > 0) { //Verify that the variable contains information //Serial.println(readString.toInt()); //printing the input data in integer form User_Input = readString.toInt(); // here input data is store in integer form } if(User_Input == 181) tuning(); REV = map (User_Input, -180, 180, -3400, 3400); // mapping degree into pulse setpoint = REV; newPosition = myEnc.read(); //Serial.println(REV); Serial.println(newPosition); //PID while work to achive this value consider as SET value input = newPosition; // data from encoder consider as a Process value myPID.Compute(); // calculate new output pwmOut(output); //Serial.println(output); } void pwmOut(int out) { // to H-Bridge board if (out > 0) { analogWrite(10, out); // drive motor CW digitalWrite(9, LOW); } else { digitalWrite(10, LOW); analogWrite(9, abs(out)); // drive motor CCW } } void tuning() { PIDAutotuner tuner = PIDAutotuner(); tuner.setTargetInputValue(2000); tuner.setLoopInterval(10000); tuner.setOutputRange(-200, 200); tuner.setZNMode(PIDAutotuner::ZNModeLessOvershoot); tuner.startTuningLoop(micros()); long microseconds; while (!tuner.isFinished()) { newPosition = myEnc.read(); long prevMicroseconds = microseconds; microseconds = micros(); double input = newPosition; double output = tuner.tunePID(input, microseconds); pwmOut(output); while (micros() - microseconds < 10000) delayMicroseconds(1);} pwmOut(0); kp = tuner.getKp(); ki = tuner.getKi(); kd = tuner.getKi(); Serial.println(kp); Serial.println(ki); Serial.println(0.01); int x2 = ki*100; EEPROM.write(0, kp); EEPROM.write(5, x2); EEPROM.write(2, 1); //kd = tuner.getKd(); //PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); asm volatile ( "jmp 0"); } S K L 0