Thiết kế và chế tạo robot cân băng trên quả cầu

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO ROBOT CÂN BẰNG TRÊN QUẢ CẦU Người hướng dẫn: Sinh viên thực hiện: Số thẻ sinh viên : Lớp: TS NGUYỄN DANH NGỌC CAO THANH BỘ PHẠM TRƯỜNG HƯNG 101130153 101130164 13CDT1 Đà Nẵng, 2018 TÓM TẮT Tên đề tài: Thiết kế chế tạo robot cân băng cầu Sinh viên thực hiện: Phạm Trường Hưng Số thẻ SV: 101130164 Lớp: 13CDT1 Sinh viên thực hiện: Cao Thanh Bộ Số thẻ SV: 101130153 Lớp: 13CDT1 Đề tài thực với mục đích chế tạo mơ hình để nghiên cứu thuật toán điều khiển phức tạp, ứng dụng việc giảng dạy trường Ngồi mơ hình thành cơng có số ứng dụng sau: Robot lễ tân, robot an ninh robot vận chuyển hàng kho… Đề tài tiến hành nghiên cứu số nội dung sau: • Tìm hiều tổng quan đề tài, lịch sử đời nguyên mẫu trước thực giới • Từ mơ hình thiết kế, ta tiến hành mơ hình hóa mơ hình mơ hình tốn, từ phân tích động học động lực học Từ phân tích ta tiến hành nghiên cứu thuật toán điều khiển phù hợp • Hai phương pháp điều khiển đưa để nghiên cứu điều khiển cho mơ hình Bộ điều khiển PID Bộ điều khiển LQR Đề tài nghiên cứu tìm hiểu hai điều khiển này, ưu nhược điểm phương pháp tính tốn, phương pháp thực nghiệm • Nghiên cứu thiết kế mơ hình khí, phù hợp với ứng dụng cụ thể • Tìm hiểu thành phần phần cứng phù hợp để điều khiển mơ mạch xử lý góc, mạch xử lý trung tâm động cơ, thành phần quan trọng • Đề tài nghiên cứu qua số chuẩn giao tiếp vi điều khiển để ứng dụng mô hình Cuối đề tài rút số kết luận ảnh hưởng kết cấu đến việc điều khiển hiệu điều khiển mơ hình ĐẠI HỌC ĐÀ NẴNG CỘNG HỊA XÃ HƠI CHỦ NGHĨA VIỆT NAM TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ Độc lập - Tự - Hạnh phúc NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP TT Họ tên sinh viên Số thẻ SV Lớp Ngành Phạm Trường Hưng 101130164 13CDT1 Cơ điện tử Cao Thanh Bộ 101130153 13CDT1 Cơ điện tử Tên đề tài đồ án: Thiết kế chế tạo Robot cân cầu Đề tài thuộc diện: ☐ Có ký kết thỏa thuận sở hữu trí tuệ kết thực Các số liệu liệu ban đầu: …………………………………… …………………………………………… …… ………………………………………………………………………………………… … ………………………………….… ……………………… ……………………… Nội dung phần thuyết minh tính tốn: a Phần chung: TT Họ tên sinh viên Nội dung Phạm Trường Hưng Cao Thanh Bộ • Tìm hiểu tổng quan đề tài • Xây dựng mơ hình tốn • Kết luận đánh giá kết b Phần riêng: TT Họ tên sinh viên Phạm Trường Hưng Cao Thanh Bộ Nội dung • Thiết kế chế tạo ngun mẫu robot • Tìm hiểu thiết kế thiết kế điều khiển Các vẽ, đồ thị ( ghi rõ loại kích thước vẽ ): TT Họ tên sinh viên Phạm Trường Hưng Cao Thanh Bộ Nội dung • • • • • Kết cấu khí Sơ đồ mạch Mơ hình mô Sơ đồ khối chức Lưu đồ thuật toán Họ tên người hướng dẫn: Phần/ Nội dung: TS Nguyễn Danh Ngọc Thiết kế điều khiển TS Lê Hoài Nam Ngày giao nhiệm vụ đồ án: Ngày hồn thành đồ án: Thiết kế khí …… /……./201… …… /……./201… Đà Nẵng, ngày tháng năm 201 Trưởng Bộ mơn……………………… Người hướng dẫn LỜI NĨI ĐẦU Ngày nay, với phát triển công nghiệp, cách mạng công nghiệp lần thứ tư diễn Các nhà máy dây chuyền ngày đại với mức độ xác tự động hóa cao, điều địi hỏi người kĩ sư làm việc cần phải có kiến thức chun mơn vững vàng phải rèn luyện kĩ sinh viên Đặc biệt sinh viên nghành Cơ điện tử, với đặc thù ngành phải biết nhiều rộng, từ khí, điện tử lập trình phải rèn luyện kĩ hơn, nên có kinh nghiệm thực tế nhiều Với mong muốn tạo mô hình, mà từ đưa vào ứng dụng vào việc giảng dạy nhà trường để nâng cao khả sinh viên, chúng em định chọn đề tài “Thiết kế chế tạo Robot cân cầu” Mơ hình kết hợp khó nhiều kĩ năng, yêu cầu phải có kiến thức thiết kế CAD/CAM, có khả tính tốn mơ hình hóa mơ hình, xử lý tín hiệu cảm biến, thiết kế điều khiển khả lập trình nhúng Từ đó, sinh viên có nhiều kiến thức thơng qua việc học tập dựa mơ hình thực Nếu thành cơng, ngồi mục đích ứng dụng vào giảng dạy với khả linh động kích thước mình, mơ hình cịn ứng dụng vào làm Robot lễ tân, Robot an ninh Robot chở hàng kho… Nội dung báo cáo Đồ án tốt nghiệp gồm chương: Chương TỔNG QUAN VỀ ROBOT CÂN BẰNG TRÊN QUẢ CẦU Chương MƠ HÌNH TOÁN HỌC Chương THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN Chương THIẾT KẾ VÀ CHẾ TẠO NGUYÊN MẪU ROBOT Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN Nhân đây, em xin đại diện cho nhóm xin gởi lời cảm ơn chân thành sâu sắc đên thầy Khoa Cơ khí, trường Đại học Bách khoa – Đại học Đà nẵng, suốt năm qua giảng dạy, truyền thụ kiến thức cho chúng em Xin gởi lời cảm ơn đặc biệt đến Thầy TS Nguyễn Danh Ngọc, người hỗ trợ mặt kiến thức cho nhóm, kịp thời động viên lúc khó khăn Tuy kết mơ hình chưa thành cơng, thực giúp đỡ thầy vô có ý nghĩa với nhóm Một lần em xin chân thành cảm ơn.! Nhóm sinh viên thực i CAM ĐOAN Tôi xin cam đaong đồ án tơi tự tính tốn, thiết kế nghiên cưứu hướng dẫn TS Nguyễn Danh Ngọc Để hồn thành đồ án nầy, tơi sử dụng tài liệu ghi mục taài liệu tham khảo, ngồi khơng sử dụng tài liệu khác mà không ghi Nếu sai xin chịu hình thức kỷ luật theo quy định Nhóm sinh viên thực Phạm Trường Hưng Cao Thanh Bộ ii Tóm tắt Nhiệm vụ đồ án Lời nói đầu cảm ơn Lời cam đoan liêm học thuật i ii Mục lục Danh sách bảng biểu, hình vẽ sơ đồ iii vi Danh sách cụm từ viết tắt viii Trang Chương 1.1 TỔNG QUAN VỀ ROBOT CÂN BẰNG TRÊN QUẢ CẦU Khái niệm cấu trúc 1.1.1 Khái niệm 1.1.2 Cấu trúc 1.2 Lịch sử phát triển Chương 2.1 MÔ HÌNH TỐN HỌC Giả thiết mơ hình 3D 2.1.1 Giả thiết 2.1.2 Mơ hình 3D 2.2 Hệ tọa độ phép biến đổi hệ toạ độ 2.2.1 Định nghĩa hệ toạ độ 2.2.2 Các phép biến đổi hệ tọa độ 2.3 Xây dựng mơ hình tốn học 2.3.1 Các biến số mơ hình 2.3.2 Tham số mơ hình 11 2.3.3 Phương trình chuyển động 14 2.3.4 Mơ hình khơng gian trạng thái 15 2.4 Mô đáp ứng hệ thống 19 2.4.1 Đáp ứng tự 20 2.4.2 Đáp ứng cưỡng 21 iii Chương 3.1 THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 22 Nền tảng lí thuyết điều khiển PID 22 3.1.1 Cấu trúc tổng quan 22 3.1.2 Điều chỉnh thông số điều khiển PID 23 3.2 Nền tảng lí thuyết điều khiển LQG 23 3.2.1 Điều khiển không gian trạng thái 23 3.2.2 Bộ điều chỉnh LQR 24 3.2.3 Bộ lọc Kalman 27 3.3 Thiết kế hệ thống điều khiển cho BBR 29 3.3.1 Thiết kế hệ thống điều khiển PID 29 3.3.2 Bộ điều khiển LQG 31 3.3.3 Đánh giá chất lượng điều khiển 32 Chương THIẾT KẾ VÀ CHẾ TẠO NGUYÊN MẪU ROBOT 35 4.1 Kiến trúc tổng quan 35 4.2 Thiết kế kết cấu khí 36 4.2.1 Yêu cầu chung 36 4.2.2 Ảnh hưởng thông số mơ hình đến chất lượng điều khiển 36 4.2.3 Thiết kế mơ hình 3D 37 4.2.4 Lựa chọn vật liệu, phương pháp gia công chi tiết khí 39 4.2.5 Thơng số mơ hình 40 4.3 Thiết kế hệ thống điện tử 40 4.3.1 Sơ đồ khối hệ thống điện tử 40 4.3.2 Khối xử lý trung tâm 41 4.3.3 Khối nguồn 43 4.3.4 Khối cảm biến góc nghiêng 45 4.3.5 Khối điều khiển động 46 4.3.6 Khối điều khiển từ xa 49 4.3.7 Khối thu thập liệu, giao tiếp máy tính 49 iv 4.4 Chế tạo nguyên mẫu 49 4.4.1 Chế tạo lắp ráp nguyên mẫu robot 49 4.4.2 Chế tạo hệ thống giá treo di động hỗ trợ thực nghiệm 50 4.5 Lưu đồ thuật tốn chương trình điều khiển 51 4.5.1 Chương trình xử lý góc 51 4.5.2 Chương trình điều khiển trung tâm 53 4.5.3 Chương trình điều khiển động 54 4.6 Hệ thống thu thập liệu 55 Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 57 5.1 Kết luận 57 5.2 Một số kinh nghiệm hướng phát triển 57 TÀI LIỆU THAM KHẢO 58 Phụ lục A – Xác định ma trận Jacobian ánh xạ từ vector vận tốc biến khớp đến khâu chấp hành cuối Phụ lục B - Momen quán tính khối lăng trụ đặc với trục quay B1 – Momen quán tính khối lăng trụ đặc với trục x y B2 – Momen quán tính khối lăng trụ đặc với trục z Phụ lục C - Xác định ma trận J Phụ lục D - Xác định ma trận M, C, G 15 Phụ lục E - Xác định ma trận A, B, C 22 Phụ lục F - Các thông số mơ hình BBR 28 Phụ lục G – Chương trình điều khiển 30 v DANH SÁCH CÁC HÌNH VẼ Hình 1.1 - Môt số nguyên mẫu robot (từ trái qua phải) Đại học CMU , TGU, ETH Zurich Hình 2.1- Mơ hình BBR 3D Hình 2.2 - Các hệ tọa độ Hình 2.3 - Vector vận tốc bóng Hình 2.4 - Vị trí xếp bánh xe đa hướng 10 Hình 2.5 - Ước lượng momen qn tính phần thân 12 Hình 2.6 - Mô tả hệ hở Simulink 19 Hình 2.7 - Khối BBR_3D Simulink 20 Hình 2.8 - Đáp ứng tự với góc pitch ban đầu 0.10 21 Hình 2.9 - Đáp ứng cưỡng với momen ban đầu 0.01N.m 21 Hình 3.1 - Sơ đồ khối hệ thống điều khiển không gian trạng thái [2] 24 Hình 3.2 - Q trình dự đốn cập nhật 29 Hình 3.3 - Hệ thống điều khiển BBR với điều khiển PID 30 Hình 3.4 Hệ thống điều khiển BBR với điều khiển LQG 32 Hình 3.5 Đáp ứng hệ thống cân 32 Hình 3.6 - Đáp ứng hệ thống với điều khiển LQG PID vọt lố nhỏ tín hiệu đặt thay đổi: (a) thay đổi xs; (b) thay đổi ys (c) thay đổi 𝜓𝑧 33 Hình 3.7 - Đáp ứng hệ thống với điều khiển LQG PID đáp ứng nhanh tín hiệu đặt thay đổi: (a) thay đổi xs; (b) thay đổi ys (c) thay đổi 𝜓𝑧 33 Hình 4.1 - Kiến trúc tổng quan nguyên mẫu BBR 35 Hình 4.2 - Mơ hình 3D nguyên mẫu BBR 37 Hình 4.3 - Mơ hình 3D nguyên mẫu BBR: phần đế 38 Hình 4.4 - Mơ hình 3D ngun mẫu BBR: phận 38 Hình 4.5 - Bóng rổ size 39 Hình 4.6 - Bánh xe đa hướng loại hàng (trái) nối trục (phải) 40 Hình 4.7 – Sơ đồ khối hệ thống điện tử 40 Hình 4.8 - Arduino Mega2560 42 Hình 4.9 - Module thu phát HC-11 CC1101 43 Hình 4.10 - Acquy 44 Hình 4.11 - Module hạ áp LM2596 44 Hình 4.12 - Kit điều khiển Arduino Nano 45 Hình 4.13 - Cảm biến góc nghiêng MPU6050 HMC5883L 46 vi Phụ lục G – Chương trình điều khiển Chương trình xử lý góc #include #include "I2Cdev.h" #include "MPU6050.h" #include "kalmanft.h" #include "HMC5883L.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif MPU6050 accgyro; HMC5883L compass; KFL kalX; KFL kalY; KFL kalZ; int16_t ax, ay, az, gx, gy, gz; double pitch, roll, yaw, gxrate, gyrate, gzrate; volatile double kalroll, kalpitch, kalyaw, time1; volatile double comp_roll, comp_pitch, comp_yaw; double prekalroll, prekalpitch, precomp_yaw; double psi_x_dot, psi_y_dot, psi_z_dot ; // -typedef struct TIME_ { float dt = 0; long lastTime = 0; } time_; time_ timeLoopTest; // -SPI typedef struct angleDouble { double psi_x; double psi_y; double psi_z; }; typedef struct angleInt { int psi_x; int psi_y; Phụ lục 30 int psi_z; }; angleDouble bodyDouble; angleInt bodyInt; typedef struct angleDataDouble { double psi_x; double psi_y; double psi_x_dot; double psi_y_dot; }; typedef struct angleDataInt { int psi_x; int psi_y; int psi_x_dot; int psi_y_dot; }; angleDataDouble bodyDataDouble; angleDataInt bodyDataInt; // void setup() { // SPI -SPI.begin (); // Slow down the master a bit SPI.setClockDivider(SPI_CLOCK_DIV128); // #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif Serial.begin(115200); ////Serial.println(" doi khoi dong giao tiep I2C mot lat "); accgyro.initialize(); Phụ lục 31 //set hmc5883l compass.begin(); compass.setRange(HMC5883L_RANGE_1_3GA); compass.setMeasurementMode(HMC5883L_CONTINOUS); compass.setDataRate(HMC5883L_DATARATE_30HZ); compass.setSamples(HMC5883L_SAMPLES_8); compass.setOffset(0, 0); //end set accgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); pitch = atan(- (double) ax / sqrt((double) ay * (double) ay + (double) az * (double) az )) * (180.0 / 3.14159); roll = (atan2(ay, az) * 180.0) / 3.14159; time1 = micros(); // comp_roll = roll; // comp_pitch = pitch; kalX.setangle(roll); kalY.setangle(pitch); } void loop() { // timeLoopTest.lastTime = micros(); //********************************* //hmc5883l Vector norm = compass.readNormalize(); //lay du lieu hmc5883l //****************************************************************************** //mpu6050 accgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); gxrate = (double)gx / 131.0; //set o 250o/s gyrate = (double)gy / 131.0; gzrate = (double)gz / 131.0; roll = (atan2(ay, az) * 180.0) / 3.14159; pitch = atan(- (double) ax / sqrt((double) ay * (double) ay + (double) az * (double) az )) * (180.0 / 3.14159); double dt = (micros() - time1) / 1000000.0; time1 = micros(); //****************************************************************************** *************** // lọc bu // comp_roll = Phụ lục 0.85 * (comp_roll + gxrate * dt) + 0.15 * roll ; 32 // comp_pitch = 0.93 * (comp_pitch + gyrate * dt) + 0.07 * pitch; kalroll = (kalX.getangle(roll, gxrate, dt));//-0.28 +1.2 kalpitch = (kalY.getangle(pitch, gyrate, dt)); //+2.75 - 1.51 double Bfy = norm.ZAxis * sin(comp_roll) - norm.YAxis * cos(comp_roll); double Bfx = norm.XAxis * cos(comp_pitch) + norm.YAxis * sin(comp_pitch) * sin(kalroll) + norm.ZAxis * sin(comp_pitch) * cos(comp_roll); yaw = atan2(-Bfy, Bfx) * 180 / PI; if ((yaw < -90 && comp_yaw > 90) || (yaw > 90 && comp_yaw < -90)) comp_yaw = yaw; comp_yaw = (0.98 * (comp_yaw + gzrate * dt) + 0.02 * yaw); // SPI bodyDouble.psi_x = rount_double(kalroll); //-0.3 bodyDouble.psi_y = rount_double(kalpitch); //+2 bodyDouble.psi_z = rount_double(comp_yaw); bodyInt = d2IntNhan1000(bodyDouble); digitalWrite(SS, LOW); // SS is pin 10 SPI_writeAnything (bodyInt); digitalWrite(SS, HIGH); // // timeLoopTest.dt = (float)(micros() - timeLoopTest.lastTime); // // Serial.print ("timeLoopTest: " + (String)timeLoopTest.dt); Serial.print ("psix :" + (String)(bodyDouble.psi_x) + " " + "psiy :" + (String)(bodyDouble.psi_y)); // Serial.println(); } // Chuyen doi angleInt d2IntNhan1000(angleDouble _body) { angleInt tmp; tmp.psi_x = (int)(_body.psi_x * 1000); tmp.psi_y = (int)(_body.psi_y * 1000); tmp.psi_z = (int)(_body.psi_z * 1000); return tmp; } angleDataInt d2IntNhan1000(angleDataDouble _body) { angleDataInt tmp; tmp.psi_x = (int)(_body.psi_x * 1000); tmp.psi_y_dot = (int)(_body.psi_y_dot * 1000); Phụ lục 33 tmp.psi_y = (int)(_body.psi_y * 1000); tmp.psi_x_dot = (int)(_body.psi_x_dot * 1000); return tmp; } //double rount_double(double n) //{ // long t,u; // t=n*1000; // // u=t % // if(u>=5) // // { u=t/10 +1 ; // // } else // // u=t/10; n=(double)u/100; // // return n; 10; //} // double rount_double(double n) { long t, u; t = n * 100; u = t % 10; if (u >= 5) { u = t / 10 + ; } else u = t / 10; n = (double)u / 10; return n; } Chuong trình xử lý trung tâm Phụ lục 34 #include #include #include "configure.h" #include #include "SPI_anything.h" #include "serial3.h" // -typedef struct TIME_ { float dt = 0; long lastTime = 0; } time_; time_ timeNhanGoc, timeHC101, timeLoopTest, timePID; data_float VEL = {0, 0, 0}; data_float revData = {0, 0, 0}; // -struct angleFloat { float psix = 0; float psiy = 0; float psiz = 0; }; struct angleInt { int psix = 0; int psiy = 0; int psiz = 0; }; angleFloat ANGLE_DIR; angleInt BODYINT; typedef struct angleDataDouble { double psi_x; double psi_y; double psi_x_dot; double psi_y_dot; }; typedef struct angleDataInt { int psi_x; int psi_y; int psi_x_dot; Phụ lục 35 int psi_y_dot; }; angleDataDouble bodyDataDouble; angleDataInt bodyDataInt; // -PID_H psi_x(35, 0.01, 0, 0, 0, 0, 0, 2000); PID_H psi_y(35, 0.01, 0, 0, 0, 0, 0, 2000); PID_H psi_z(15, 0.0, 0, 0, 0, 0, 0, 2000); // -void setup() { init_config(); Serial.begin(115200); // cai chan Tx co van de??????? Serial1.begin(115200); // Serial HC101 < > may tinh Serial2.begin(115200); // Serial HC101 < > mach dk Serial3.begin(115200); // Serrial gui va nhan du lieu voi board dong co psi_x.setPoint = -1.1 + 0.1 - 0.5; psi_y.setPoint = -1.3 + 0.2 + 0.5; psi_z.setPoint = 0; } void loop() { //Hieu chinh PID if (Serial2.available()) { UART_readStruct(revData, 2); while (Serial2.available()) { Serial2.read(); } } //Nhan goc timeNhanGoc.dt = (float)(micros() - timeNhanGoc.lastTime); SPI_readAnything(BODYINT); ANGLE_DIR = Int2fChia1000(BODYINT); //Cap nhat gia tri goc psi_x.measure = ANGLE_DIR.psix; psi_y.measure = ANGLE_DIR.psiy; Phụ lục 36 psi_z.measure = ANGLE_DIR.psiz; timeNhanGoc.lastTime = micros(); //Tinh PID timePID.dt = (float)(micros() - timePID.lastTime) / 1000000; psi_x.compute_pos(timePID.dt); psi_y.compute_pos(timePID.dt); psi_z.compute_pos(timePID.dt); timePID.lastTime = micros(); //Goi dieu khien Tauxyz2Tau123(VEL, psi_x.out, psi_y.out, 0); UART_sendStruct(VEL, 3); //Goi du lieu len may tinh timeHC101.dt = (float)(micros() - timeHC101.lastTime) / 1000000; if (timeHC101.dt >= 0.1) { timeHC101.lastTime = micros(); // // Serial2.print ("psi_x.get_e_k: " + (String)(psi_x.get_e_k()) + "\t"); Serial2.print ("psi_x.PID: " + (String)(psi_x.get_kp()) + " " + (String)(psi_x.get_ki()) + " " + (String)(psi_x.get_kd()) + "\t"); // // Serial2.print ("psi_y.get_e_k: " + (String)(psi_y.get_e_k()) + "\t"); // Serial2.print ("psi_y.PID: " + (String)(psi_y.get_kp()) + " " + (String)(psi_y.get_ki()) + " " + (String)(psi_y.get_kd()) + "\t"); Serial2.print ("psi_.measure: " + (String)psi_x.measure + " " + (String)psi_y.measure + " " + (String)psi_z.measure + "\t"); Serial2.println(); } } //Void loop void Tauxyz2Tau123(data_float& OUT_PSIZ) { tmp, float OUT_PSIX, float OUT_PSIY, float tmp.val_1 = * sqrt(2) * OUT_PSIX / - sqrt(2) * OUT_PSIZ / 3; tmp.val_2 = -sqrt(2) * OUT_PSIX / + sqrt(6) * OUT_PSIY / - sqrt(2) * OUT_PSIZ / 3; Phụ lục 37 tmp.val_3 = -sqrt(2) * OUT_PSIX / - sqrt(6) * OUT_PSIY / - sqrt(2) * OUT_PSIZ / 3; } //void Tauxyz2Tau123 void veDoThi_2(float x1, float x2, float x3) { /* CHU Y CHU Y: Neu goi qua nhanh qua thi ben nhan se bi crash, goi cham thoi */ // Serial.print((String)(millis() / 100)); // Serial.print(","); Serial.print(x1); Serial.print(","); Serial.print(x2); Serial.print(","); Serial.println(x3); } // Chuyen doi angleFloat Int2fChia1000(angleInt _body) { angleFloat tmp; tmp.psix = (float)_body.psix / 1000; tmp.psiy = (float)_body.psiy / 1000; tmp.psiz = (float)_body.psiz / 1000; return tmp;}; Phụ lục 38 Chương trình điều khiển động #include "EncoderH.h" #include "configure.h" #include "serial3.h" #include "init_I.h" #include "MotorHung.h" #include SimpleKalmanFilter simpleKalmanFilter1(0.5, 0.5, 0.1); SimpleKalmanFilter simpleKalmanFilter2(0.5, 0.5, 0.1); SimpleKalmanFilter simpleKalmanFilter3(0.5, 0.5, 0.1); Encoder E_1(CA1, CB1); Encoder E_2(CA2, CB2); Encoder E_3(CA3, CB3); #define VEL_MAX 255 struct DATA { float val_1; float val_2; float val_3; }; data_float omgRev = {0, 0, 0}; typedef struct TIME_ { float dt = 0; long lastTime = 0; } time_; time_ timeE, timePID, timeVanTocTest, timePWM; MotorHung M_1(PWM1, DIR1, 37); MotorHung M_2(PWM2, DIR2, 30); MotorHung M_3(PWM3, DIR3, 34); float OUT1, OUT2, OUT3; //float Km = 0.000675; 27/04/2018 float Km = 0.027; // he so moment dong cua dong co DA SUA NGAY float Imax = 400; float Vmax = 300; Phụ lục 39 int ValPWM1, ValPWM2, ValPWM3; long TIMELOOP = 0; float SAMPLE_TIME = 0.003; // 5ms thi thay doi PWM lan void setup() { Serial.begin(115200); Serial2.begin(115200); M_1.setSpeedMotor(0); M_2.setSpeedMotor(0); M_3.setSpeedMotor(0); } void loop() { //Nhan U neu co if (Serial2.available() > 0) { UART_readStruct(omgRev, 2); while (Serial2.available()) { Serial2.read(); //Doc het nhung byte lai cua bo nho dem } // Serial.print ("omgRev.val_x: " + (String)(omgRev.val_1) (String)omgRev.val_2 + " " + (String)omgRev.val_3 + "\t\t"); // // + " " + Serial.print ((String)millis() + "\t\t"); Serial.println(); } //Lien tuc dieu khien theo PWM hien tai timePWM.dt = (float)(micros() - timePWM.lastTime) / 1000000; if (timePWM.dt >= SAMPLE_TIME) { timePWM.lastTime = micros(); // // omgRev.val_1 = constrain(omgRev.val_1, -VEL_MAX, VEL_MAX); omgRev.val_2 = constrain(omgRev.val_2, -VEL_MAX, VEL_MAX); // // omgRev.val_3 = constrain(omgRev.val_3, -VEL_MAX, VEL_MAX); // // // M_1.setSpeedMotor(-omgRev.val_1); M_2.setSpeedMotor(-omgRev.val_2); M_3.setSpeedMotor(-omgRev.val_3); veDoThi_2(M_1.setSpeedMotor(-omgRev.val_1),M_2.setSpeedMotor(omgRev.val_2),M_3.setSpeedMotor(-omgRev.val_3)); // M_1.setSpeedMotor(0);//37 Phụ lục 40 // M_2.setSpeedMotor(0);//30 // } M_3.setSpeedMotor(34);//34 } struct DATA MMT2CUR(struct DATA moment) { struct DATA cur; cur.val_1 = moment.val_1 / (99.5 * Km); cur.val_2 = moment.val_2 / (99.5 * Km); cur.val_3 = moment.val_3 / (99.5 * Km); return cur; } data_float moment2velocity(data_float _moment_float) { data_float _velocity_float; } void veDoThi(float x1, float x2, float x3, float x4) { /* CHU Y CHU Y: Neu goi qua nhanh qua thi ben nhan se bi crash, goi cham thoi */ // // Serial.print((String)(millis() / 100)); Serial.print(","); Serial.print(x1); Serial.print(","); Serial.print(x2); Serial.print(","); Serial.print(x3); Serial.print(","); Serial.println(x4); } void veDoThi_2(float x1, float x2,float x3) { /* CHU Y CHU Y: Neu goi qua nhanh qua thi ben nhan se bi crash, goi cham thoi Phụ lục 41 */ // // Serial.print((String)(millis() / 100)); Serial.print(","); Serial.print(x1); Serial.print(","); Serial.print(x2); Serial.print(","); Serial.println(x3); } Chương trình thu thập liệu import sys import time import numpy import serial import matplotlib.pyplot as plt from drawnow import * from matplotlib.legend_handler import HandlerLine2D stime = [] sval_1 = [] sval_2 = [] sval_3 = [] count = ser = serial.Serial('COM10', 115200) plt.ion() def makeFig_3(): plt.title('Omega') plt.grid(True) plt.legend(loc='upper left') plt.xlabel('Time(micros)') plt.ylabel('Omega(x/s)') line1, = plt.plot(stime, sval_1, 'r ', label='d1m3') line2, = plt.plot(stime, sval_2, 'b ', label='d2m1') Phụ lục 42 line3, = plt.plot(stime, sval_3, 'g ', label='d3m2') plt.legend(handler_map={line1: HandlerLine2D(numpoints=4)}) def makeFig_1(): plt.title('Omega') plt.grid(True) plt.legend(loc='upper left') plt.xlabel('Time(millis)') plt.ylabel('Omega(x/s)') plt.plot(stime, sval_1, 'r ') # for i in range(0, 3000): # Muon ve lau hon thi cho cai lon hon while True: # Cho du lieu while ser.inWaiting() == 0: pass # # Doc du lieu data = ser.readline() # Xu ly timeVal, val_1, val_2, val_3 = data.split(',') timeVal = float(timeVal) val_1 = float(val_1) # dcm, cay cu vai, goi qua la string ma python cung ve dc -_-, thua gio phair chuyen qua float val_2 = float(val_2) val_3 = float(val_3) stime.append(timeVal) # Ghep cac du lieu lia mang de ve thi sval_1.append(val_1) sval_2.append(val_2) sval_3.append(val_3) # print "timeVal : ", timeVal, " Val_1: ", val_1, " Val_2: ", val_2, " Val_3: ", val_3 print timeVal, ",", drawnow(makeFig_1) val_1, ",", val_2, ",", val_3 # drawnow(makeFig_3) plt.pause(0.00001) count = count + Phụ lục 43 if count > 100: stime.pop(0) sval_1.pop(0) sval_2.pop(0) sval_3.pop(0) Phụ lục 44 ... chương: Chương TỔNG QUAN VỀ ROBOT CÂN BẰNG TRÊN QUẢ CẦU Chương MÔ HÌNH TỐN HỌC Chương THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN Chương THIẾT KẾ VÀ CHẾ TẠO NGUYÊN MẪU ROBOT Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN Nhân... : Phạm Trường Hưng – Cao Thanh Bộ GVHD:TS Nguyễn Danh Ngọc 34 Thiết kế chế tạo robot cân cầu Chương THIẾT KẾ VÀ CHẾ TẠO NGUYÊN MẪU ROBOT 4.1 Kiến trúc tổng quan Kiến trúc tổng quan hệ thống thể... TỔNG QUAN VỀ ROBOT CÂN BẰNG TRÊN QUẢ CẦU 1.1 Khái niệm cấu trúc 1.1.1 Khái niệm Robot cân cầu (ball balancing robot – BBR), gọi Ballbot gọi theo tên nguyên mẫu nghiên cứu chế tạo vào năm 2006