3.3.2 .Đáp ứng xung của vị trí robot
4.2 Xây dựng mạch điện điều khiển:
4.2.9 Thi công lắp ráp mạch điện thực tế :
Hình 4.24 Mạch điều khiển động cơ
Danh sách linh kiện sử dụng
Stt Ký hiệu Tên linh kiện Tiêu chuẩn Số
lƣợng Nhà cung cấp 1 IC2 , IC3 L298 L298N , 46v, 4A , - 40~150'C 2 ST 2 D2,D3,D4,D5,D6,D7,D8 ,D9 Diode FR207 1000V , 2A 8 WTE 3 C3, C4,C5 Capacitor C104 3
4 U1, U2 , U3, U4,U5,U6 Photo-
transistor
PC817 , 5V , 5mA ,
100 us 6 Sharp
5
LED1, LED2 , LED3, LED4,LED5,LED6, LED7
Led 7
6 R2 , R3,
R5,R7,R9,R11,R13 Resistor R _ 220 ohm , 1/4w 7 ABCo
7 R4,R6,R8,R10,R12, R14 Resistor R_10K , 1/4w 6
8 D1 Diode 1N5822 1
9 R1 R_metan oxide R_150 ohm , 2W 1 ABCO
10 C1 , C2 C_AL 100uF/ 50V 2 AUK
11 IC1 LM7805 DC5v , 1A 1 Fairchild
12 HD1 , HD2 , HD4 Header boad CN2 3 Yeon-ho
4.2.9.2 Mạch xử lý trung tâm
Hình 4.25 Mạch xử lý trung tâm
Danh sách linh kiện sử dụng
Stt Ký hiệu Tên linh kiện Tiêu chuẩn Số
lƣợng Nhà cung cấp 1 U1 DSPIC30F4013 40 Pin , 48 K.byte , Sram _2048 , EEPROM 1024 , timer _5 1 Microchip 2 J1 LCD16* LCD 1 Joun-ho
3 VR1 Resistor_Var R_val _ 10K 1 ABCo
4 SW10 Button on/off : 1s1p 1 5 R1 R_cacbon R_10K 1 ABCo 6 C2, C3 C-FILM,LEAD-PEF 22p 2 AUK 7 Y1 RESONATOR- CERAMIC 4M Hz 1 8 R2 R_cacbon R_220 1 ABCo 9 D1 LED-light-red 10 mA 1 10 D22 Diode zensor 1N5822 1 11 LN1 Coil L _ 100 uH 1
12 C66 C_ All C_470uF , 35V 1 AUK
13 U22 LM2576T Out – 5v DC 1 Semi
14 C5 C-FILM,LEAD-PEF 33P 1 AUK
15 C50 C_ All 100uF/ 50V 1 AUK
18 JP1, JP4 , JP5 Header board CN5 3 Joun-ho
19 JP6 , JP8 Header board CN6 2 Joun-ho
4.3 Lập trình và nguyên lý điều khiển
Hình 4.26 Lưu đồ xử lý Chip PIC3050
Quy trình xử lý và điều khiển robot cân bằng
Đầu tiên khi khởi động hệ robot, các cấu hình của chip DSpic4013, LCD, cảm biến MPU6050 .e.t.c được thiết lập.
Sau khi robot được cấp nguồn hệ, mất khoảng 3 giây để ổn định kết nối bluetooth giữa robot và PC. Sau đó giữ robot cân bằng trong khoảng 10s để tìm
ra giá trị góc cài đặt cho robot. Robot sẽ hoạt động và giữ cân bằng quay vị trí cài đặt này.
Tiếp theo Robot đọc giá trị vị trí di chuyển của robot qua 2 cảm biến encoder gắn ở 2 bánh xe. Giá trị độ di chuyển của robot sẽ bằng trung bình cộng của 2 cảm biến encoder.
Bước tiếp theo, robot đọc giá trị góc lệch của robot từ cảm biến MPU6050. Ở đây sử dụng 2 sensor tích hợp trong MPU6050 là accelerometer đo gia tốc góc và gyro sensor đo sự trôi của góc.
Các dữ liệu của góc đo được được đưa vào bộ lọc kalman để khắc phục sự nhiễu do gia tốc accelerometer và sự trôi góc của gyro. Trong khoảng thời gian ngắn thì tín hiệu gyro sensor bi sai lệch, và với khoảng thời gian dài sẽ ảnh hưởng tới gia tốc accel.
Góc lệnh sau khi được xử lý kết hợp với vị trí dịch chuyển của robot và đưa vào bộ lọc điều khiển PID. Với các hệ số của bộ điều khiển PID được nhập và thay đổi từ PC.
Sau khi xử lý với bộ điều khiển PID, tốc độ và hướng dịch chuyển của động cơ bánh xe sẽ được đưa vào bộ điều khiển động cơ. Ở đây động cơ bánh xe sẽ được điều khiển quay thuận, quay ngược với duty được tính ở bước trên để giữ cho hệ robot đứng cân bằng và ổn định nhất.
Kết thúc quá trình điều khiển động cơ, các dữ liệu góc lệch, vị trí, tốc độ .e.t.c sẽ được phản hồi và hiển thị trên màn hình PC để người điều khiển có thể quan sát và kiểm định được chất lượng của bộ lọc Kalman, bộ điều khiển PID từ đó thay đổi các tham số PID cho phù hợp với hệ thống.
Sau khi kết thúc toàn bộ các quá trình, hệ thống chờ đủ 10ms sẽ tiếp tục thực hiện chu trình mới từ bước đọc cảm biến tới khi kết thúc bước điều khiển động cơ và giao tiếp PC.
KẾT LUẬN
Với thời gian và điều kiện vật chất có hạn, Luận văn đã thực hiện được cơ bản những mục tiêu đề ra:
- Đã tìm hiểu tổng quan lý thuyết tính toán xây dựng các mô hình động lực học cho một robot cân bằng.
- Đã nghiên cứu các cơ sở lý thuyết điều khiển hiện đại như PID và LQR áp dụng cho điều khiển robot cân bằng.
- Đã tìm hiểu công cụ Matlab và Simulink và sử dụng mô phỏng thành công robot tự cân bằng với 2 giải thuật điều khiển PID và LQR.
- Đã tự thiết kế và chế tạo về cơ khí, thiết kế láp ráp các mạch điện tử điều khiển cho nền tảng phần cứng robot.
- Đã phát triển phần mềm điều khiển nhúng cho robot tự cân bằng với chương trình điều khiển, thu thập xử lý số liệu với các giải thuật như lọc Kalman, giải thuật điều khiển PID viết bằng ngôn ngữ C trên giao diện CCS cho vi điều khiển DsPic.
Robot đã hoạt động được ở mức tự cân bằng tại chỗ, trong tương lai cần tiếp tục phát triển để có thể điều khiển robot di chuyển được theo hướng mong muốn.
Code lập trình vi điều khiển :
---
---*. Chƣơng trình chính ‘ Main ‘ ---
/* Luan van cao hoc
Hoc Vien : Pham Thanh Vu Ma SV : 12105378
Chuyen nghanh : Ky thuat dien tu khoa hoc : K19
Ten de tai : Nghien cuu cac luat dieu khien hien dai tren mo hinh robot can bang */ #include "main.h" #include "lcd.c" #include <stdio.h> #include "math.h" #include "Kalman2.c" #include "MPU6050.c" #include "dongco.c" #include "PID.c" #include "Encoder.c"
#define RAD_TO_DEG 180/PI void Init_Main() { setup_timer1(TMR_INTERNAL|TMR_DIV_BY_8); set_pwm_duty(1,255); set_pwm_duty(2,255); set_pwm_duty(3,255); set_pwm_duty(4,255); setup_compare(4,COMPARE_PWM | COMPARE_TIMER2 ); setup_compare(3,COMPARE_PWM | COMPARE_TIMER2 ); setup_compare(2,COMPARE_PWM | COMPARE_TIMER2 ); setup_compare(1,COMPARE_PWM | COMPARE_TIMER2 ); setup_timer2(TMR_INTERNAL | TMR_DIV_BY_256,255); } void Init_Sensor() {
delay_ms(500); int8 x; x = Mpu6050_Read(WHO_AM_I); if (x!=104) { LCD_Gotoxy(2,0); sprintf(str,"No sensor"); LCD_puts(str); while(true); } } void Update_LCD() { if(accX!=k) { LCD_Gotoxy(0,0); sprintf(str,"%4d %4d V%4d",AccX,consKi1,speed);//chs LCD_puts(str); k = accX; } } void PrinttParameter() { LCD_Gotoxy(0,1); int P=(int16)(pTerm); int I=(int16)(iTerm); int D=(int16)(dTerm);
sprintf(str,"P%3d I%3d D%3d",P,I,D); LCD_puts(str); } void Update_Angle() { accY = GetData(ACCEL_YOUT_H); accZ = GetData(ACCEL_ZOUT_H); gyroX = GetData(GYRO_XOUT_H); //gyroXrate = (float)((gyroX+285)*2000/32768);
gyroXrate = (float)((gyroX+140)/131);
accXangle = (float)((atan2(accY,accZ))*RAD_TO_DEG); accXa = (signed int16)(accXangle*10);
while(get_timer1()<10000) {
}
int16 timer = get_timer1()/1000; set_timer1(0);
// for(vu=0;vu<5;vu++) // {
kalAngleX = Kalman_Calculate( accXangle, gyroXrate, timer); // }
accX = (signed int16)(kalAngleX*10); }
void Update_vitri() {
count = (countA + countB) / 2 ; vitri = count / 70.8 / 12 *3.14*100 ; Fvitri = (float)(vitri/1000);
}
void Update_PID(float setPoint) {
dolech = Fvitri/0.1 + kalAngleX ; speed = Pid_Calculate(setPoint,dolech,consK,consKp,consKi,consKd);//kalAngle X sp = (signed int16)(speed*1); if(speed<1&&speed>-1) { DC_Stop(BACKWARD); DC_Stop(FORWARD); } else if((speed>-256)&&(speed<-1)) { DC_Forward(-speed); } else if((speed>1)&&(speed<256)) {
} else { DC_Stop(BACKWARD); DC_Stop(FORWARD); } } #INT_RDA void Receive_isr() { ch=getc(); if (ch=='1') chs=1; if (ch=='2') chs=2; if (ch=='3') chs=3; if (ch=='4') chs=4; if (ch=='5') chs=5; if (ch=='6') chs=6; if (ch=='7') chs=7; if (ch=='8') chs=8; if (ch=='9') chs=9; //else chs=0; } void main() { enable_interrupts(INT_RDA); enable_interrupts(INT_EXT0); enable_interrupts(INT_EXT1); ext_int_edge( H_TO_L ); Init_Main(); LCD_Init(); Init_Sensor(); LCD_Clear(); set_timer1(0); while(true) { if(chs==9) { delay_ms(5000);
for(m=1;m<=1000;m++) { Update_Vitri(); Update_Angle(); Update_PID(AA_set); PrinttParameter(); Update_LCD(); A_set = A_set+accXangle; AA_set=A_set/m;
A_set1= (signed int16)(AA_set*1000); printf("A_set%d %d\n"A_set1,gyroX); } chs=0; } Update_Vitri(); Update_Angle(); Update_PID(AA_set); PrinttParameter(); Update_LCD(); if(chs==1)printf("M%d K%d V%d\n"accXa,accX,sp); if(chs==8)
{printf("Kp%d Ki%d Kd%d\n"consKp1,consKi1,consKd1); chs=0;}
if(chs==2)
{consKp=consKp+0.1;
consKp1 = (signed int16)(consKp*10); printf("M%d V%d\n"chs,consKp1); chs=0;}
if(chs==3)
{consKp=consKp-0.1;
consKp1 = (signed int16)(consKp*10); printf("M%d V%d\n"chs,consKp1); chs=0;}
{consKi=consKi+0.1;
consKi1 = (signed int16)(consKi*10); printf("M%d V%d\n"chs,consKi1); chs=0;}
if(chs==5)
{consKi=consKi-0.1;
consKi1 = (signed int16)(consKi*10); printf("M%d V%d\n"chs,consKi1); chs=0;}
if(chs==6)
{consKd=consKd+0.1;
consKd1 = (signed int16)(consKd*10); printf("M%d V%d\n"chs,consKd1); chs=0;}
if(chs==7)
{consKd=consKd-0.1;
consKd1 = (signed int16)(consKd*10); printf("M%d V%d\n"chs,consKd1); chs=0;}
} }
--- --- Code cảm biến MPU6050---
#define GYRO_ZOUT_L 0x48 #define USER_CTRL 0x6A #define PWR_MGMT_1 0x6B #define WHO_AM_I 0x75
#define mpu6050 0xD0 //Dia chi cua cam bien mpu6050 co 7 bit doi sang trai 1 bit de set read or write
//=================================================== =================//Ham Display_Value =================// // Ham nay phai dung chung voi thu vien LCD 16x2 or tuong duong // value gia tri muon xuat hien tren LCD
// Hien thi gia tri value tren LCD tai vi tri dong x va cot thu y void Display_Value(float value, unsigned char x, unsigned char y)
//=========HamMpu6050_Read =========================// // Ham nay dung de doc gia tu cam bien MPU6050
// Luu y la dia chi cua MPU6050 duoc gan truoc boi bien hang: "mpu6050"
// Du lieu dau vao cua ham la dia chi thanh ghi MPU6050 "address" // Ham se tra ve gia tri 8bit
unsigned char Mpu6050_Read(unsigned char address) {
//Doan code viet cho PIC, dung CCS int8 Data; // Khai bao bien Data i2c_start(); // start condition
i2c_write(mpu6050); // Truyen/ghi dia chi den MPU6050
i2c_write(address); // Chon dia chi thanh ghi address cua MPU6050 i2C_start(); // Khoi dong lai I2C
i2C_write(mpu6050|1); // Set MPU6050 truyen du lieu ve Master Data=i2C_read(0); // Doc du lieu tu MPU6050 ve Master i2c_stop(); // Ngung truyen nhan I2C
return Data; //*/ // Tra ve gia tri doc duoc cho bien Data }
//======================Ham GetData ===============// // Day la ham doc du lieu ve tu MPU6050. Ham nay dung chung voi ham Mpu6050_Read
// Ham nay khong co doi so. Ham nay co gia tri tra ve la gia tri doc duoc tu
// cam bien MPU6050
// Gia tri nhap vao ham la dia chi thanh ghi cua MPU6050 // Du lieu nhan ve thuong la 16bit
int16 GetData(unsigned char address) {
int16 H=0,L=0;
//==================================================// i2c_start(); // start condition
i2c_write(mpu6050); // Truyen/ghi dia chi den MPU6050
i2c_write(address); // Chon dia chi thanh ghi address cua MPU6050 i2C_start(); // Khoi dong lai I2C
L=i2C_read(0); // Doc du lieu tu MPU6050 ve Master (8 bit cao) i2c_stop(); // Ngung truyen nhan I2C
//==================================================// //! H=Mpu6050_Read(address); //! L=Mpu6050_Read(address+1); return (H<<8)|L; } //================Ham Mpu6050_Write ================// // Ham nay dung de set dia chi thanh ghi cho MPU6050
// Luu y la dia chi cua MPU6050 duoc gan truoc boi bien hang: "mpu6050"
// Du lieu dau vao cua ham la dia chi thanh ghi MPU6050 "address" // kem theo chuoi "data" muon truyen di
void Mpu6050_Write(unsigned char address,unsigned char Data) {
//Doan code viet cho PIC, dung CCS i2c_start(); // start condition
i2c_write(mpu6050); // Truyen/ghi dia chi den MPU6050
i2c_write(address); // Chon dia chi thanh ghi address cua MPU6050 i2c_write(Data); // Ghi du lieu Data vao vi tri thanh ghi
i2c_stop(); //*/ // Ngung truyen nhan I2C }
//===============Ham Mpu6050_Init =====================// // Ham nay de set che do lam viec cho MPU6050
// Ham nay khong co doi so. Va khong co gia tri tra ve. // Day la ham duoc viet dua tren ham Mpu6050_Write void Mpu6050_Init()
{
//Tat ca thong so trong cac ham duoi day deu duoc khai bao dia chi truoc // hoac tra data sheet cua MPU6050
Mpu6050_Write(PWR_MGMT_1,0X80); // Moi nhet vo nen chua tra lai
delay_ms(5);
Mpu6050_Write(PWR_MGMT_1, 0x00); // Thanh ghi thu 107 cua MPU6050 duoc set
Mpu6050_Write(SMPLRT_DIV, 0x07); // Thanh ghi thu 25 cua MPU6050 duoc set
// sao cho du lieu xuat ra o tan so 1kHz // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
Mpu6050_Write(CONFIG, 0x00); // Thanh ghi thu 26 cua MPU6050 duoc set
// Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
Mpu6050_Write(GYRO_CONFIG, 0x03); // Thanh ghi thu 27 cua MPU6050 duoc set
// Detect goc xoay +_ 250degree/s
Mpu6050_Write(ACCEL_CONFIG, 0x03); // Thanh ghi thu 28 cua MPU6050 duoc set
// Detect gia toc +_ 2.g
Mpu6050_Write(USER_CTRL, 0x00); // 0X6A disable FIFO, disable master mode
Mpu6050_Write(PWR_MGMT_1, 0x01); // PLL with X axis gyroscope reference and disable sleep mode
delay_ms(10); }
//===================== END OF FILE ====================// --- --- Code điều khiển PID---
#define GUARD_GAIN 50.0 #define KI_GAIN 50.0
float error=0; float last_error = 0;
float integrated_error = 0;
float pTerm = 0, iTerm = 0, dTerm = 0; int16 constrain(int16 x,int16 y, int16 z) { if (x>z) x=z; if(x<y) x=y; return x;
int16 Pid_Calculate(float targetPosition, float currentPosition, float K, float Kp, float Ki, float Kd)
{
error = targetPosition - currentPosition; pTerm = Kp * error;
integrated_error += error;
integrated_error = constrain(integrated_error, -KI_GAIN, KI_GAIN); iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error); last_error = error;
return -(int16)constrain(K*(pTerm + iTerm + dTerm), -255, 255); }
--- --- code bộ lọc Kalman ---
float Kalman_Calculate(float newAngle, float newRate,int looptime) {
dt = (float)(looptime)/1000; // x_angle += dt * (newRate - x_bias);