MỤC LỤCDanh mục hình ảnh6CHƯƠNG 1 TỔNG QUAN VỀ ROBOT HAI BÁNH TỰ CÂN BẰNG71.1 Giới thiệu chung71.2 Thế nào là robot hai bánh tự cân bằng71.3 Tại sao phải thiết kế robot hai bánh tự cân bằng81.4 Mục tiêu của đồ án101.5 Phương pháp nghiên cứu101.6 Giới hạn của đồ án11CHƯƠNG 2 THIẾT KẾ CHẾ TẠO122.1 Sơ đồ khối122.2 Thiết kế phần cứng122.3 Mạch điện tử132.3.1 Nguồn điện132.3.2 Bộ điều khiển trung tâm Arduino Uno R3142.3.2.1 Giới thiệu chung142.3.2.2 Cấu trúc phần cứng và chức năng162.3.2.3 Môi trường phát triển (trình biên dịch IDE)192.3.3 Mạch điều khiển động cơ Robo shield202.3.4 Cảm biến MPU6050212.3.4.1 Giới thiệu cảm biến MPU6050212.3.4.2 Các chân giao tiếp232.3.5 Động cơ Encoder 334 Xung + Giảm Tốc RP201232.4 Các phương thức giao tiếp242.4.1 Điều chế độ rộng xung PWM242.4.2 Giao thức I2C272.4.3 Giao tiếp UART292.5 Giải thuậtlập trình302.5.1 Nguyên lý điều khiển cân bằng302.5.2 Bộ lọc Kalman312.5.2.1 Giới thiệu bộ lọc Kalman312.5.2.2 Quá trình ước lượng332.5.2.3 Giải thuật bộ lọc Kalman rời rạc342.5.2.4 Tại sao phải sử dụng bộ lọc Kalman362.5.2.5 Xây dựng bộ lọc Kalman cho cảm biến MPU6050382.5.3 Giải thuật điều khiển PID432.5.3.1 Giải thuật PID432.5.3.2 Xây dựng giải thuật PID cho robot cân bằng452.5.4 Nguyên lý hoạt động47CHƯƠNG 3 THỬ NGHIỆM VÀ KẾT LUẬN493.1 Thử nghiệm493.2 Kết quả493.3 Hướng phát triển50PHỤ LỤC52Phụ lục 1 Chương trình chính52Phụ lục 2 Chương trình thư viện bộ lọc Kalman58Phụ lục 3 Chương trình thư viện PID
Khoa: Điện tử viễn thông SVTH : Nhóm robot cân TRƯỜNG ĐẠI HỌC ĐIỆN LỰC KHOA ĐIỆN TỬ VIỄN THÔNG ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ, CHẾ TẠO ROBOT HAI BÁNH TỰ CÂN BẰNG GIẢNG VIÊN HƯỚNG DẪN : THS TRẦN TRỌNG THẮNG SINH VIÊN THỰC HIỆN : TRẦN ĐÌNH PHÚ : BÙI ĐỨC CƯỜNG CHUYÊN NGÀNH : ĐIỆN TỬ VIỄN THÔNG LỚP : Đ6-ĐTVT2 KHÓA : 2011-2016 Hà Nội, năm 2016 TRƯỜNG ĐẠI HỌC ĐIỆN LỰC GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân KHOA ĐIỆN TỬ VIỄN THÔNG ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ, CHẾ TẠO ROBOT HAI BÁNH TỰ CÂN BẰNG GIẢNG VIÊN HƯỚNG DẪN : THS TRẦN TRỌNG THẮNG SINH VIÊN THỰC HIỆN : TRẦN ĐÌNH PHÚ : BÙI ĐỨC CƯỜNG CHUYÊN NGÀNH : ĐIỆN TỬ VIỄN THÔNG LỚP : Đ6-ĐTVT2 KHÓA : 2011-2016 Hà Nội, năm 2016 LỜI CẢM ƠN Trong thời gian đề tài nghiên cứu khoa học bước ngoặt cuối đánh dấu trưởng thành sinh viên giảng đường đại học.Để trở thành cử nhân hay kỹ sư đóng góp học cho phát triển đất nước GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân Lời chúng em xin chân thành cảm ơn hướng dẫn tận tình thầy ThS Trần Trọng Thắng, khoa Điện tử-Viễn thông, trường Đại học Điện lực Trong suốt thời gian thực đề tài, thầy dành nhiều thời gian để hướng dẫn chúng em thực đề tài Thầy hướng dẫn chúng em tìm hiểu kiến thức cần thiết để thực đề tài Bên cạnh chúng em xin gởi lời cảm ơn chân thành đến thầy ThS Hoàng Xuân Đông, thầy nhiệt tình hướng dẫn, sẵn sàng giúp đỡ chúng em gặp khó khăn việc lập trình, chế tạo robot Em xin chân thành cảm ơn thầy cô khoa Điện tử-Viễn thông, thầy cô trường giảng dạy, giúp đỡ chúng em suốt bốn năm học qua Chính thầy cô xây dựng cho chúng em kiến thức tảng kiến thức chuyên môn để em hoàn thành đề tài Hà nội, ngày tháng năm 2016 SINH VIÊN THỰC HIỆN Trần Đình Phú Bùi Đức Cường MỤC LỤC GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân Danh mục hình ảnh CHƯƠNG TỔNG QUAN VỀ ROBOT HAI BÁNH TỰ CÂN BẰNG 1.1 Giới thiệu chung Cùng với phát triển khoa học kỹ thuật, ngày robot có khả thay người làm việc môi trường độc hại, sản xuất bắt chước người hình thức, hành vi suy nghĩ Hiện lĩnh vực robot phát triển nhanh nhờ vào phát triển liên tục công nghệ, robot chế tạo để phục vụ cho nhiều mục đích khác Với ý GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân tưởng sử dụng robot thay người “Robot hai bánh tự cân bằng” đề tài nhiều tác giả quan tâm mô hình robot có khả di chuyển linh hoạt lại không chiếm nhiều không gian 1.2 Thế robot hai bánh tự cân Hình 1.1: Mô tả nguyên lý cân Đối với xe hay robot ba hay bốn bánh, việc thăng ổn định chúng nhờ trọng tâm chúng nằm bề mặt chân đế bánh xe tạo Đối với xe bánh có cấu trúc xe đạp, việc thăng không di chuyển hoàn toàn không thể, việc thăng xe dựa tính chất quay hồi chuyển hai bánh xe quay Còn xe hay robot hai bánh tự cân bằng, loại có hai bánh với trục hai bánh xe trùng nhau, robot cân bằng, trọng tâm cần giữ nằm bánh xe Điều giống ta giữ gậy dựng thẳng đứng cân lòng bàn tay Thực ra, trọng tâm toàn robot nằm vị trí nào, cách tìm Do vậy, thay tìm cách xác định trọng tâm nằm bánh xe ta cho xe di chuyển nhằm triệt tiêu góc nghiêng xe GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân Hình 1.2: Mô tả cách di chuyển 1.3 Tại phải thiết kế robot hai bánh tự cân Việc thiết kế robot hai bánh tự cân tảng để phát triển xe hai bánh tự cân sau ta cần so sánh xe hai bánh tự cân với thể loại ba bánh hay bốn bánh Những mobile robot xây dựng hầu hết robot robot di chuyển ba bánh xe, với hai bánh lái lắp ráp đồng trục, bánh đuôi nhỏ Có nhiều kiểu khác nhau, kiểu thông dụng Còn xe bánh, thường đầu xe có hai bánh truyền động đầu xe lại gắn hai bánh lái Hình 1.3: Xe ba bánh mặt phẳng Việc thiết kế ba hay bốn bánh làm cho xe/mobile robot thăng ổn định nhờ trọng lượng chia cho hai bánh lái bánh đuôi, hay khác để đỡ trọng lượng xe Nếu trọng lượng đặt nhiều GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân vào bánh lái xe/robot không ổn định dễ bị ngã, đặt nhiều vào bánh đuôi hai bánh khả bám Nhiều thiết kế xe/robot di chuyển tốt địa hình phẳng, di chuyển lên xuống địa hình lồi lõm (mặt phẳng nghiêng) Khi di chuyển xuống đồi, trọng lượng xe/robot dồn vào đầu xe làm bánh lái khả bám trượt ngã, bậc thang, chí dừng hoạt động quay tròn bánh xe Hình 1.4: Xe bánh lên Khi di chuyển lên đồi, việc tệ hơn, trọng tâm thay đổi phía sau chí làm xe/robot bị lật úp di chuyển bậc thang.Việc bố trí bốn bánh xe, giống xe đồ chơi hay loại xe bốn bánh sử dụng giao thông không gặp vấn đề điều làm xe/robot không gọn Hình 1.5: Xe bánh xuống Ngược lại, xe dạng hai bánh đồng trục lại thăng linh động di chuyển địa hình dốc, thân hệ thống không ổn định Khi GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân leo sườn dốc, tự động nghiêng trước giữ cho trọng lượng dồn hai bánh lái Tương tự vậy, bước xuống dốc, nghiêng sau giữ trọng tâm rơi vào bánh lái Chính vậy, tượng trọng tâm xe rơi vùng đỡ bánh xe để gây lật úp Hình 1.6: Hai bánh lên xuống linh động 1.4 Mục tiêu đồ án Mục tiêu đồ án xây dựng robot có khả di chuyển hai bánh,làm phương tiện di chuyển hiệu linh động, dễ dàng xoay trở không gian chật hẹp.Trong khuôn khổ tháng thực luận văn tốt nghiệp, mục tiêu đề sau: • Tìm hiểu loại robot cân bằng, nguyên lý cân • Thiết kế mạch điện tử kết hợp cảm biến thực chức đo góc (phần cứng) • Giải thuật cho vi điều khiển kết hợp bù trừ cảm biến để có giá trị đo góc xác • Xây dựng thuật toán điều khiển cho động cơ, giữ thăng • Lập trình điều khiển GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân 1.5 Phương pháp nghiên cứu Đề tài tiếp cận dựa phương pháp sau: • Phương pháp khảo sát tài liệu, tìm hiểu tài liệu liên quan đến đến đề tài như: cấu trúc robot hai bánh tự cân bằng, cảm biến MPU6050, mạch điều khiển động • Phương pháp khảo sát thuật toán lọc nhiễu cho cảm biến như: lọc Kalman thuật toán điều khiển PID • Phương pháp thực nghiệm tiến hành xây dựng thuật toán mô hình 1.6 Giới hạn đồ án Đề tài tập trung vào việc xây dựng mô hình phần cứng robot như: kết cấu mô hình, mạch điều khiển động cơ, thuật toán vi điều khiển lọc Kalman giải thuật cân PID Robot cân có khả điều khiển để di chuyển chưa có nhiều ứng dụng GVHD : ThS Trần Trọng Thắng Page Khoa: Điện tử viễn thông SVTH : Nhóm robot cân CHƯƠNG THIẾT KẾ CHẾ TẠO 2.1 Sơ đồ khối Cảm biến MPU6050 2.1 i2c Arduino UNO R3 I2C Mạch điều khiển PWM động DC 2.2 Khối nguồn Hình 2.1: Sơ đồ khối hệ thống robot 2.2 Thiết kế phần cứng Đồ án sử dụng Arduino Uno R3 điều khiển trung tâm Khung robot dc chế tạo từ miếng nhựa cứng liên kết với vít đồng với hai động đặt đồng trục cho phép robot di chuyển theo hai hướng trước sau Hai động điều khiển robo shield, sử dụng cảm biến MPU6050 gián vào mặt phẳng khung nhựa để xác định góc nghiêng robot GVHD : ThS Trần Trọng Thắng Page 10 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân Hình 2.24: Mô hình sản phẩm hoàn thành 3.2 Kết *Kết đạt đồ án: • Ôn lại tìm hiểu thêm nhiều kiến thức động lực học,điện tử • Sử dụng thành công lọc kalman mô hình robot để tính toán góc • Xây dựng thành công giải thuật cân bám theo vị trí ban đầu dựa thuật toán PID • Làm quen với lĩnh vực lập trình chế tạo khí • Nghiên cứu bước đầu chế tạo thành công mô hình robot đơn giản 3.3 Hướng phát triển Dựa kết robot chạy thành công,chúng em có ý tưởng phát triển thêm để chế tạo thành sản phẩm chở người.Ý tưởng GVHD : ThS Trần Trọng Thắng Page 50 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân đưa dựa vào số yêu cầu thực tế tạo phương tiện giúp người di chuyển thuận tiện nhỏ gọn di chuyển nhà,phục vụ việc giải trí dùng số công việc vận chuyển hàng hóa,đồ đạc.Bước đầu chúng em lắp đặt phần khung xe,xe di chuyển thời gian hạn chế nên sản phẩm chưa hoàn thiện.Hiện chúng em tiến hành thực phát triển thêm Hình 2.25: Mô hình xe chế tạo TÀI LIỆU THAM KHẢO 1) http://www.Google.com 2) http://www.Arduino360.com GVHD : ThS Trần Trọng Thắng Page 51 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân 3) Alwafi Husein, Attitude and altitude control of two wheel trirotor hybrid robot, 4) 5) 6) 7) 8) Master’s thesis, 2013 Xe hai bánh tự cân di chuyển địa hình phẳng Mai Tuấn Đạt Arduino.cc 123doc.org http://www.instructables.com http://www.Arduino.vn PHỤ LỤC Phụ lục Chương trình #include "I2Cdev.h" //Thu vien I2C #include "MPU6050.h" // Thu vien MPU #include "Kalman.h" // Thu vien Kalman #include "pwm01.h" // Thu vien xuat xung PWM #include // Thu vien PID cua arduino GVHD : ThS Trần Trọng Thắng Page 52 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân #include // Thu vien ngat timer cua Arduino #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif MPU6050 accelgyro; Kalman kalmanY; int16_t accX, accY, accZ; int16_t tempRaw; int16_t gyroX, gyroY, gyroZ; uint32_t timer; uint16_t PWM_L,PWM_R,PWM_T; bool dir = 0; bool play = 0; double CounterEncoder= 0; double Value; double accXangle, accYangle; // Goc cua cam bien gia toc double temp; double gyroXangle, gyroYangle; // Goc cam bien quay hoi chuyen double Setpoint, Input, Output; double Kp,Ki,Kd; // Thong so phan PID double kalAngleX, kalAngleY; // Goc tinh toan su dung bo loc Kalman long frequency = 15000; // Tan so PWM cho dong co GVHD : ThS Trần Trọng Thắng Page 53 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân char PIN_PWM_L = 8; // Chan xuat xung PWM o dong co trai char PIN_DIR_L = 23; // Chan chieu cua dong co trai char PIN_PWM_R = 9; // Chan xuat xung PWM o dong co phai char PIN_DIR_R = 25; // Chan chieu cua dong co phai char PIN_BUTTON = 27; // nut nhan khoi dong pid #define LED_PIN 13 double Angle; //Goc tren robot double SetAngle = 88; // Goc robot o vi tri can bang PID myPID(&Input, &Output, &Setpoint,0,0,0, DIRECT); // Cat dat thong so PID #define OUTPUT_READABLE_ACCELGYRO void timerIsr() // Ham ngat timer ms { Angle = kalAngleY; if(Angle (SetAngle + 20)) Angle= SetAngle + 20; SetAngle=86-Value; Input = SetAngle-Angle; myPID.Compute(); // Tinh toan PID (Output >= 0)? dir=0:dir=1; // Xac dinh chieu de robot can bang digitalWrite(PIN_DIR_L,dir); digitalWrite(PIN_DIR_R,dir); PWM_T=4095-abs(Output); GVHD : ThS Trần Trọng Thắng Page 54 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân PWM_L= PWM_T; PWM_R= PWM_T*0.98; pwm_write_duty( PIN_PWM_R, PWM_R ); // Xuat xung PWM dieu khien dong co pwm_write_duty( PIN_PWM_L, PWM_L ); } void setup() { // Cai dat I2C #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // Cat dat UARRT voi toc baud la 9600 Serial.begin(9600); // Cat dat thong so dau cho PMU accelgyro.initialize(); accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG; kalmanY.setAngle(accYangle); gyroYangle = accYangle; GVHD : ThS Trần Trọng Thắng Page 55 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân timer = micros(); // Cac thong so PID Kp=90; Ki=8500; Kd=0.3; Setpoint = 0; // Thong so cat dat ban dau cua PID myPID.SetOutputLimits(-2000,2000); // Gioi han dau cua PID myPID.SetTunings(Kp,Ki,Kd); myPID.SetMode(AUTOMATIC); // PID chay o che Auto // Cai dat thong so xuat xung PWM pwm_set_resolution(12); // Xung PWM voi 12 bit pwm_setup( 6, frequency, 1); pwm_setup( 7, frequency, 2); pwm_setup( 8, frequency, 2); pwm_setup( 9, frequency, 2); pwm_write_duty( PIN_PWM_L, 4095); pwm_write_duty( PIN_PWM_R, 4095); // Cai dat che OUTPUT/INPUT pinMode(LED_PIN, OUTPUT); pinMode(PIN_DIR_L, OUTPUT); pinMode(PIN_DIR_R, OUTPUT); attachInterrupt(39, Encoder, RISING); // Ngat ngoai canh xuong cho encoder GVHD : ThS Trần Trọng Thắng Page 56 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân pinMode(39, INPUT); pinMode(41, INPUT); pinMode(PIN_BUTTON, INPUT); } void loop() { accelgyro.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); // Doc du lieu I2C tu cam bien IMU accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; // Tinh toan doi tu radian sang double gyroYrate = -((double)gyroY/131.0); gyroYangle += gyroYrate*((double)(micros()-timer)/1000000); // Tinh toan thong so goc quay chua qua bo loc Kalman kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()timer)/1000000); // Tinh toan goc quay qua bo loc Kalman timer = micros(); temp = ((double)tempRaw + 12412.0) / 340.0; if(play==0) // Kiem tra nut de robot thuc hien can bang { if(digitalRead(PIN_BUTTON)) { play=1; GVHD : ThS Trần Trọng Thắng Page 57 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân Timer3.attachInterrupt(timerIsr); // Cat dat ngat timer Timer3.start(1000); // Cai dat thoi gian ngat la ms } } // Tinh toan sai so tren encoder Value=(CounterEncoder/3300); Value = constrain(Value,-5,5); // Dua du lieu qua cong UART Serial.print(CounterEncoder); Serial.print("\t"); Serial.print(Value); Serial.print("\t"); Serial.print(SetAngle); Serial.print("\t"); Serial.println(kalAngleY); } void Encoder() // Ham ngat tinh toan encoder { if(((digitalRead(39)==0)&&(digitalRead(41)==1)) || ((digitalRead(39)==1) && (digitalRead(41)==0))) CounterEncoder ; else if(((digitalRead(39)==0)&&(digitalRead(41)==0)) || ((digitalRead(39)==1) && (digitalRead(41)==1))) CounterEncoder++; } GVHD : ThS Trần Trọng Thắng Page 58 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân Phụ lục Chương trình thư viện lọc Kalman / Dinh nghia bo loc Kalman #ifndef _Kalman_h #define _Kalman_h class Kalman { public: Kalman() { // Khai bao cac bien co dinh cho bo loc Kalman cac he so co the duoc thay doi Q_angle = 0.001; Q_bias = 0.003; R_measure = 0.03; bias = 0; // Reset bias // Thiet lap ma tran phuong sai loi P[0][0] = 0; P[0][1] = 0; P[1][0] = 0; P[1][1] = 0; }; // dua cac thong so newAngle, newrate va delat time moi giay vao gia tri goc tinh toan double getAngle(double newAngle, double newRate, double dt) { GVHD : ThS Trần Trọng Thắng Page 59 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân // cap nhat thoi gian (uoc luong) // uoc luong gia tri moi rate = newRate - bias; angle += dt * rate; // Uoc luong ma tran covariance tiep theo P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1]; P[1][0] -= dt * P[1][1]; P[1][1] += Q_bias * dt; // Cap nhat gia tri (hieu chinh) // Cap nhat gia tri Zk S = P[0][0] + R_measure; K[0] = P[0][0] / S; K[1] = P[1][0] / S; // Tinh loi Kalam y = newAngle - angle; angle += K[0] * y; bias += K[1] * y; // Cap nhat lai ma tran covariance P[0][0] -= K[0] * P[0][0]; P[0][1] -= K[0] * P[0][1]; P[1][0] -= K[1] * P[0][0]; GVHD : ThS Trần Trọng Thắng Page 60 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân P[1][1] -= K[1] * P[0][1]; return angle; }; void setAngle(double newAngle) { angle = newAngle; }; / / su dung de thiet lap goc, co nghia la goc bat dau double getRate() { return rate; }; // tro lai muc rate / * Khai bao cac chuong trinh de dieu chinh bo loc Kalman * / void setQangle(double newQ_angle) { Q_angle = newQ_angle; }; void setQbias(double newQ_bias) { Q_bias = newQ_bias; }; void setRmeasure(double newR_measure) { R_measure = newR_measure; }; private: / * lọc biến Kalman * / double Q_angle; // qua trinh nhieu phuong sai cho gia toc double Q_bias; // qua trinh nhieu phuong sai cho quay hoi chuyen double R_measure; // Do dac nhieu phuong sai double angle; // cac goc tinh boi bo loc Kalman – mot phan ma tran x double bias; // quay hoi chuyen tinh boi bo loc Kalman - bang x 1matrix double rate; // cap nhat ti le getAngle double P[2][2]; // Ma tran hiep phuong sai loi – day la ma tran x double K[2]; // loi Kalman – day la ma tran x double y; // goc khac ma tran x1 double S; // uoc luong sai so loi ma tran x GVHD : ThS Trần Trọng Thắng Page 61 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân }; #endif Phụ lục Chương trình thư viện PID #ifndef PID_v1_h #define PID_v1_h #define LIBRARY_VERSION 1.0.0 class PID { public: // Khai bao cac chuc nang su dung #define AUTOMATIC #define MANUAL #define DIRECT #define REVERSE // Cac chuc nang su dung PID(double*, double*, double*, // * Lien ket PID ngo vao double, double, double, int); // Thiet lap cac thong so dieu chinh void SetMode(int Mode); // dieu khien he PID bang tay (0) tu dong (non-0) bool Compute(); // Thuc hien tinh toan PID, thiet lap tan so va thoi gian lay mau void SetOutputLimits(double, double); // Gia tri dau 0-255 doi voi 8-bit GVHD : ThS Trần Trọng Thắng Page 62 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân va tu - 65536 doi voi 16-bit void SetTunings(double, double, double); // Cai dat he so Kp, Ki,Kd void SetControllerDirection(int); // * Dieu khien truc tiep void SetSampleTime(int); // * Cai dat tan so theo mili giay // Hien thi chuc nang // Dat cac he so PID double GetKp(); double GetKi(); double GetKd(); int GetMode(); int GetDirection(); private: void Initialize(); double dispKp; double dispKi; double dispKd; double kp; double ki; double kd; int controllerDirection; double *myInput; // Lien ket cac bien dau vao, dau diem dat double *myOutput; // Lien ket bien ngo GVHD : ThS Trần Trọng Thắng Page 63 Khoa: Điện tử viễn thông SVTH : Nhóm robot cân double *mySetpoint; // He so dat ban dau unsigned long lastTime; double ITerm, lastInput; unsigned long SampleTime; double outMin, outMax; bool inAuto; }; #endif lastTime = now; } void SetTunings(double Kp, double Ki, double Kd) { kp = Kp; ki = Ki; kd = Kd; } GVHD : ThS Trần Trọng Thắng Page 64 [...]... điều khiển cân bằng GVHD : ThS Trần Trọng Thắng Page 29 Khoa: Điện tử viễn thông bằng SVTH : Nhóm robot cân Hình 2.15: Nguyên lý điều khiển Khi robot đứng thẳng tại vị trí cân bằng robot sẽ bị lực tác động làm cho robot ngã về phía trước hoặc phía sau như Hình 2.15 Nếu robot bị đổ về phía trước như Hình 2.15(a) thông qua cảm biến IMU robot sẽ xác định được góc nghiêng và điều khiển hai bánh xe di chuyển...Khoa: Điện tử viễn thông bằng SVTH : Nhóm robot cân Hai bánh xe được bao bọc bởi cao su có nhiều rãnh để ăng độ bám cao, giúp robot có thể điều khiển cân bằng tốt hơn Hình 2.2: Mô hình robot sau khi lắp ráp 2.3 Mạch điện tử 2.3.1 Nguồn điện Nguồn điện sủ dụng pin 12v, mỗi PIN có dung lượng khoảng 3000mAh được đấu nối tiếp với nhau giúp robot có đủ nguồn điển hoạt động trong một thời... định được góc nghiêng và điều khiển hai bánh xe di chuyển về phía trước theo hướng mà robot bị nghiêng Hay khi robot bị đổ về phía sau như Hình 2.15(c) thì robot sẽ điều khiển hai bánh xe di chuyển về phía sau theo hướng robot bị nghiêng cho đến khi góc nghiêng của robot bằng với góc mà tại đó robot đứng thẳng thì robot sẽ dừng lại 2.5.2 Bộ lọc Kalman 2.5.2.1 Giới thiệu bộ lọc Kalman Được đề xuất từ... GVHD : ThS Trần Trọng Thắng Page 16 Khoa: Điện tử viễn thông bằng SVTH : Nhóm robot cân Môi trường phát triển Arduino được phát triển dựa trên ngôn ngữ lập trình Processing và dự án Wiring Được thiết kế cho những người không biết nhiều về lập trình, bao gồm trình soạn thảo có chứ năng đánh dấu cú pháp, tự động đóng mở các cặp ngoặc nhọn, tự động thụt cách đầu dòng Kèm theo đó là khả năng biên dịch... mịn nhất là hệ số duty có thể được điều chế Có hai cách tạo ra xung PWM: • Mạch tương tự (analog): có giá trị thay đổi liên tục và có độ phân giải vô hạn cả về thời gian và biên độ, có thể được dùng để điều khiển trực tiếp nhiều thứ, như độ lớn âm được phát ra từ một radio, tốc độ của động cơ Tuy nhiên, việc xây dựng và thiết kế mạch tương tự thường tốn kém Có thể tạo ra một mạch PWM nhưng độ chính xác... GVHD : ThS Trần Trọng Thắng Page 15 Khoa: Điện tử viễn thông bằng SVTH : Nhóm robot cân Hình 2.5: Sơ đồ chân arduino Arduino Uno có 14 chân digital (chân 0-13) và 6 chân analog ( chân A0-A5) Các chân digital chúng ta có thể cấu hình để làm chân nhận dữ liệu vào từ các thiết bị ngoại vi hoặc làm chân để truyền tín hiệu ra các thiết bị ngoại vi Bằng cách sử dụng các hàm pinMode(), digitalWrite() và digitalRead()... được Đơn giản hơn, Arduino là một thiết bị có thể tương tác với ngoại cảnh thông qua các cảm biến và hành vi được lập trình sẵn GVHD : ThS Trần Trọng Thắng Page 13 Khoa: Điện tử viễn thông bằng SVTH : Nhóm robot cân Với thiết bị này, việc lắp ráp và điều khiển các thiết bị điện tử sẽ dễ dàng hơn hơn bao giờ hết Hiện tại có nhiều loại vi điều khiển và đa số được lập trình bằng ngôn ngữ C/C++ hoặc Assembly... được truyền trên đường này theo thứ tự từng bit một • SCL - Serial Clock: Là đường giữ nhịp nối tiếp I2C là chuẩn truyền thông nối tiếp đồng bộ, cần có 1 đường tạo xung giữ nhịp cho quá trình truyền/nhận, cứ mỗi xung trên đường giữ nhịp SCL, một bit dữ liệu trên đường SDA sẽ được lấy mẫu GVHD : ThS Trần Trọng Thắng Page 26 Khoa: Điện tử viễn thông bằng SVTH : Nhóm robot cân Hình 2.13: Trạng thái lấy mẫu... ↓và SCL = HIGH • Điều kiện Stop: SDA ↑và SCL = HIGH Cả hai điều kiện Star và Stop đều được tạo ra bởi Master Sau tín hiệu Start, bus I2C ở trạng thái “busy” thực hiện tác vụ truy xuất nối tiếp Sau Stop bus I2C ở trạng thái “free” cho lần kế tiếp GVHD : ThS Trần Trọng Thắng Page 27 Khoa: Điện tử viễn thông bằng SVTH : Nhóm robot cân Hình 2.14: Sơ đồ kết nối giữa arduino với MPU6050 theo chuẩn giao thức... phương pháp điều chế độ rộng xung * Nguyên lý: Nhìn trên hình vẽ, TON và TOFF lần lượt là khoảng thời gian kích (trạng thái HIGH) và ngắt tín hiệu (trạng thái LOW) trong một chu kỳ Việc làm này sẽ tạo ra một mức điện áp trung bình VTB cấp cho động cơ tương ứng với một mức tốc GVHD : ThS Trần Trọng Thắng Page 22 Khoa: Điện tử viễn thông bằng SVTH : Nhóm robot cân độ động cơ Hình 2.10: Điều chế độ rộng xung ... robot cân Hình 1.2: Mô tả cách di chuyển 1.3 Tại phải thiết kế robot hai bánh tự cân Việc thiết kế robot hai bánh tự cân tảng để phát triển xe hai bánh tự cân sau ta cần so sánh xe hai bánh tự. .. hai bánh xe quay Còn xe hay robot hai bánh tự cân bằng, loại có hai bánh với trục hai bánh xe trùng nhau, robot cân bằng, trọng tâm cần giữ nằm bánh xe Điều giống ta giữ gậy dựng thẳng đứng cân. ..Khoa: Điện tử viễn thông SVTH : Nhóm robot cân KHOA ĐIỆN TỬ VIỄN THÔNG ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ, CHẾ TẠO ROBOT HAI BÁNH TỰ CÂN BẰNG GIẢNG VIÊN HƯỚNG DẪN : THS TRẦN TRỌNG THẮNG SINH