Thiết kế mô hình robot tự hành trong nhà kho ứng dụng xử lý ảnh

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 CNKT ĐIỆN TỬ - TRUYỀN THÔNG THIẾT KẾ MƠ HÌNH ROBOT TỰ HÀNH TRONG NHÀ KHO ỨNG DỤNG XỬ LÝ ẢNH GVHD: ThS NGÔ BÁ VIỆT SVTH: NGUYỄN QUỐC HỒNG PHẠM NGỌC THỊNH SKL009528 Tp Hồ Chí Minh, tháng 7/2022 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO - ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ - TRUYỀN THƠNG Đề tài: THIẾT KẾ MƠ HÌNH ROBOT TỰ HÀNH TRONG NHÀ KHO ỨNG DỤNG XỬ LÝ ẢNH GVHD: SVTH: MSSV: SVTH: MSSV: ThS Ngơ Bá Việt Nguyễn Quốc Hồng 18161075 Phạm Ngọc Thịnh 18161156 TP HỒ CHÍ MINH – 07/2022 TP HỒ CHÍ MINH – 07/2022 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO - ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ - VIỄN THÔNG Đề tài: THIẾT KẾ MÔ HÌNH ROBOT TỰ HÀNH TRONG NHÀ KHO ỨNG DỤNG XỬ LÝ ẢNH GVHD: SVTH: MSSV: SVTH: MSSV: ThS Ngô Bá Việt Nguyễn Quốc Hoàng 18161075 Phạm Ngọc Thịnh 18161156 TP HỒ CHÍ MINH – 07/2022 CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc TP Hồ Chí Minh, ngày 28 tháng 07 năm 2022 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Nguyễn Quốc Hoàng MSSV: 18161075 Phạm Ngọc Thịnh MSSV: 18161156 Ngành: Công nghệ kỹ thuật Điện tử - Viễn Thông Giáo viên hướng dẫn: Ths Ngô Bá Việt Ngày nhận đề tài: 01/03/2022 Ngày nộp đề tài: 31/07/2022 Tên đề tài: Thiết kế mơ hình robot tự hành kho ứng dụng xử lý ảnh Các số liệu, tài liệu ban đầu: - Đồ Án Tốt Nghiệp (ĐATN) trường ĐHSPKT 2021 của Tạ Minh Quân, Võ Văn Lộc, “THIẾT KẾ ROBOT VẬN CHUYỂN HÀNG ỨNG DỤNG CƠNG NGHỆ DỊ LINE VÀ NHẬN DẠNG ARUCO” - CƠNG TRÌNH NGHIÊN CỨU KHOA HỌC SINH VIÊN, “NGHIÊN CỨU VÀ ỨNG DỤNG KĨ THUẬT DEEP LEARNING VÀO XE TỰ HÀNH”, đề tài nghiên cứu trường ĐH SPKT 2019 - Đồ Án Tốt Nghiệp (ĐATN) trường ĐHSPKT 2017 của Nguyễn Khánh Dương và Sỹ Quốc Bình với đề tài: “ROBOT TÌM ĐƯỜNG TRONG MÊ CUNG DÙNG GIẢI THUẬT PID” Nội dung thực đề tài: - Đọc tài liệu tham khảo, tóm tắt hướng của đề tài - Tìm hiểu thuật tốn xử lý ảnh và tìm đường - Tìm hiểu cách thức hoạt động của ngoại vi Thiết kế sơ đồ khối, sơ đồ ngun lý toàn mạch Tính tốn, thiết kế mơ hình sử dụng board Arduino, máy tính, module và động có sẵn thị trường Thiết kế mơ hình và chạy thử sản phẩm i - Viết báo cáo thực - Bảo vệ luận văn Sản phẩm: Mô hình robot tự hành kho thiết kế gờm xe tự hành với cấu di chuyển huấn luyện từ thuật toán Reinforce Learning ( học tăng cường) cụ thể Q-Table xử lý ảnh TRƯỞNG NGÀNH GIÁO VIÊN HƯỚNG DẪN ii CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Họ tên sinh viên: Nguyễn Quốc Hoàng MSSV: 18161075 Phạm Ngọc Thịnh MSSV: 18161156 Ngành: Công nghệ kỹ thuật Điện tử - Viễn Thông Tên đề tài: Thiết kế mơ hình robot tự hành kho ứng dụng xử lý ảnh Họ tên Giáo viên hướng dẫn: Ngô Bá Việt NHẬN XÉT Về nội dung đề tài & khối lượng thực hiện: - Đề tài hoàn thành tiến độ Nội dung thực phù hợp với u cầu của đề tài Mơ hình hoạt động theo mục tiêu đề - SV tích cực tự nghiên cứu lập trình xử lý ảnh tham gia làm việc nhóm để xây dựng mơ hình Ưu điểm: - Sử dụng nhận dạng Aruco Q-table để tính tốn và điều khiển robot theo lộ trình, tiết kiệm thời gian tối ưu đường Khuyết điểm: - Mơ hình cần xây dựng giao diện lựa chọn đích đến nhà kho nhằm đơn giản hóa việc sử dụng robot cho người dùng Đề nghị cho bảo vệ hay không? - Đề nghị cho bảo vệ Đánh giá loại: Giỏi Điểm: (Bằng chữ: chín) Tp Hồ Chí Minh, ngày tháng năm 2022 Giáo viên hướng dẫn iii CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN Họ tên sinh viên: Nguyễn Quốc Hồng MSSV: 18161075 Phạm Ngọc Thịnh MSSV: 18161156 Ngành: Cơng nghệ kỹ thuật Điện tử - Viễn Thông Tên đề tài: Thiết kế mơ hình robot tự hành kho ứng dụng xử lý ảnh Họ tên Giáo viên phản biện: 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: Đề nghị cho bảo vệ hay không? Đánh giá loại: Điểm:……………….(Bằng chữ: ) Tp Hồ Chí Minh, ngày tháng năm 2022 Giáo viên phản biện iv LỜI CẢM ƠN Lời đầu tiên, nhóm chúng em xin gửi chân thành gửi lời cảm ơn sâu sắc đến Quý Thầy Cô Trường Đại Học Sư Phạm Kỹ Thuật đặc biệt là thầy cô khoa Đào Tạo Chất Lượng Cao và Khoa Điện - Điện Tử truyền đạt nhiều kiến thức bổ ích và tạo điều kiện tốt cho chúng em suốt trình học tập, rèn luyện phát triển trường Đặc biệt nhóm xin gửi lời cám ơn sâu sắc với toàn chân thành của mình, tới giáo viên hướng dẫn ThS Ngô Bá Việt suốt thời gian vừa qua tận tình góp ý, dạy và hướng dẫn để chúng em hoàn thành đề tài hoàn chỉnh Được hướng dẫn của thầy, nhóm chúng em học hỏi và có thêm nhiều kinh nghiệm quý báu từ kiến thức chuyên môn kỹ cần thiết khác nhằm phục vụ cho trình phát triển thân tương lai Bên cạnh đó, nhóm em xin chân thành cảm ơn gia đình, người thân, bạn bè quan tâm, giúp đỡ, động viên và là nguồn động lực tinh thần lớn, tạo điều kiện để chúng em thực và hoàn thành Đồ án Tốt nghiệp Một lần chúng em xin chân thành cám ơn! Người thực đề tài Phạm Ngọc Thịnh Nguyễn Quốc Hoàng v LỜI CAM ĐOAN Nhóm thực Đờ Án Tốt Nghiệp cam đoan không chép nội dung và kết của đề tài và công trình nghiên cứu khác Các nội dung tham khảo trích dẫn đầy đủ Người thực đề tài Phạm Ngọc Thịnh Nguyễn Quốc Hồng vi TĨM TẮT Hiện nay, giới chuyển động thay đổi qua ngày, công nghệ ln liên tục thay đổi, máy móc dần thay người nhiều lĩnh vực đời sống, giúp đỡ của robot người dần trở thành điều quen thuộc sống Đặc biệt là lĩnh vực công nghiệp nhiều nhà máy tiến hành thay dần có mặt của người loại robot tân tiến để giúp tăng suất cho trình làm việc "Robot tự hành" là số đó, là người máy thơng minh của chúng không khác gì người cách sử dụng trí tuệ nhân tạo thuật tốn thơng minh để xử lý liệu, chúng thay người đảm nhiệm nhiệm vụ vận chuyển, giao hàng nhà máy, cửa hàng, ,các trung tâm thương mại,… Với điều tuyệt vời kích thích trí tị mị ham muốn tìm hiểu, học hỏi điều lạ của chúng em loại robot thông minh và đầy hữu ích này, chúng em định chọn đề tài “Thiết kế mơ hình robot tự hành kho” để tìm hiểu việc làm nào để chế tạo robot tự hành với khả xử lý thơng minh với là tìm hiểu thuật tốn xử lý thơng minh nay, cách áp dụng kiến thức học tập, kinh nghiệm chui rèn trường thời gian qua, với ham muốn tìm hiểu điều lạ để nhằm giải vấn đề Đề tài sử dụng thuật toán học tăng cường (RL) để tìm đường tới đích cho robot qua phần thưởng mà đạt khoảng từ -1 đến từ đưa định tối ưu Kết hợp cùng camera thu thập liệu từ môi trường nhằm xác định vị trí của robot đờ vii TÀI LIỆU THAM KHẢO [1] Trịnh Tiến Tùng - Huỳnh Thị Ngọc Như “Thiết kế mô hình robot khử khuẩn không gian y tế”, Đồ án tốt nghiệp trường DHSPKT Học kì 2020-2021 [2] Nguyễn Thị Thu Lan, “Giáo Trình: Robot Cơng Nghiệp”, Trường Cao Đẳng GTVT, Tp.HCM, 2017 [3] Nguyễn Khánh Dương, Sỹ Quốc Bình, “Robot tìm đường mê cung dùng giải thuật PID”, Đồ án tốt nghiệp, trường ĐHSPKT, Tp.HCM, 2017 [4] Võ Văn Lộc, Tạ Minh Quận “Thiết kế robot vận chuyển hàng ứng dụng cơng nghệ dị line nhận dạng Aruco”, Đờ án tốt nghiệp, trường ĐHSPKT, Tp.HCM, 2021 [5] "Giới Thiệu Xe Tự Hành AGV – Những Điều Cần Lưu Ý Về AGV", baoanjsc.com.vn, 6/2019 [6] Mai Anh, "Amazon thử nghiệm robot giao hàng", Báo Thanh Niên, 07/2021 [7] Viet Competence Trading, "Các loại cảm biến dùng xe tự hành", BLOG, Xe Tự Hành AVR [8] Đai học Đông Á, "Một số kỹ thuật định vị cho robot di động", Khoa CNKT điện điện tử, 11/2015 [9] Công ty TNHH Tự Động Hưng Phát, "Cảm biến siêu âm là gì? Ưu điểm và nhược điểm sao?" [10] "Công nghệ sử dụng máy đo khoảng cách laser", 4/2021 [11] Freecodecamp, “An introduction to Q-Learning: reinforcement learning”, 09/2018 [12] "Các loại camera quan sát thông dụng nay", Nhà Đẹp Số [13] "Mạch cầu H (Mạch cầu H là gì?)", Điện tử tương lai [14] Hoang Hang, " Mạch Buck gì? Mạch giảm điện áp? Nguyên lý ứng dụng của mạch Buck nay", 08/2021 [15] "Động điện chiều gì?", Bao An Automation Muhammad Ryan, “Let’s Build a Simple Moving Bot with Qlearning”, 11/2018 [16] Jannat Un Nisa, “Amazon Has Filed A Patent For Delivery Robots” July 20, 2021 88 [17] Nguyen Viet Anh, " Giới thiệu học tăng cường ứng dụng Deep Q-Learning chơi game CartPole", 07/2019 [18] Hà Mỹ Duyên, "Định vị Camera nhận diện điểm đánh dấu Marker", Đại học Duy Tân, 03/2022 [19] Sunita Nayak, "Augmented Reality using Aruco Markers in OpenCV (C++/ Python)", LearOnpenCV, 03/2021 [20] Wikipedia, "Vi điều khiển" , 08/2021 89 PHỤ LỤC Chương trình xử lý ảnh aruco tracking import cv2 import cv2 as cv import numpy as np import cv2.aruco as aruco import time import serial # Khai báo thưu viện serial # port = "/dev/ttyACM0" # ser = serial.Serial(port,9600) # ser = serial.Serial('/dev/ttyACM0',9600) bạn xem cap = cv2.VideoCapture(1,cv2.CAP_DSHOW) # Lưu ý x số cổng USB hồi with np.load('laptop.npz') as X: mtx, dist = [X[i] for i in ('mtx', 'dist')] #print(mtx) #print(dist) markerLength = 4.9 TT=0 u=0 ID = {} TVEC = [] dem = n= while (True): ret, frame = cap.read() frame = cv2.flip(frame, 1) dictionary = cv.aruco.Dictionary_get(cv.aruco.DICT_ARUCO_ORIGINAL) parameters = cv.aruco.DetectorParameters_create() corners, ids, rejectedCandidates = cv.aruco.detectMarkers(frame, dictionary, parameters=parameters) font = cv2.FONT_HERSHEY_SIMPLEX if np.all(ids != None): rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, markerLength, mtx, dist) # a = tvec[0,0,2] for i in range(0, ids.size): # draw axis for the aruco markers aruco.drawAxis(frame, mtx, dist, rvec[i], tvec[i], 3) # draw a square around the markers 90 aruco.drawDetectedMarkers(frame, corners, ids, (255,0,0)) # code to show ids of the marker found strg = '' for i in range(0, ids.size): strg = str(tvec[i][0]) xa = float(tvec[i][0][0]) ya = float(tvec[i][0][2]) cv2.putText(frame,"TVEC: " + strg , (0, 64), font, 0.7, (0, 255, 0), 2, cv2.LINE_AA) #break else: # code to show 'No Ids' when no markers are found cv2.putText(frame, "Khong Phat Hien Marker", (0, 64), font, 1, (0, 0, 255), 2, cv2.LINE_AA) # display the resulting frame cv2.imshow('frame', frame) if (cv2.waitKey(1) & 0xFF == ord('q')): break # When everything done, release the capture cap.release() cv2.destroyAllWindows() Chương trình hiệu chỉnh cho camera #from turtle import width import numpy as np import cv2 as cv import glob ################ FIND CHESSBOARD CORNERS - OBJECT POINTS AND IMAGE POINTS ############################# chessboardSize = (9,6) frameSize = (640,480) # termination criteria: tiêu chi để kết thúc việc hiệu chỉnh (mặc định) criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) 91 # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ,(6,5,0) objp = np.zeros((chessboardSize[0] * chessboardSize[1], 3), np.float32) objp[:,:2] = np.mgrid[0:chessboardSize[0],0:chessboardSize[1]].T.reshape(1,2) size_of_chessboard_squares_mm = 24 objp = objp * size_of_chessboard_squares_mm print(objp) # Arrays to store object points and image points from all the images objpoints = [] # 3d point in real world space imgpoints = [] # 2d points in image plane images = glob.glob('images/*.png') for image in images: img = cv.imread(image) gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) # Find the chess board corners ret, corners = cv.findChessboardCorners(gray, chessboardSize, None) # If found, add object points, image points (after refining them) if ret == True: objpoints.append(objp) corners2 = cv.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) imgpoints.append(corners) # Draw and display the corners cv.drawChessboardCorners(img, chessboardSize, corners2, ret) cv.imshow('img', img) cv.waitKey(1000) cv.destroyAllWindows() ############## CALIBRATION ####################################################### ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, frameSize, None, None) height, width, chanel = img.shape newmtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (height, width), 1, (height, width)) np.savez('laptop.npz', mtx=mtx, dist=dist) 92 Chương trình Q-table #!/usr/bin/python import numpy as np import random import serial # Serial imported for Serial communication import time # Required to use delay functions # be sure to set this to the same rate used on the Arduino SERIAL_RATE = 9600 time.sleep(1) # ArduinoSerial = serial.Serial('COM10',9600) #setting row = column = iteration = 150 targetsym = trapsym = playersym = lrate = 0.2 discount = 0.9 epsilon = 1.0 minepsilon = 0.01 maxstep = 100 decay_epsilon = 0.005 actchoice = ['MOVE_DOWN', 'MOVE_UP', 'MOVE_RIGHT', 'MOVE_LEFT'] #target target = np.asarray([3,1]) trap = [] trap.append([0,0]) trap.append([0,1]) trap.append([2,3]) trap.append([4,2]) trap = np.asarray(trap) def get_robot_route(self, direction_rb): """ Chổ nè thầy """ robot_route = []; direction = direction_rb optimal_route = self if optimal_route != None: for index, i in enumerate(optimal_route): if direction == 'DOWN': if i == "MOVE_LEFT": # LEFT robot_route.append("GO RIGHT") direction = 'LEFT' if i == "MOVE_RIGHT": # RIGHT robot_route.append("GO LEFT") direction = 'RIGHT' if i == "MOVE_DOWN": # DOWN robot_route.append("GO FORWARD") 93 direction = 'DOWN' if i == "MOVE_UP": # UP robot_route.append("GO BACKWARD") direction = 'UP' elif direction == 'LEFT': if i == "MOVE_LEFT": # LEFT robot_route.append("GO FORWARD") direction = 'LEFT' if i == "MOVE_RIGHT": # RIGHT robot_route.append("GO BACKWARD") direction = 'RIGHT' if i == "MOVE_DOWN": # DOWN robot_route.append("GO LEFT") direction = 'DOWN' if i == "MOVE_UP": # UP robot_route.append("GO RIGHT") direction = 'UP' elif direction == 'RIGHT': if i == "MOVE_LEFT": # LEFT robot_route.append("GO BACKWARD") direction = 'LEFT' if i == "MOVE_RIGHT": # RIGHT robot_route.append("GO FORWARD") direction = 'RIGHT' if i == "MOVE_DOWN": # DOWN robot_route.append("GO RIGHT") # robot_route.append("GO LEFT") direction = 'DOWN' if i == "MOVE_UP": # UP robot_route.append("GO LEFT") direction = 'UP' elif direction == 'UP': if i == "MOVE_LEFT": # LEFT robot_route.append("GO LEFT") direction = 'LEFT' if i == "MOVE_RIGHT": # RIGHT robot_route.append("GO RIGHT") direction = 'RIGHT' if i == "MOVE_DOWN": # DOWN robot_route.append("GO BACKWARD") direction = 'DOWN' if i == "MOVE_UP": # UP robot_route.append("GO FORWARD") direction = 'UP' return (robot_route) else: print("Can not find route state invalid") return def send_to_arduino(steps_robot2): robot_route = steps_robot2 94 k_x = # send_bytes = [] for each_move in robot_route: if each_move == 'GO FORWARD': # send_bytes.append(b'51') ArduinoSerial.write(b'3') print("MOVE_FORWARD") time.sleep(0.5) ArduinoSerial.write(b'3') time.sleep(1.1) #ArduinoSerial.write(b'6') #time.sleep(2) elif each_move == 'GO LEFT': if k_x == 1: ArduinoSerial.write(b'1') time.sleep(0.8) ArduinoSerial.write(b'1') print("TURN_LEFT") time.sleep(0.55) ArduinoSerial.write(b'3') print("MOVE_FORWARD") time.sleep(1.2) #ArduinoSerial.write(b'6') #time.sleep(2) k_x = else: ArduinoSerial.write(b'1') print("TURN_LEFT") time.sleep(0.8) ArduinoSerial.write(b'3') print("MOVE_FORWARD") time.sleep(1.1) #ArduinoSerial.write(b'6') #time.sleep(0.8) elif each_move == 'GO RIGHT': ArduinoSerial.write(b'4') time.sleep(0.8) ArduinoSerial.write(b'4') print("TURN_RIGHT") time.sleep(0.55) ArduinoSerial.write(b'3') print("MOVE_FORWARD") time.sleep(1.2) #rduinoSerial.write(b'6') #Qtime.sleep(2) elif each_move == 'GO BACKWARD': # send_bytes.append(b'52') # send_bytes.append('3') ArduinoSerial.write(b'1') print("TURN_BACKWARD") time.sleep(2) ArduinoSerial.write(b'3') print("MOVE_FORWARD") 95 time.sleep(1.1) def init_board(player): #make a board board = np.zeros((row, column)) #init board board[target[0], target[1]] = targetsym for j in range(len(trap)): board[trap[j,0], trap[j,1]] = trapsym board[player[0], player[1]] = playersym return board def calcreward(posplayer1, posplayer2, postarget): jarak1 = np.absolute(posplayer1-postarget) jarak2 = np.absolute(posplayer2-postarget) trapped = False #check if trapped for i in range(len(trap)): if np.array_equal(posplayer2, trap[i]): trapped = True if np.sum(jarak1-jarak2) > 0: reward = else: reward = -1 if np.array_equal(posplayer2, target): reward = 100 elif trapped: reward = -100 return reward def action(qtable, posplayer): global chance = random.uniform(0,1) if chance < epsilon: while True: choice = random.randrange(len(actchoice)) if actchoice[choice] == 'MOVE_DOWN': cnt = posplayer[0] cnt += if cnt < row: posplayer[0] = cnt break elif actchoice[choice] == 'MOVE_UP': cnt = posplayer[0] cnt -= if cnt > 0: posplayer[0] = cnt break elif actchoice[choice] == 'MOVE_RIGHT': cnt = posplayer[1] cnt += if cnt < column: 96 posplayer[1] = cnt break elif actchoice[choice] == 'MOVE_LEFT': cnt = posplayer[1] cnt -= if cnt > 0: posplayer[1] = cnt break else: choice = np.argmax(qtable[posplayer[0], posplayer[1]]) if actchoice[choice] == 'MOVE_DOWN': cnt = posplayer[0] cnt += if cnt < row: posplayer[0] = cnt elif actchoice[choice] == 'MOVE_UP': cnt = posplayer[0] cnt -= if cnt > 0: posplayer[0] = cnt elif actchoice[choice] == 'MOVE_RIGHT': cnt = posplayer[1] cnt += if cnt < column: posplayer[1] = cnt elif actchoice[choice] == 'MOVE_LEFT': cnt = posplayer[1] cnt -= if cnt > 0: posplayer[1] = cnt #decaying epsilon epsilon -= decay_epsilon return choice, posplayer def realaction(qtable, posplayer): choice = np.argmax(qtable[posplayer[0], posplayer[1]]) if actchoice[choice] == 'MOVE_DOWN': cnt = posplayer[0] cnt += if cnt < row: posplayer[0] = cnt elif actchoice[choice] == 'MOVE_UP': cnt = posplayer[0] cnt -= if cnt > 0: posplayer[0] = cnt elif actchoice[choice] == 'MOVE_RIGHT': cnt = posplayer[1] cnt += if cnt < column: posplayer[1] = cnt elif actchoice[choice] == 'MOVE_LEFT': cnt = posplayer[1] 97 cnt -= if cnt > 0: posplayer[1] = cnt return actchoice[choice], posplayer ############ #init board# ############ #make q table qtab = np.zeros((row, column, 4)) hasil = 'NONE' for i in range(iteration): if hasil == 'TARGET': break #start player player = np.asarray([3,4]) board = init_board(player) for j in range(maxstep): poslama = np.zeros((2)) poslama[:] = player choice, player = action(qtab, player) R = calcreward(poslama, player, target) #update q table qtab[int(poslama[0]), int(poslama[1]), choice] = qtab[int(poslama[0]), int(poslama[1]), choice] + lrate*(R + \ (discount**j)*(np.max(qtab[int(poslama[0]), int(poslama[1])] qtab[int(poslama[0]), int(poslama[1]), choice]))) #update board #print 'cek posisi lama', poslama, player board[int(poslama[0]), int(poslama[1])] = board[player[0], player[1]] = playersym print (board) #break if landed in trap trapped = False #check if trapped for k in range(len(trap)): if np.array_equal(player, trap[k]): trapped = True if trapped: break print (np.max(qtab)) #check solution #start player player = np.asarray([3,4]) 98 board = init_board(player) move = [] for j in range(maxstep): poslama = np.zeros((2)) poslama[:] = player nextmove, player = realaction(qtab, player) move.append(nextmove) #update board board[int(poslama[0]), int(poslama[1])] = board[player[0], player[1]] = playersym print(board) trapped = False #check if trapped for i in range(len(trap)): if np.array_equal(player, trap[i]): trapped = True if trapped: print('FAILED') break if np.array_equal(player, target): break print (move) ArduinoSerial = serial.Serial(SERIAL_PORT, SERIAL_RATE) time.sleep(1) print(send_to_arduino(get_robot_route(move,'UP'))) Code Arduino // chan nano - TX, chan nano - Rx // quay thuận in 1, 4, quay nghịch in 2,3 - động //////////////////////////////////// #include int in1=8; int in2=9; int in3=10; int in4=11; int M1_EN=3; int M2_EN=4; int bluetoothByte; void setup() { Serial.begin(9600); pinMode(in1, OUTPUT); pinMode(in2, OUTPUT); pinMode(in3, OUTPUT); pinMode(in4, OUTPUT); // delay(100); } 99 void loop() { if (Serial.available() > 0) { //đọc liệu gửi bluetoothByte = Serial.read(); Serial.println("CONNECTION IS OK!"); } if(bluetoothByte == 49) // so { Serial.println("TURN LEFT IS OK!"); //dunglai(); quaytrai(); } else if (bluetoothByte == 52)// so { Serial.println("TURN RIGHT IS OK!"); //dunglai(); quayphai(); } else if (bluetoothByte == 50) //so { Serial.println("MOVE BACKWARD IS OK!"); //dunglai(); lui(); } if (bluetoothByte == 51) //so { Serial.println("MOVE FORWARD IS OK!"); //dunglai(); dithang(); } else if (bluetoothByte == 54) // so { Serial.println("STOP IS OK!"); dunglai(); } } //////////////////////////// void quayphai () { //Serial.print(tem); digitalWrite(in1, HIGH); digitalWrite(in2, LOW); digitalWrite(in3, LOW); digitalWrite(in4, HIGH); // delay(2000); // digitalWrite(in3, LOW); // digitalWrite(in4, LOW); // digitalWrite(in1, LOW); // digitalWrite(in2, LOW); } 100 void quaytrai () { //Serial.print(tem); digitalWrite(in1, LOW); digitalWrite(in2, HIGH); digitalWrite(in3, HIGH); digitalWrite(in4, LOW); // delay(2000); // digitalWrite(in3, LOW); // digitalWrite(in4, LOW); // digitalWrite(in1, LOW); // digitalWrite(in2, LOW); } void dithang() { // Serial.print(tem); digitalWrite(in1, HIGH); digitalWrite(in2, LOW); digitalWrite(in3, HIGH); digitalWrite(in4, LOW); // delay(2000); // digitalWrite(in3, LOW); // digitalWrite(in4, LOW); // digitalWrite(in1, LOW); // digitalWrite(in2, LOW); } void lui () { // Serial.print(tem); digitalWrite(in1, LOW); digitalWrite(in2, HIGH); digitalWrite(in3, LOW); digitalWrite(in4, HIGH); // delay(2000); // digitalWrite(in3, LOW); // digitalWrite(in4, LOW); // digitalWrite(in1, LOW); // digitalWrite(in2, LOW); } void dunglai() { digitalWrite(in3, LOW); digitalWrite(in4, LOW); digitalWrite(in1, LOW); digitalWrite(in2, LOW); } 101 S K L 0