Thiết kế, chế tạo thiết bị thăm dò, kiểm tra đường ống có đường kính 300mm 3

MẪU 14KHCN 27 CHƯƠNG 3 THIẾT KẾ GIAO DIỆN VÀ CHƯƠNG TRÌNH ĐIỀU KHIỂN ROBOT (ĐĨA CD ĐÍNH KÈM) 3 1 Chương trình firmware cho board nhúng Raspberry thu ảnh, xử lý ảnh theo yêu cầu; truyền kết quả về board Arduino theo giao tiếp I2C; gửi ảnh lưu trữ ảnh trên HDD; build service chạy mặc định khi khởi động; tối ưu code chạy độc lập với hardware; 3 1 1 Lưu đồ giải thuật Hình 16 Lưu đồ giải thuật điều khiển 28 Bắt ĐầuBắt Đầu So sánh tốc độ chạy bằng thả dâySo sánh tốc độ chạy bằng thả dây Đọc tốc độ ph.

CHƯƠNG THIẾT KẾ GIAO DIỆN VÀ CHƯƠNG TRÌNH ĐIỀU KHIỂN ROBOT (ĐĨA CD ĐÍNH KÈM) 3.1 Chương trình firmware cho board nhúng Raspberry: thu ảnh, xử lý ảnh theo yêu cầu; truyền kết board Arduino theo giao tiếp I2C; gửi ảnh lưu trữ ảnh HDD; build service chạy mặc định khởi động; tối ưu code chạy độc lập với hardware; 3.1.1 Lưu đồ giải thuật Hình 16: Lưu đồ giải thuật điều khiển 27 Bắt Bắt Đầu Đầu Đ So So sánh sánh tốc tốc độ độ chạy chạy bằng thả thả dây dây S Đọc Đọc tốc tốc độ độ phần thả phần thả dây dây Kết Kết thúc thúc Đọc Đọc tốc tốc độ độ của robot robot Thay Thay đổi đổi tốc tốc độ độ của robot robot và tốc tốc độ độ thả thả dây dây Hình 17: Lưu đồ giải thuật phần thu dây tự động 3.1.2 Chương trình điều khiển Chương trình điều khiển động bánh xe #include "Arduino.h" #include #define SPEED_PIN #define DIRECTION_PIN_0 #define DIRECTION_PIN_1 #define STEPPER_PUL #define STEPPER_DIR #define RUN #define STOP #define FORWARD #define REVERSE #define SPEED_SOLUTION 25 #define MAX_SPEED 250 #define MIN_SPEED #define MAX_STEP_DELAY 250 #define MIN_STEP_DELAY bool run_stop = STOP; bool direction = FORWARD; uint8_t motor_speed = 0; int lastdir=FORWARD; 28 uint8_t step_delay = 0; uint8_t num_of_performed_step = 1; void motorRun(uint8_t direction, uint8_t speed); void stepperRun(uint8_t direction, uint8_t step_delay_time, uint32_t num_of_step); void setup() { Wire.begin(8); Wire.onReceive(receiveEvent); Serial.begin(115200); pinMode(DIRECTION_PIN_0, OUTPUT); pinMode(DIRECTION_PIN_1, OUTPUT); pinMode(SPEED_PIN, OUTPUT); pinMode(STEPPER_PUL, OUTPUT); pinMode(STEPPER_DIR, OUTPUT); } void receiveEvent(int howMany) { while (0 = SPEED_SOLUTION) motor_speed-=SPEED_SOLUTION; break;} } } } void loop(){ if(run_stop == RUN) { if(motor_speed < SPEED_SOLUTION) { motor_speed = MIN_SPEED; } step_delay = map(motor_speed, 0, 250, 70, 30); num_of_performed_step = map(motor_speed, 0, 250, 0,750); Serial.println(motor_speed); if(direction == FORWARD) { motorRun(FORWARD, motor_speed); stepperRun(FORWARD, step_delay, num_of_performed_step); } 29 else { motorRun(REVERSE, motor_speed); stepperRun(REVERSE, step_delay, num_of_performed_step); } } else { motor_speed = 0; step_delay = 0; num_of_performed_step = 1; motorRun(FORWARD, 0); } } void motorRun(uint8_t direction, uint8_t speed){ if(lastdir != direction){ delay(1000);} else{}; if(direction == FORWARD) { digitalWrite(DIRECTION_PIN_0, LOW); digitalWrite(DIRECTION_PIN_1, HIGH); lastdir=FORWARD; } else { digitalWrite(DIRECTION_PIN_0, HIGH); digitalWrite(DIRECTION_PIN_1, LOW); lastdir=REVERSE; } analogWrite(SPEED_PIN, speed); } void stepperRun(uint8_t direction, uint8_t step_delay_time, uint32_t num_of_step){ if(direction == FORWARD) { digitalWrite(STEPPER_DIR, HIGH); } else { digitalWrite(STEPPER_DIR, LOW); } for(uint32_t i = 0; i < num_of_step; i++) { digitalWrite(STEPPER_PUL, HIGH); delayMicroseconds(step_delay_time); digitalWrite(STEPPER_PUL, LOW); delayMicroseconds(step_delay_time); } } 30 Chương trình nhận tín hiệu điều khiển từ giao diện #include #include #include #ifndef STASSID #define STASSID "TP-Link_1CA2" #define STAPSK "22530785" #endif unsigned int localPort = 8888; char packetBuffer[UDP_TX_PACKET_MAX_SIZE + 1]; char ReplyBuffer[] = "acknowledged\r\n"; WiFiUDP Udp; void setup() { Serial.begin(115200); pinMode(16, OUTPUT); digitalWrite(16,HIGH); Wire.begin(D1, D2); WiFi.mode(WIFI_STA); WiFi.begin(STASSID, STAPSK); while (WiFi.status() != WL_CONNECTED) { for(uint8_t i = 0; i < 5; i++) { digitalWrite(16,LOW); delay(250); digitalWrite(16,HIGH); delay(250); } } Serial.print("Connected! IP address: "); Serial.println(WiFi.localIP()); Serial.printf("UDP server on port %d\n", localPort); Udp.begin(localPort); } void loop() { digitalWrite(16,LOW); uint8_t packetSize = Udp.parsePacket(); if (packetSize) { int n = Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE); packetBuffer[n] = 0; Wire.beginTransmission(8); Wire.write(packetBuffer); Wire.endTransmission(); Udp.endPacket(); } while (WiFi.status() != WL_CONNECTED) { WiFi.begin(STASSID, STAPSK); Wire.beginTransmission(8); Wire.write("3"); 31 { } } Wire.endTransmission(); for(uint8_t i = 0; i < 5; i++) digitalWrite(16,LOW); delay(250); digitalWrite(16,HIGH); delay(250); } 3.2 Chương trình firmware cho board điều khiển Arduino: 3.2.1 Tổng quan hệ thống Hình 18: Sơ đồ khối tổng quát hệ thống Toàn hết thống điều khiển máy tính chủ qua wifi (thơng qua router wifi) Q trình di chuyển đường ống, robot có khả khơng thể kết nối sóng wifi, switch kết nối với router dây Lan lắp theo robot giúp cho việc kết nối wifi thiết bị ổn định Module Beagle NodeMCU1 dùng để giao tiếp với máy tính robot, Module NodeMCU2 dùng để nhận tín hiệu điều khiển robot từ máy tính gửi đến Arduino lắp thùng điều khiển 32 3.2.2 Hệ thống truyền thông tin Hình 19: Sơ đồ khối hệ thống truyền nhận thơng tin Hệ thống thông tin bao gồm việc đọc, gửi tín hiệu cảm biến lấy hình ảnh gửi đến máy tính chủ Module NodeMCU1 đọc tín hiệu cảm biến AM2315 gửi tín hiệu đến máy tính chủ thơng qua wifi Hình 20: Lưu đồ giải thuật hệ thống truyền tín hiệu cảm biến NodeMCU Sau đọc tín hiệu cảm biến, NodeMCU1 đợi lệnh kiểm tra từ máy tính Trên giao diện máy tính, phần mềm ln gửi NodeMCU1 tín hiệu “1” “2” Khi nhận tín hiệu gửi từ máy tính, NodeMCU1 gán giá trị a kiểm tra xem, a=1 gửi thơng tin nhiệt độ, a=2 gửi độ ẩm đến máy tính 33 Chương trình đọc gửi tín hiệu cam biến NodeMCU1 (Khai báo thư viện) #include (Thư viện wifi ESP) #include (Thư viện giúp ESP truyền tín hiệu) #include "SDL_ESP8266_HR_AM2315.h" (Thư viện cảm biến AM2315) (Sơ đồ kết nối dây cảm biến với NodeMCU) // Connect RED of the AM2315 sensor to 5.0V // Connect BLACK to Ground // Connect WHITE to D1 // Connect YELLOW to D2 #ifndef STASSID #define STASSID "TP-Link_1CA2" (Khai báo tên wifi) #define STAPSK "22530785" (Khai báo mật wifi) #endif (Cấp địa IP tĩnh cho NodeMCU) IPAddress staticIP(192, 168, 1, 44); IPAddress gateway(192, 168, 1, 1); IPAddress subnet(255, 255, 255, 0); unsigned int localPort = 4444; (Khởi tạo q trình truyền nhận tín hiệu) char packetBuffer[UDP_TX_PACKET_MAX_SIZE + 1]; char ReplyBuffer[] = "acknowledged\r\n"; WiFiUDP Udp; (Khởi tạo cảm biến) #if asm volatile ("nop"); #endif #pragma GCC diagnostic ignored "-Wwrite-strings" extern "C" { #include "user_interface.h" } SDL_ESP8266_HR_AM2315 am2315; float dataAM2315[2]; // [0,1] => [Humidity, Temperature] boolean myOK; // 1=successful read // (Cài đặt mặc định thông số) void setup() { Serial.begin(115200); (Kết nối wifi) WiFi.mode(WIFI_STA); WiFi.begin(STASSID, STAPSK); WiFi.config(staticIP, gateway, subnet); while (WiFi.status() != WL_CONNECTED) { Serial.print('.'); 34 } delay(500); } Serial.print("Connected! IP address: "); Serial.println(WiFi.localIP()); Serial.printf("UDP server on port %d\n", localPort); Udp.begin(localPort); void loop() { int packetSize = Udp.parsePacket(); (Nếu có giá trị tín hiệu đến) if (packetSize) { (khai báo biến n giá trị đọc được) int n = Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE); packetBuffer[n] = 0; if (n == 1) { (Nếu n=1) myOK = am2315.readData(dataAM2315); Serial.print("TempF: "); Serial.println(dataAM2315[1]); int t = round(dataAM2315[1]); (Đọc nhiệt độ) Udp.beginPacket(Udp.remoteIP(), Udp.remotePort()); (Gửi nhiệt độ) Udp.print(byte(t)); } else if (n == 2) { (Nếu n=2) myOK = am2315.readData(dataAM2315); Serial.print("Hum: "); Serial.println(dataAM2315[0]); int h = round(dataAM2315[0]); (Đọc độ ẩm) Udp.beginPacket(Udp.remoteIP(), Udp.remotePort()); (Gửi độ ẩm) Udp.print(byte(h)); } } Udp.endPacket(); (Kết thúc kết nối) } Hình 21: Lưu đồ giải thuật hệ thống truyền tín hiệu cảm biến máy tính 35 Sau gửi giá trị “1” “2”, máy tính nhận luồng giá trị khác nhau, nhiệt độ độ ẩm gửi từ NodeMCU1, hai giá trị in lên hình giao diện máy tính Chương trình nhận tín hiệu cảm biến máy tính import socket (Khai báo thư viện Socket dùng để nhận tín hiệu từ NodeMCU) (Khai báo địa IP Port NodeMCU1) host = "192.168.1.44" port = 4444 def ktnhiet():(Khai báo hàm kiểm tra nhiệt độ) (Kết nối tới NodeMCU1) sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) sock.sendto(b'1',(host,port)) (Gửi “1” đến NodeMCU1) print(sock.recv(1024)) (In giá trị nhiệt độ) sock.close() (ngắt kết nối) def ktdoam():(Khai báo hàm kiểm tra độ ẩm) (Kết nối tới NodeMCU1) sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) sock.sendto(b'2',(host,port)) (Gửi “2” đến NodeMCU1) print(sock.recv(1024)) (In giá trị độ ẩm) sock.close() (ngắt kết nối) Hình 22: Lưu đồ giải thuật hệ thống truyền tín hiệu cảm biến Beagle Chương trình gửi video dến máy tính Beagle (Kết nối tới port 5555 – port máy tính) sender = imagezmq.ImageSender(connect_to="tcp://{}:5555".format(args["server_ip"])) rpiName = socket.gethostname() vs = VideoStream(usePiCamera=True).start() (đọc camera) time.sleep(2.0) (Vịng lặp q trình đọc gửi hình ảnh qua máy tính) while True: frame = vs.read() sender.send_image(rpiName, frame) Board Beagle ln đọc tín hiệu từ camera gửi khung hình đến máy tính Như giao diện máy tính ln cập nhập lúc thơng số: Nhiệt độ, độ ẩm, video 36 Chương trình điều khiển robot Arduino #include (Khai báo thư viện để nhận tín hiệu từ NodeMCU) (Khai báo chân động cơ) #define SPEED_PIN #define DIRECTION_PIN_0 #define DIRECTION_PIN_1 #define STEPPER_PUL #define STEPPER_DIR (Khai báo giá trị biến điều khiển) #define RUN #define STOP #define FORWARD #define REVERSE #define SPEED_SOLUTION 25 #define MAX_SPEED 250 #define MIN_SPEED #define MAX_STEP_DELAY 250 #define MIN_STEP_DELAY int steppermode = 0; bool run_stop = STOP; bool direction = FORWARD; uint8_t motor_speed = 125; int lastdir=FORWARD; uint8_t step_delay = 0; uint8_t num_of_performed_step = 1; void motorRun(uint8_t direction, uint8_t speed); void stepperRun(uint8_t direction, uint8_t step_delay_time, uint32_t num_of_step); void setup() { (Khai báo chân giao tiếp với NodeMCU) Wire.begin(8); Wire.onReceive(receiveEvent); Serial.begin(115200); (Cài đặt chân tín hiệu điều khiển) pinMode(DIRECTION_PIN_0, OUTPUT); pinMode(DIRECTION_PIN_1, OUTPUT); pinMode(SPEED_PIN, OUTPUT); pinMode(STEPPER_PUL, OUTPUT); pinMode(STEPPER_DIR, OUTPUT); } char data; (Đọc tín hiệu từ NodeMCU) void receiveEvent(int howMany) { while (0 = SPEED_SOLUTION) motor_speed-=SPEED_SOLUTION; break;} case '7': {steppermode=!steppermode; break;} } } } (Vòng lặp) void loop() { if (steppermode == 0) { if(run_stop == RUN) { if(motor_speed < SPEED_SOLUTION) { motor_speed = MIN_SPEED; } step_delay = map(motor_speed, 0, 250, 70, 30); num_of_performed_step = map(motor_speed, 0, 250, 0,750); if(direction == FORWARD) { motorRun(FORWARD, motor_speed); stepperRun(FORWARD, step_delay, num_of_performed_step); } else { motorRun(REVERSE, motor_speed); stepperRun(REVERSE, step_delay, num_of_performed_step); } } else { motor_speed = 0; step_delay = 0; num_of_performed_step = 1; motorRun(FORWARD, 0); } } else 43 { if(run_stop == RUN) { if(motor_speed < SPEED_SOLUTION) { motor_speed = MIN_SPEED; } step_delay = 30; num_of_performed_step = 750; motor_speed = 0; motorRun(FORWARD, motor_speed); if(direction == FORWARD) { stepperRun(FORWARD, step_delay, num_of_performed_step); } else { stepperRun(REVERSE, step_delay, num_of_performed_step); } } } } else { motor_speed = 0; step_delay = 0; num_of_performed_step = 1; motorRun(FORWARD, 0); } void motorRun(uint8_t direction, uint8_t speed) (Khai báo hàm chạy động cơ) { if(lastdir != direction){ delay(1000);} else{}; if(direction == FORWARD) { digitalWrite(DIRECTION_PIN_0, LOW); digitalWrite(DIRECTION_PIN_1, HIGH); lastdir=FORWARD; } else { digitalWrite(DIRECTION_PIN_0, HIGH); digitalWrite(DIRECTION_PIN_1, LOW); lastdir=REVERSE; } analogWrite(SPEED_PIN, speed); } void stepperRun(uint8_t direction, uint8_t step_delay_time, uint32_t num_of_step) 44 (Khai báo hàm thu thả dây) { if(direction == FORWARD) { digitalWrite(STEPPER_DIR, HIGH); } else { digitalWrite(STEPPER_DIR, LOW); } for(uint32_t i = 0; i < num_of_step; i++) { digitalWrite(STEPPER_PUL, HIGH); delayMicroseconds(step_delay_time); digitalWrite(STEPPER_PUL, LOW); delayMicroseconds(step_delay_time); } } 3.3 Phần mềm máy tính: Phần mềm truy xuất camera Robot; hiển thị thông số cảm biến; điều khiển robot theo chế độ manual Chương trình máy tính gồm giao diện có chức sau: - Hiển thị hình ảnh từ camera gắn Robot (hình ảnh gửi từ board Rapsberry PI gắn Robot) - Hiển thị thông số nhiệt độ, độ ẩm từ cảm biến gắn robot - Có nút chức để điều khiển robot theo chế độ manual 3.3.1 Lưu đồ giải thuật phần mềm xử lý ảnh Hình 26: Lưu đồ giải thuật khối xử lý ảnh giao diện máy tính 45 Chương trình máy tính def show_frame(): global lastActive global lastActiveCheck global ESTIMATED_NUM_PIS global ACTIVE_CHECK_PERIOD global ACTIVE_CHECK_SECONDS top.after(10) #_, frame = cap.read() (rpiName, frame) = imageHub.recv_image() frame = cv2.resize(frame,(1600,800)) frame = cv2.flip(frame, 1) frame = frame+c cv2image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGBA) img = Image.fromarray(cv2image) imgtk = ImageTk.PhotoImage(image=img) display1.imgtk = imgtk #Shows frame for display display1.configure(image=imgtk) ## ## nhiet_do_box.insert(0,s1.recv(1024)) do_am_box.insert(0,s2.recv(1024)) imageHub.send_reply(b'OK') if rpiName not in lastActive.keys(): print("[INFO] receiving data from {} ".format(rpiName)) lastActive[rpiName] = datetime.now() #cv2.imshow('frame',frame) key = cv2.waitKey(1) & 0xFF if (datetime.now() - lastActiveCheck).seconds > ACTIVE_CHECK_SECONDS: for (rpiName, ts) in list(lastActive.items()): if (datetime.now() - ts).seconds > ACTIVE_CHECK_SECONDS: print("[INFO] lost connection to {}".format(rpiName)) lastActive.pop(rpiName) frameDict.pop(rpiName) # set the last active check time as current time lastActiveCheck = datetime.now() top.update() #top.after(10) ###### .***KIEM TRA CAM BIEN******####### def ktnhiet(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) sock.sendto(b't',(host,port)) nhiet_do_box.delete(0,5000) #print(sock.recv(1024)) nhiet_do_box.insert(0,sock.recv(1024)) 46 sock.close() def ktdoam(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) sock.sendto(b'hh',(host,port)) do_am_box.delete(0,5000) #print(sock.recv(1024)) do_am_box.insert(0,sock.recv(1024)) sock.close() ######### ****DIEU KHIEN DONG CO*********###### def start(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'4', (UDP_IP, UDP_PORT)) def stop(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'3', (UDP_IP, UDP_PORT)) def forward(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'1', (UDP_IP, UDP_PORT)) def backward(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'2', (UDP_IP, UDP_PORT)) def increase(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'5', (UDP_IP, UDP_PORT)) def decrease(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'6', (UDP_IP, UDP_PORT)) def thu(): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'7', (UDP_IP, UDP_PORT)) ########### DIEU KHIEN CAMERA*********######### ####xu_ly_anh### def tang_do_sang(): global c c = c+5 def giam_do_sang(): global c c = c-5 47 def tang_do_tuong_phan(): global c c = c*2 def giam_do_tuong_phan(): global c if c > 4: c=c/2 def Camera_up(): global H global x if(H>0): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'1', (hostc,portc)) H=H-x if(H0): sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) # UDP sock.sendto(b'4444', (hostc,portc)) V=V-y if(V