TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN Đồ án I ĐỀ TÀI: TÌM HIỂU NGUYÊN LÝ HOẠT ĐỘNG ENCODER, CẢM BIẾN GIA TỐC MPU6050 KẾT HỢP BỘ LỌC KALMAN Giảng viên hướng dẫn Họ tên sinh viên Mã số sinh viên Mã lớp : : : : TS Vũ Vân Hà Nguyễn Việt Hoàng 20191856 717569 Hà Nội, 08/2022 TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN LỜI NĨI ĐẦU Qua q trình thực đồ án I, em tìm hiểu học tập nhiều điều Mặc dù đồ án thực thời gian dài, nhiên thiếu kinh nghiệm việc tìm kiếm thơng tin nên mong có sai sót mong thầy bỏ qua Em cố gắng thực đồ án môn học, tiếp thu kiến thức, kỹ kì học Em xin chân thành cảm ơn Hà Nội, ngày 12 tháng năm 2022 TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN rate=newRate-bias; angle+=dt*rate; //Step P[0][0]+=dt*(dt*P[1][1]-P[1][0]-P[0][1]+Q_angle); P[0][1]-=dt*P[1][1]; P[1][0]-=dt*P[1][1]; P[1][1]+=Q_bias*dt; // step y = newAngle-angle; // Step S= P[0][0]+R_measure; // Step K[0]=P[0][0]/S; K[1]=P[1][0]/S; // Step angle += K[0]*y; bias+= K[1]*y; // Step float P00_temp = P[0][0]; float P01_temp = P[0][1]; P[0][0]-= K[0]*P00_temp; P[0][1]-=K[0]*P01_temp; P[1][0]-=K[1]*P00_temp; P[1][1]-=K[1]*P01_temp; return angle; } 18 TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN Chương trình đọc liệu encoder: #include LiquidCrystal_I2C lcd(0x27,16,2); // set the LCD address to 0x27 for a 16 chars and line display /* encoder 334 * dien ap dong co hoat dong 3V * dien ap encoder 3,3V * 8200 vòng/phút 12VDC * 16800 vòng/phút 24VDC * đọ phân giải N=334 xung/vòng */ int i=0; int motorp =10; int motorm =9; int chanelA = 2;//int0 int chanelB = 4; int pulse =0; int prepulse= 0; float velocity; float position1; float Time; float Ton; float Toff; //float Ts = Ton + Toff; int count = 0; 19 TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN int rotation = 0; // rotation =1 quay thuan, = dung, = -1 quay nghich; void encoder1(){ if (digitalRead (chanelB) == LOW){ pulse++; if (pulse/334==0) count++; } else{ pulse ; // Read position if (pulse/334==0) count ; } } void speedRate(float D,float Hz ){ // D = Ton/Ts; // Ts = 1/Hz; D khoang (0,1) Ton = D*1/Hz; Toff = (1-D)*1/Hz; forward (rotation); delay(Ton*1000); forward (0); delay(Toff*1000); } void forward(int x) { pinMode(motorp,OUTPUT ); 20 TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN pinMode(motorm,OUTPUT); 21 TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN if (x == 1){ digitalWrite(9, LOW); // forward(1) quay thuan digitalWrite(10, HIGH); }else if (x == -1){ digitalWrite(10, LOW); // forward(-1) quay nghich digitalWrite(9, HIGH); }else if (x == 0){ digitalWrite(9, LOW); // forward(0) stop digitalWrite(10, LOW); } } void Display(float mlTime) { if(millis() - Time > 1000*mlTime){ position1=2*3.1415*pulse/334.0; velocity = (pulse-prepulse)/344.0*60/mlTime; Serial.print("Phi(rad): "); Serial.println(position1); Serial.print("Velocity (rpm): "); Serial.println(velocity); Serial.print("\ n"); prepulse = pulse; Time = millis(); } } void Display_lcd(float mlTime) 22 TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN { if(millis() - Time > 1000*mlTime){ 23 TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN position1=2*3.1415*pulse/334.0; velocity = (pulse-prepulse)/344.0*60/mlTime; lcd.setCursor(12,0); lcd.print((int)position1); // lcd.setCursor(3,0); lcd.print(count); lcd.setCursor(11,1); lcd.print((int)velocity); prepulse = pulse; Time = millis(); } } void setup() { lcd.init(); lcd.backlight(); lcd.setCursor(0,0); lcd.print("Phi(rad):"); lcd.setCursor(0,1); lcd.print("V(rpm):"); rotation = -1; // dieu chinh chieu quay, quay thuan Serial.begin(9600); forward(rotation); pinMode(chanelB, INPUT_PULLUP); pinMode(chanelA, INPUT_PULLUP); encoder1(); attachInterrupt(0, encoder1, FALLING); float Time = millis(); } void loop() 24 // Int0 = pin TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN TIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMANTIM.HIEU.NGUYEN.ly.HOAT.DONG.ENCODER cam.BIEN.GIA.toc.MPU6050.ket.hop.bo.loc.KALMAN