Đồ án môn học cơ điện tử tên đề tài nghiên cứu điều khiển cánh tay robot bốn bậc tự do

BỘ CÔNG THƯƠNG TRƯỜNG ĐẠI HỌC CÔNG NGHIỆP HÀ NỘI KHOA CƠ KHÍ - ĐỒ ÁN MÔN HỌC CƠ ĐIỆN TỬ TÊN ĐỀ TÀI: NGHIÊN CỨU ĐIỀU KHIỂN CÁNH TAY ROBOT BỐN BẬC TỰ DO GVHD: TS.PHAN ĐÌNH HIẾU Nhóm sinh viên: Nguyễn Quang Trung - 0941020287 Phạm Thị Bích - 0941020267 Nguyễn Văn Hùng - 0941020253 Trương Phi Thường - 0941020277 Hà Nội – 2018 BỘ CƠNG THƯƠNG CỘNG HỒ XÃ HỘI CHỦ NGHĨA VIỆT NAM TRƯỜNG ĐH CÔNG NGHIỆP HÀ NỘI Độc lập - Tự - Hạnh phúc ĐỒ ÁN MÔN HỌC CÔNG NGHỆ CƠ ĐIỆN TỬ Số: 01 Họ tên sinh viên: Phạm Thị Bích Trương Phi Thường Nguyễn Quang Trung Nguyễn Văn Hùng Lớp: ĐH CĐT3 Khoá: Khoa: Cơ khí Giáo viên hướng dẫn: Phan Đình Hiếu NỘI DUNG Tên đề tài: Nghiên cứu điều khiển cánh tay robot bốn bậc tự Phần vẽ TT Tên vẽ Khổ giấy Số lượng Bản vẽ lắp hệ thống khí A3 Bản vẽ sơ đồ khối hệ thống điều khiển A3 Lưu đồ thuật toán điều khiển A3 Phần thuyết minh Chương 1: Giới thiệu chung Chương 2: Cơ sở lý thuyết Chương 3: Mơ hình hóa mơ hệ thống Ngày giao đề: GIÁO VIÊN HƯỚNG DẪN Ngày hồn thành: DUYỆT LỜI NĨI ĐẦU Ngày nay, thiết bị đa trở nên phổ biến sống sản xuất Với kết hợp mạnh mẽ Cơ khí - Điện tử - Tin học thời gian gần đây, thiết bị ngày thơng minh, linh hoạt hữu ích sống Với xu đó, Cơ Điện Tử chuyên ngành phát triển mạnh giới Việt Nam Với tư cách sinh viên ngành Cơ Điện Tử, trải qua năm học trường, chúng em có kiến thức tổng quát Cơ khí Điện tử - Tin học, kết hợp lĩnh vực học để thiết kế thiết bị với tính đại, hoạt động bền vững với độ tin cậy cao Với đề tài giao “Nghiên cứu điều khiển cánh tay robot bậc tự do”, chúng em nghiên cứu ứng dụng Arduino giao tiếp với máy tính phương pháp điều khiển PID vào mơ hình tay máy thiết kế Nội dung tìm hiểu chúng em viết ba chương: Chương 1: Giới thiệu chung Chương 2: Cơ sở robot công nghiệp Chương 3: Mơ hình hóa mơ điều khiển Chương 4: Thiết kế thi công hệ thống Chương 5: Kết đánh giá Trong q trình hồn thành đồ án môn Cơ điện tử, mặ dù cố gắng tìm hiểu, thiết kế, tính tốn chắn khơng tránh khỏi thiếu sót, chúng em mong góp ý thầy để đồ án chúng em hoàn thiện Chúng em xin chân thành cảm ơn nhiệt tình hướng dẫn thầy Phan Đình Hiếu thầy khoa Cơ khí trường Đại học Công nghiệp Hà Nội MỤC LỤC LỜI NÓI ĐẦU i MỤC LỤC ii DANH MỤC HÌNH ẢNH .v DANH MỤC BẢNG vii CHƯƠNG GIỚI THIỆU CHUNG 1.1 Lịch sử hình thành 1.2 Xu hướng phát triển 1.3 Nội dung nghiên cứu 1.4 Phương pháp nghiên cứu đề tài .3 1.4.1 Lý thuyết 1.4.2 Thực nghiệm 1.5 Phạm vi nghiên cứu đề tài 1.6 Lý thuyết 1.6.1 Thiết kế mơ hình CHƯƠNG CƠ SỞ ROBOT CÔNG NGHIỆP 2.1 Một số định nghĩa robot .5 2.1.1 Bậc tự robot (DOF : Degrees Of Freedom) 2.1.2 Hệ toạ độ (Coordinate frames) 2.1.3 Trường công tác robot (Workspace or Range of motion) .6 2.2 Cấu trúc phân loại robot công nghiệp .6 2.2.1 Cấu trúc robot công nghiệp 2.2.2 Phân loại robot .8 2.3 Động học động lực hoc robot 11 2.3.1 Động học robot 11 2.3.2 Động lực học robot .14 2.4 Thiết kế quỹ đạo robot 15 2.5 Cơ sở điều khiển robot 17 2.5.1 Yêu cầu điều khiển robot .17 2.5.2 Nguyên tắc điều khiển 18 2.5.3 Các phương pháp điều khiển Robot 18 2.5.4 Điều khiển lực .24 CHƯƠNG MƠ HÌNH HĨA VÀ MƠ PHỎNG ĐIỀU KHIỂN .25 3.1 Mơ hình hóa hệ thống khí 25 3.1.1 Yêu cầu toán thiết kế .25 3.1.2 Mơ hình cánh tay robot .25 3.1.3 Trường công tác 29 3.2 Giải toán robot bậc tự .29 3.2.1 Bài toán động học thuận .29 3.2.2 Bài toán động học ngược 31 3.2.3 Bài toán động lực học 33 3.2.4 Bài toán thiết kế quỹ dạo .39 3.3 Mơ hình hóa hệ thống điều khiển 42 3.3.1 Điều khiển phản hồi 42 3.3.2 Sơ đồ hệ thống điều khiển 43 3.3.3 Thiết kế điều khiển 44 3.4 Sơ đồ khối hệ thống tính tốn chọn động 52 CHƯƠNG THIẾT KẾ VÀ THI CÔNG HỆ THỐNG 54 4.1 Thiết kế hệ thống thi công hệ thống khí .54 4.1.1 Thiết kế hệ thống khí 54 4.1.2 Thi công khí 58 4.2 Thiết kế mạch điều khiển 65 4.2.1 Chọn vi điều khiển .65 4.2.2 Mạch công suất module L298: 67 4.2.3 LCD giao tiếp I2C 68 4.2.4 Tính tốn chọn động .69 4.3 Thiết kế giao diện điều khiển 71 4.3.1 Ý tưởng thiết kế giao diện 71 4.3.2 Các chế độ điều khiển 73 CHƯƠNG KẾT QUẢ VÀ ĐÁNH GIÁ 78 5.1 Kết quả, phân tích đánh giá .78 5.1.1 Kết nghiên cứu học 78 5.1.2 Kết xây dựng mơ hình 79 5.2 Hướng phát triển 79 5.2.1.Đánh giá kết đạt 79 5.2.2 Hướng phát triển 80 TÀI LIỆU THAM KHẢO 81 PHỤ LỤC 82 DANH MỤC HÌNH ẢNH Hình 2.1 Hệ tọa độ khớp robot Hình 2.2 Trường cơng tác robot Hình 2.3 Cấu trúc chung hệ robot Hình 2.4 Robot tọa độ kiểu Descarte Hình 2.5 Robot tọa độ trụ .8 Hình 2.6 Robot tọa độ cầu Hình 2.7 Robot kiểu SCARA .9 Hình 2.8 Cấu trúc hệ điều khiển hở 10 Hình 2.9 Cấu trúc hệ điều khiển phản hồi 11 Hình 2.10 Quy tắc đặt trục tọa độ Denavit - Hartenberg 12 Hình 2.11 Sơ đồ hệ thống điều khiển phản hồi 19 Hình 2.12 Sơ đồ tổng quát hệ thống điều khiển phản hồi 20 Hình 2.13 Cấu trúc điều khiển PID 21 Hình 2.14 Hệ điều khiển thích nghi theo mơ hình mẫu 23 Hình 2.15 Hệ điều khiển thích nghi tự chỉnh 23 Hình 3.1 Mơ hình u cầu làm việc 25 Hình 3.2 Mơ hình cánh tay robot 26 Hình 3.3 Khơng gian làm việc robot 29 Hình 3.4 Hệ trục tọa độ theo quy tắc Denavit – Hatenberg 29 Hình 3.5 Cấu trúc điều khiển .44 Hình 3.6 Cấu trúc điều khiển PD 44 Hình 3.7 Sơ đồ hệ điều khiển PID .45 Hình 3.8 Đồ thị đáp ứng góc quay khớp .50 Hình 3.9 Đồ thị đáp ứng vận tốc góc khớp 51 Hình 3.10 Đồ thị momen điều khiển khớp 51 Hình 3.11 Sơ đồ khối hệ điều khiển 52 Hình 4.1 Bộ truyền bánh 56 Hình 4.2 Khung đế robot 58 Hình 4.3 Khâu 60 Hình 4.4 Khâu 62 Hình 4.5 Khâu 63 Hình 4.6 Tay kẹp V2 65 Hình 4.7 Vi điều khiển Arduino Mega 2560 .65 Hình 4.8 Giao tiếp USB 2.0 67 Hình 4.9 Sơ đồ nguyên lý L298 68 Hình 4.10 Nguyên lý Encoder .70 Hình 4.11 Các bước thực nhiệm vụ điều khiển 72 Hình 4.12 Giao diện chương trình điều khiển cánh tay robot 73 Hình 4.13 Cửa số chọn cổng kết nối 73 Hình 4.14 Màn hình điều khiển Manual (Teach) .74 Hình 4.15 Màn hình điều khiển Auto 76 DANH MỤC BẢNG Bảng 1.1 Mơ hình khâu cánh tay robot .28 Bảng 1.2 Thông số kĩ thuật robot .29 Bảng 1.3 Bảng thông số động học D-H: 31 Bảng 4.1 Các chi tiết truyền đai 61 Bảng 4.2 Chi tiết phương pháp gia công khung đế 63 Bảng 4.3 Chi tiết phương pháp gia công khâu .65 Bảng 4.4 Chi tiết phương pháp gia công khâu .67 Bảng 4.5 Chi tiết phương pháp gia công khâu .69 double x2 = point_2.getX(); double y2 = point_2.getY(); double z2 = point_2.getZ(); double a = (x1 + x2)/2; double b = (y1 + y2)/2; double R = (Math.sqrt(Math.pow(x2-x1, 2) + Math.pow(y2-y1, 2)) )/ 2; double phi_1 = Math.atan2((y1-b), (x1-a)); double phi_2 = Math.atan2((y2-b), (x2-a)); double coefficient[] = TrajectoryPlanning.calculateCoefficientCubicPolynomial( phi_1, phi_2,0, 0, time); int n = (int) (time/timeStep); double dt = 0; double x[] = new double[n], y[] = new double[n], z[] = new double[n]; double phi = 0; for(int i = 0; i < n; i++){ phi = coefficient[0] + coefficient[1]*dt + coefficient[2]*Math.pow(dt, 2) + coefficient[3]*Math.pow(dt, 3); x[i] = a + R*Math.sin(phi); y[i] = b + R*Math.cos(phi); z[i] = z1; plan.add(new Point(DataNumber.handleNumber(x[i], 3), DataNumber.handleNumber(y[i], 3), DataNumber.handleNumber(z[i], 3))); dt += timeStep; } return plan; } } + Điều khiển chương trình chính: package com.phithuong.controllers; import com.fazecast.jSerialComm.SerialPort; import com.fazecast.jSerialComm.SerialPortDataListener; import com.fazecast.jSerialComm.SerialPortEvent; import com.phithuong.models.*; import com.phithuong.models.Point; import com.phithuong.views.*; import javax.swing.*; import java.awt.*; import java.awt.event.ActionEvent; import java.awt.event.ActionListener; import java.awt.event.KeyEvent; import java.awt.event.KeyListener; import java.io.*; import java.util.ArrayList; import java.util.List; import java.util.Vector; public class Controller { //Login screen private LoginScreen loginScreen; //MainProgram Screen private MainScreen mainScreen; private VectorAngle currentVectorAngle; private Point currentPoint; private RollPitchYaw currentRollPitchYaw; private VectorAngle controlVectorAngle; private Point controlPoint; private RollPitchYaw controlRollPitchYaw; private final Point homePosition = new Point(150, 150, 250); //SerialPort Screen private SerialPortScreen serialPortScreen; private SerialPortModel serialPortModel; //Text Screen private TextScreen textScreen; //Help Screen private HelpScreen helpScreen; //Logic program private boolean stateRunning = false; private boolean stateStop = true; private boolean stateReset = false; private boolean stateHome = false; private boolean stateDefault = false; //Constructor public Controller(){ super(); } //===================================================== //Handling events cause by select objects public void handlingEventsControl(){ handlingLoginScreenEvent(); handlingMainScreenEvent(); handlingButtonConnectionEvent(); } //===================================================== //Handling events main screen public void handlingMainScreenEvent(){ menuItemSerialPortActionPerform(); handlingButtonSavePositionAuto(); handlingButtonDeletePositionAuto(); } //handling button set control mode handlingButtonSetControlEvent(); //handle operation handlingButtonRunEvent(); handlingButtonStopEvent(); handlingButtonResetEvent(); handlingButtonHomeEvent(); //handle menu item handlingEventMenuItemNew(); handlingMenuItemOpen(); handlingMenuItemSave(); handlingMenuItemExit(); handlingEventMenuItemHelp(); //handle manual control handlingEventButtonIncrease(); handlingEventButtonDecrease(); handlingEventButtonSaveLocation(); handlingEventButtonDeleteLocation(); handlingButtonPickUp(); handlingButtonDrop(); handlingButtonPickUpAuto(); handlingButtonDropAuto(); //handle list position handlingButtonWriteFile(); //===================================================== //Handling logic program: Run, Stop, Reset, Home public void handlingButtonRunEvent(){ mainScreen.getBtnRun().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { if(stateDefault == true){ stateDefault = false; mainScreen.getLblRun().setBackground(Color.GREEN); mainScreen.getLblStop().setBackground(Color.WHITE); mainScreen.getLblReset().setBackground(Color.WHITE); stateRunning = true; mainScreen.getLblStateOperation().setText("Running"); btnRunActionPerform(); } } }); } public void btnRunActionPerform() { if(mainScreen.getCbTypeOfControl().getSelectedItem().equals("Auto")){ } OutputStream os = serialPortModel.getSerialPort().getOutputStream(); String fileName = "./data/program.txt"; FileHandling fileHandling = new FileHandling(fileName); fileHandling.readByLine(fileName, os); }else { return; } public void handlingButtonStopEvent(){ mainScreen.getBtnStop().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { if(stateRunning == true){ stateRunning = false; stateDefault = true; mainScreen.getLblStateOperation().setText("Stop"); mainScreen.getLblRun().setBackground(Color.WHITE); mainScreen.getLblStop().setBackground(Color.RED); mainScreen.getLblReset().setBackground(Color.WHITE); } btnStopActionPerform(); } }); } public void btnStopActionPerform(){ if(stateRunning == true){ stateRunning = false; mainScreen.getLblStop().setVisible(true); sendCommand("#stop"); stateDefault = true; }else { return; } } public void handlingButtonResetEvent(){ mainScreen.getBtnReset().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { mainScreen.getListPositionModel().removeAllElements(); if(stateRunning == true || stateDefault == true){ stateDefault = true; mainScreen.getLblStateOperation().setText("Stop"); mainScreen.getLblRun().setBackground(Color.WHITE); mainScreen.getLblStop().setBackground(Color.WHITE); mainScreen.getLblReset().setBackground(Color.RED); btnResetActionPerform(); } } } }); public void btnResetActionPerform() { if(mainScreen.getCbTypeOfControl().isEnabled() == false){ mainScreen.getCbTypeOfControl().setEnabled(true); mainScreen.getBtnSetOptionControl().setEnabled(true); } try { sendCommand("#reset"); }catch (NullPointerException ex){ ex.printStackTrace(); } } public void handlingButtonHomeEvent(){ mainScreen.getBtnHomePosition().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { if(stateDefault == true){ backToReference(); sendCommand(controlPoint.toString()); }else return; } }); } public void backToReference(){ controlPoint = homePosition; } //===================================================== = //Handling event option control public void handlingButtonSetControlEvent(){ mainScreen.getBtnSetOptionControl().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { mainScreen.getCbTypeOfControl().setEnabled(false); mainScreen.getBtnSetOptionControl().setEnabled(false); String typeControl = mainScreen.getCbTypeOfControl().getSelectedItem().toString(); String speed = String.valueOf((int)mainScreen.getSpinnerNumberModelSpeed().getValue()); String time = String.valueOf((double)mainScreen.getSpinnerNumberModelTimeSimpling().getValue()) ; String cmd = "#" + typeControl.toLowerCase() + "," + speed + "," + time; System.out.println(cmd); } } }); try { sendCommand(cmd); }catch (NullPointerException ex){ ex.printStackTrace(); } //Handling event auto control public void controlAuto(){ } public void handlingButtonPickUpAuto(){ mainScreen.getBtnPickupAuto().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { int n = mainScreen.getListPositionModel().size(); if (n == 0) mainScreen.getListPositionModel().addElement("pick"); else if (n >= 1) { if (mainScreen.getListPositionModel().getElementAt(n 1).equalsIgnoreCase("pick")) JOptionPane.showMessageDialog(null, "Command is already existed", "Error", JOptionPane.ERROR_MESSAGE); else mainScreen.getListPositionModel().addElement("pick"); } } }); } public void handlingButtonDropAuto(){ mainScreen.getBtnDropAuto().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { int n = mainScreen.getListPositionModel().size(); if (n == 0) mainScreen.getListPositionModel().addElement("drop"); else if (n >= 1) { if (mainScreen.getListPositionModel().getElementAt(n 1).equalsIgnoreCase("drop")) JOptionPane.showMessageDialog(null, "Command is already existed", "Error", JOptionPane.ERROR_MESSAGE); else mainScreen.getListPositionModel().addElement("drop"); } } } }); public void handlingButtonSavePositionAuto(){ mainScreen.getBtnSaveData().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { btnSavePositionAutoActionPerform(); } }); } public void btnSavePositionAutoActionPerform(){ if (mainScreen.getRbJointAuto().isSelected()) { if (mainScreen.getTfAngleQ1().getText().equals("") || mainScreen.getTfAngleQ2().getText().equals("") || mainScreen.getTfAngleQ3().getText().equals("") || mainScreen.getTfAngleQ4().getText().equals("") ){ JOptionPane.showMessageDialog(null, "Fields can't be empty", "Error", JOptionPane.ERROR_MESSAGE); return; } Angle angle_1 = new Angle(Double.parseDouble(mainScreen.getTfAngleQ1().getText())); System.out.println(angle_1.getAngle()); Angle angle_2 = new Angle(Double.parseDouble(mainScreen.getTfAngleQ2().getText())); Angle angle_3 = new Angle(Double.parseDouble(mainScreen.getTfAngleQ3().getText())); Angle angle_4 = new Angle(Double.parseDouble(mainScreen.getTfAngleQ4().getText())); controlVectorAngle.getListAngle().setElementAt(angle_1, 0); controlVectorAngle.getListAngle().setElementAt(angle_2, 1); controlVectorAngle.getListAngle().setElementAt(angle_3, 2); controlVectorAngle.getListAngle().setElementAt(angle_4, 3); controlPoint = ForwardKinematic.calculateForwardKinetic(controlVectorAngle); } else if (mainScreen.getRbPositionAuto().isSelected()) { if (mainScreen.getTfPositionX().getText().equals("") || mainScreen.getTfPositionY().getText().equals("") || mainScreen.getTfPositionZ().getText().equals("")){ JOptionPane.showMessageDialog(null, "Fields can't be empty", "Error", JOptionPane.ERROR_MESSAGE); return; } double X = Double.parseDouble(mainScreen.getTfPositionX().getText()); double Y = Double.parseDouble(mainScreen.getTfPositionY().getText()); double Z = Double.parseDouble(mainScreen.getTfPositionZ().getText()); controlPoint.setX(X); controlPoint.setY(Y); controlPoint.setZ(Z); } int n = mainScreen.getListPositionModel().getSize(); if (n == 0){ mainScreen.getListPositionModel().addElement(mainScreen.getCbTypeTrajectory().get SelectedItem().toString().charAt(0) + controlPoint.toString()); }else if(n >= 1){ if (mainScreen.getListPositionModel().get(n1).substring(1).equals(controlPoint.toString())){ JOptionPane.showMessageDialog(null, "This point is already existed", "Error", JOptionPane.ERROR_MESSAGE); return; }else { mainScreen.getListPositionModel().addElement(mainScreen.getCbTypeTrajectory().get SelectedItem().toString().charAt(0) + controlPoint.toString()); } } } public void handlingButtonDeletePositionAuto(){ mainScreen.getBtnDeleteData().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { if (mainScreen.getListPosition().getSelectedIndex() == -1){ JOptionPane.showMessageDialog(null, "No point is selected", "Message", JOptionPane.INFORMATION_MESSAGE); return; }else { mainScreen.getListPositionModel().removeElementAt(mainScreen.getListPosition().get SelectedIndex()); } } }); } //Handling event manual control public void controlManual(){ } public void handlingButtonPickUp(){ mainScreen.getBtnPickup().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { sendCommand("pickup"); } }); } public void handlingButtonDrop(){ mainScreen.getBtnDrop().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { sendCommand("drop"); } }); } public void handlingEventButtonIncrease(){ mainScreen.getBtnIncrease().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { if(stateRunning == true && mainScreen.getCbTypeOfControl().getSelectedItem().equals("Manual")){ btnIncreaseActionPerform(); }else { JOptionPane.showMessageDialog(null, "System isn't in running state;", "Message", JOptionPane.INFORMATION_MESSAGE); } } }); } public void btnIncreaseActionPerform(){ double[] angles = this.getRealityAngleValue(); double[] positions = this.getRealityPositionValue(); if (positions[0] > 400) positions[0] = 400; if (positions[1] > 400) positions[1] = 400; if (positions[2] > 400) positions[2] = 400; if (angles[0] > 360) angles[0] = 360; if (angles[1] > 360) angles[1] = 360; if (angles[2] > 360) angles[2] = 360; if (angles[3] > 360) angles[3] = 360; if (mainScreen.getRbManualJoint().isSelected()){ if (mainScreen.getRbJointQ1().isSelected()){ angles[0]++; controlVectorAngle.getListAngle().get(0).setAngle(angles[0]); }else if(mainScreen.getRbJointQ2().isSelected()){ angles[1]++; controlVectorAngle.getListAngle().get(1).setAngle(angles[1]); }else if(mainScreen.getRbJointQ3().isSelected()){ angles[2]++; controlVectorAngle.getListAngle().get(2).setAngle(angles[2]); }else { angles[3]++; controlVectorAngle.getListAngle().get(3).setAngle(angles[3]); } }else if (mainScreen.getRbManualPosition().isSelected()){ if (mainScreen.getRbPositionX().isSelected()){ positions[0]++; controlPoint.setX(positions[0]); }else if (mainScreen.getRbPositionY().isSelected()){ positions[1]++; controlPoint.setY(positions[1]); }else { positions[2]++; controlPoint.setZ(positions[2]); } } for (int i = 0; i < 4; i++){ System.out.println("Angle " + i + " = " + controlVectorAngle.getListAngle().get(i).getAngle()); } sendCommand("#data" + controlVectorAngle.toString()); } public void handlingEventButtonDecrease(){ mainScreen.getBtnDecrease().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { if(stateRunning == true && mainScreen.getCbTypeOfControl().getSelectedItem().equals("Manual")){ btnDecreaseActionPerform(); }else { JOptionPane.showMessageDialog(null, "System isn't in running state;", "Message", JOptionPane.INFORMATION_MESSAGE); } } }); } public void btnDecreaseActionPerform(){ double[] angles = this.getRealityAngleValue(); double[] positions = this.getRealityPositionValue(); if (mainScreen.getRbManualJoint().isSelected()) { if (mainScreen.getRbJointQ1().isSelected()) { angles[0] ; if(angles[0] < 0) controlVectorAngle.getListAngle().get(0).setAngle(angles[0]); } else if(mainScreen.getRbJointQ2().isSelected()) { angles[1] ; if(angles[1] < 0) angles[1] = 0; controlVectorAngle.getListAngle().get(1).setAngle(angles[1]); } else if(mainScreen.getRbJointQ3().isSelected()) { angles[2] ; if(angles[2] < 0) angles[2] = 0; controlVectorAngle.getListAngle().get(2).setAngle(angles[2]); } else if(mainScreen.getRbJointQ4().isSelected()) { angles[3] ; if(angles[3] < 0) angles[3] = 0; controlVectorAngle.getListAngle().get(3).setAngle(angles[3]); } } } else if (mainScreen.getRbManualPosition().isSelected()) { if (mainScreen.getRbPositionX().isSelected()) { positions[0] ; if (positions[0] < 0) positions[0] = 0; controlPoint.setX(positions[0]); } else if (mainScreen.getRbPositionY().isSelected()) { positions[1] ; if (positions[1] < 0) positions[1] = 0; controlPoint.setY(positions[1]); } else if(mainScreen.getRbPositionZ().isSelected()) { positions[2] ; if (positions[2] < 0) positions[2] = 0; controlPoint.setZ(positions[2]); } controlVectorAngle = InverseKinematic.calculateInverseKinetic(controlPoint); } sendCommand("#data" + controlVectorAngle.toString()); public void handlingEventButtonDeleteLocation(){ mainScreen.getBtnDeleteLocation().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { if (mainScreen.getListPosition().getSelectedIndex() == -1){ JOptionPane.showMessageDialog(null, "No point is selected", "Message", JOptionPane.INFORMATION_MESSAGE); return; }else { mainScreen.getListPositionModel().removeElementAt(mainScreen.getListPosition().get SelectedIndex()); } } }); } public void handlingEventButtonSaveLocation(){ mainScreen.getBtnSaveLocation().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { int n = mainScreen.getListPositionModel().getSize(); if(n == 0){ mainScreen.getListPositionModel().addElement(currentPoint.toString()); } else if (n >= 1){ String str = mainScreen.getListPositionModel().get(n-1).toString(); if (currentPoint.toString().equals(str)) JOptionPane.showMessageDialog(null, "This point is already existed.", "Message", JOptionPane.INFORMATION_MESSAGE); else mainScreen.getListPositionModel().addElement(currentPoint.toString()); } } }); } //===================================================== = // Handling events serialPort screen public void handlingButtonConnectionEvent(){ serialPortScreen.getButtonConnect().addActionListener(new ActionListener() { @Override public void actionPerformed(ActionEvent e) { String portName = serialPortScreen.getCbSerialPort().getSelectedItem().toString(); int baudRate = Integer.parseInt(serialPortScreen.getCbBaudRate().getSelectedItem().toString()); if(serialPortScreen.getButtonConnect().getText().equals("Connect")){ SerialPort chosenPort = SerialPort.getCommPort(portName); serialPortModel = new SerialPortModel(chosenPort); serialPortModel.configurationSerialPort(baudRate, 8, 1, 0); chosenPort.openPort(); stateDefault = true; chosenPort.addDataListener(new SerialPortDataListener() { @Override public int getListeningEvents() { return SerialPort.LISTENING_EVENT_DATA_AVAILABLE; } @Override public void serialEvent(SerialPortEvent serialPortEvent) { if(serialPortEvent.getEventType() == SerialPort.LISTENING_EVENT_DATA_AVAILABLE){ updateActualPosition(); } } }); serialPortScreen.setSerialPortModel(serialPortModel); //Set label if(serialPortModel.getSerialPort().isOpen()){ serialPortScreen.getButtonConnect().setText("Disconnect"); serialPortScreen.getLblStateConnection().setText("State: Connected"); serialPortScreen.getCbSerialPort().setEnabled(false); serialPortScreen.getCbBaudRate().setEnabled(false); } }else { serialPortScreen.getSerialPortModel().getSerialPort().closePort(); serialPortScreen.getButtonConnect().setText("Connect"); serialPortScreen.getLblStateConnection().setText("State: Not connected"); serialPortScreen.getCbSerialPort().setEnabled(true); serialPortScreen.getCbBaudRate().setEnabled(true); } } }); } public void updateActualPosition(){ InputStream is = serialPortModel.getSerialPort().getInputStream(); BufferedInputStream bis = new BufferedInputStream(is); String result = ""; try { char ch = (char)bis.read(); while(ch != -1){ result += ch; ch = (char)bis.read(); } bis.close(); is.close(); }catch (IOException ex){ ex.printStackTrace(); } currentVectorAngle = new VectorAngle().convertStringToVectorAngle(currentVectorAngle, result); currentPoint = ForwardKinematic.calculateForwardKinetic(currentVectorAngle); //Show on UI mainScreen.getLblAngleQ1().setText(String.valueOf(currentVectorAngle.getListAngle( ).get(0).getAngle())); mainScreen.getLblAngleQ2().setText(String.valueOf(currentVectorAngle.getListAngle( ).get(1).getAngle())); mainScreen.getLblAngleQ3().setText(String.valueOf(currentVectorAngle.getListAngle( ).get(2).getAngle())); mainScreen.getLblAngleQ4().setText(String.valueOf(currentVectorAngle.getListAngle( ).get(3).getAngle())); } mainScreen.getLblPositionX().setText(String.valueOf(currentPoint.getX())); mainScreen.getLblPositionY().setText(String.valueOf(currentPoint.getY())); mainScreen.getLblPositionZ().setText(String.valueOf(currentPoint.getZ())); public boolean sendCommand(String cmd){ OutputStream os = null; BufferedOutputStream bos = null; try { os = serialPortModel.getSerialPort().getOutputStream(); bos = new BufferedOutputStream(os); }catch (NullPointerException ex){ ex.printStackTrace(); } try { for(int i = 0; i < cmd.length(); i++){ bos.write(cmd.charAt(i)); } bos.close(); return true; }catch(IOException ex){ ex.printStackTrace(); } return false; } public void sendDataToArduino(String data){ byte[] bytes = data.getBytes(); OutputStream os = serialPortModel.getSerialPort().getOutputStream(); BufferedOutputStream bos = new BufferedOutputStream(os); try { bos.write(bytes); bos.close(); }catch (IOException ex){ ex.printStackTrace(); } } //Useful Functionality public double[] getRealityAngleValue(){ double[] angles = new double[4]; double q1 = Double.parseDouble(mainScreen.getLblAngleQ1().getText()); double q2 = Double.parseDouble(mainScreen.getLblAngleQ2().getText()); double q3 = Double.parseDouble(mainScreen.getLblAngleQ3().getText()); double q4 = Double.parseDouble(mainScreen.getLblAngleQ4().getText()); angles[0] = q1; angles[1] = q2; angles[2] = q3; angles[3] = q4; return angles; } public double[] getRealityPositionValue(){ double[] positions = new double[3]; positions[0] = Double.parseDouble(mainScreen.getLblPositionX().getText()); positions[1] = Double.parseDouble(mainScreen.getLblPositionY().getText()); positions[2] = Double.parseDouble(mainScreen.getLblPositionZ().getText()); return positions; } }