* Sử dụng thêm cảm biến từ trường giúp ổn định hơn trong cân bằng trục Z và xác định hướng của máy bay trong không gian.
* Cân bằng thêm về bị trí trong không gian, giúp mô hình máy bay không bị trôi và có thể đứng yên tại 1 điểm trong không gian mà không cần sự điều khiển của người điều khiển (có thể sử dụng GPS).
* Mô hình có thể trang bị thêm camera truyền hình ảnh trực tiếp về smartphone, máy tính. Bên cạnh đó nếu quacopter đã được ứng xử bay tốt có thể thay đổi bộ xử lí trung tâm sang các loại vi điều khiển khác mạnh hơn, nhanh hơn kết hợp với camera xử lí ảnh để có thể điều khiển quadcopter theo cử chỉ con người
* Từ mô hình bay quadcopter ta có thể vận dụng vào những hoạt động ứng dụng trong cuộc sống như:
Phun thuốc sâu Mang phao cứu hộ
Quay phim toàn cảnh trên cao Những hoạt động quân sự
65
66
KẾT LUẬN
- Nếu không sử dụng sử dụng bộ điều khiển PID thì quadcopter không thể tự động cân bằng, các góc bị lệch rất lớn
- PID hoạt động tốt, giúp máy bay cân bằng trong không gian - Máy bay còn chưa ổn định khi bay cao và khi hạ cánh
- Có thể phát triển mở rộng thêm về các ứng dụng của quadcopter trong đời sống
- Có thể lắp đặt thêm những thiết bị lên quadcopter(ví dụ: GPS) để máy bay ổn định và bay chính xác hơn
67
DANH MỤC THAM KHẢO
[1] M. Eatemadi, Mathematical Dynamics, Kinematics Modeling and PID, Equation Controller of QuadCopter, 2016.
[2] M Islam, M Okasha* and M M Idres, Dynamics and control of quadcopter using linear model predictive control approach, 2017.
[3] Mohammed Abdallah KHODJA,Mohamed TADJINE, Mohamed Seghir BOUCHERIT, Moussa BENZAOUI, Experimental Dynamics Identification and Control of a Quadcopter, 2017.
[4] Jun Li, YuntangLi, Dynamic Analysis and PID Control for a Quadrotor, 2011.
[5] Nicholas Ferry, Quadcopter Plant Model and Control System Development With MATLAB/Simulink Implementation, 2017. [6] Teppo Luukkonen, Modelling and control of quadcopter, 2011. [7] G.H. Tuta Navajas, S. Roa Prada, Building Your Own Quadrotor: A Mechatronics System Design Case Study, 2014.
[8] https://www.youtube.com/watch?v=hGcGPUqB67Q
1
PHỤ LỤC
#include <Wire.h> // Bao gồm thư viện Wire.h để giao tiếp với con quay hồi chuyển.
#include <EEPROM.h> //Bao gồm thư viện EEPROM.h để lưu trữ thông tin vào EEPROM
///////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////
//Cài đặt tăng và giới hạn PID
///////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////
float pid_p_gain_roll = 1.4; //Cài đặt thông số Kp roll float pid_i_gain_roll = 0.05; //Cài đặt thông số Ki roll float pid_d_gain_roll = 15.0; //Cài đặt thông số Kd roll
int pid_max_roll = 400; //Đầu ra tối đa của bộ điều khiển PID (+/-)
float pid_p_gain_pitch = pid_p_gain_roll; float pid_i_gain_pitch = pid_i_gain_roll; float pid_d_gain_pitch = pid_d_gain_roll;
int pid_max_pitch = pid_max_roll;
float pid_p_gain_yaw = 4.0;
float pid_i_gain_yaw = 0.02;
float pid_d_gain_yaw = 0.0;
int pid_max_yaw = 400;
boolean auto_level = true; //Tự động bật (đúng) hoặc tắt (sai)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////
//Khai báo các biến toàn cục
///////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////
2
byte last_channel_1, last_channel_2, last_channel_3, last_channel_4; byte eeprom_data[36];
byte highByte, lowByte;
volatile int receiver_input_channel_1, receiver_input_channel_2, receiver_input_channel_3, receiver_input_channel_4;
int counter_channel_1, counter_channel_2, counter_channel_3, counter_channel_4, loop_counter;
int esc_1, esc_2, esc_3, esc_4; int throttle, battery_voltage; int cal_int, start, gyro_address; int receiver_input[5];
int temperature;
int acc_axis[4], gyro_axis[4];
float roll_level_adjust, pitch_level_adjust;
long acc_x, acc_y, acc_z, acc_total_vector;
unsigned long timer_channel_1, timer_channel_2, timer_channel_3, timer_channel_4, esc_timer, esc_loop_timer;
unsigned long timer_1, timer_2, timer_3, timer_4, current_time; unsigned long loop_timer;
double gyro_pitch, gyro_roll, gyro_yaw; double gyro_axis_cal[4];
float pid_error_temp;
float pid_i_mem_roll, pid_roll_setpoint, gyro_roll_input, pid_output_roll, pid_last_roll_d_error;
float pid_i_mem_pitch, pid_pitch_setpoint, gyro_pitch_input, pid_output_pitch, pid_last_pitch_d_error;
float pid_i_mem_yaw, pid_yaw_setpoint, gyro_yaw_input, pid_output_yaw, pid_last_yaw_d_error;
float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll; boolean gyro_angles_set;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////
3
///////////////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////
void setup(){
//Serial.begin(57600);
//Copy the EEPROM data for fast access data.
for(start = 0; start <= 35; start++)eeprom_data[start] = EEPROM.read(start); start = 0; //Đặt bắt đầu trở lại 0.
gyro_address = eeprom_data[32]; //Lưu trữ địa chỉ con quay hồi chuyển trong biến.
Wire.begin(); //Khởi động I2C với tư cách chính.
TWBR = 12; //Đặt tốc độ đồng hồ I2C thành 400kHz.
//Các chân Arduino (Atmega) mặc định là đầu vào, vì vậy chúng không cần được khai báo rõ ràng là đầu vào
DDRD |= B11110000; //Định cấu hình poort kỹ thuật số 4, 5, 6 và 7 làm đầu ra.
DDRB |= B00110000; //Định cấu hình poort kỹ thuật số 12 và 13 làm đầu ra.
//Sử dụng đèn LED trên Arduino để chỉ báo khởi động.
digitalWrite(12,HIGH); //Bật đèn led cảnh báo.
//Kiểm tra chữ ký EEPROM để đảm bảo rằng chương trình thiết lập được thực thi.
while(eeprom_data[33] != 'N' || eeprom_data[34] != 'D' || eeprom_data[35] != 'P')delay(10);
//Bộ điều khiển chuyến bay cần MPU-6050 với con quay hồi chuyển và gia tốc kế
//Nếu quá trình thiết lập hoàn tất mà không có MPU-6050, hãy dừng chương trình điều khiển chuyến bay
4
if(eeprom_data[31] == 2 || eeprom_data[31] == 3)delay(10);
set_gyro_registers(); //Đặt các thanh ghi con quay hồi chuyển cụ thể.
for (cal_int = 0; cal_int < 1250 ; cal_int ++){
PORTD |= B11110000;
delayMicroseconds(1000);
PORTD &= B00001111;
delayMicroseconds(3000);
} //Hãy lấy nhiều mẫu dữ liệu con quay hồi chuyển để chúng ta có thể xác định độ lệch con quay hồi chuyển trung bình (hiệu chuẩn). for (cal_int = 0; cal_int < 2000 ; cal_int ++){ //Lấy 2000 lần đo để hiệu chuẩn. if(cal_int % 15 == 0)digitalWrite(12, !digitalRead(12)); //Thay đổi trạng thái đèn led để chỉ ra hiệu chuẩn. gyro_signalen(); //Đọc đầu ra con quay hồi chuyển. gyro_axis_cal[1] += gyro_axis[1]; //Giá trị cuộn quảng cáo thành gyro_roll_cal. gyro_axis_cal[2] += gyro_axis[2];
gyro_axis_cal[3] += gyro_axis[3];
// Cung cấp một xung 1000us trong khi hiệu chỉnh con quay hồi chuyển. PORTD |= B11110000; //Set digital poort 4, 5, 6 and 7 high.
delayMicroseconds(1000); //Wait 1000us. PORTD &= B00001111; //Set digital poort 4, 5, 6 and 7 low.
delay(3); //Wait 3 milliseconds before the next loop.
}
5
gyro_axis_cal[1] /= 2000; //Chia tổng số cuộn cho 2000.
gyro_axis_cal[2] /= 2000;
gyro_axis_cal[3] /= 2000;
PCICR |= (1 << PCIE0); //Đặt PCIE0 để bật quét PCMSK0. PCMSK0 |= (1 << PCINT0); //Đặt PCINT0 (đầu vào kỹ thuật số 8) để kích hoạt ngắt khi thay đổi trạng thái. PCMSK0 |= (1 << PCINT1);
PCMSK0 |= (1 << PCINT2);
PCMSK0 |= (1 << PCINT3);
//Chờ cho đến khi bộ thu hoạt động và bướm ga được đặt ở vị trí thấp hơn. while(receiver_input_channel_3 < 990 || receiver_input_channel_3 > 1020 || receiver_input_channel_4 < 1400){ receiver_input_channel_3 = convert_receiver_channel(3);
//Chuyển đổi tín hiệu thu thực tế cho van tiết lưu thành tiêu chuẩn 1000 - 2000us receiver_input_channel_4 = convert_receiver_channel(4);
start ++; //Trong khi chờ đợi bắt đầu tăng dần với mọi vòng lặp. // cung cấp một xung 1000us trong khi chờ đợi đầu vào của máy thu. PORTD |= B11110000; delayMicroseconds(1000); PORTD &= B00001111; delay(3); if(start == 125){ //Cứ sau 125 vòng (500ms). digitalWrite(12, !digitalRead(12)); // Đèn led nhấp nháy start = 0; //Bắt đầu lại từ 0. }
}
6
battery_voltage = (analogRead(0) + 65) * 1.2317;
loop_timer = micros(); //Đặt bộ đếm thời gian cho vòng lặp tiếp theo.
//When everything is done, turn off the led.
digitalWrite(12,LOW); // Tắt led } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////// // Chương trình vòng lặp chính ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////// void loop(){
gyro_roll_input = (gyro_roll_input * 0.7) + ((gyro_roll / 65.5) * 0.3); //Đầu vào con quay hồi chuyển là độ / giây.
gyro_pitch_input = (gyro_pitch_input * 0.7) + ((gyro_pitch / 65.5) * 0.3); gyro_yaw_input = (gyro_yaw_input * 0.7) + ((gyro_yaw / 65.5) * 0.3);
//////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////
//Tính toán góc con quay hồi chuyển //0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_pitch * 0.0000611; //Tính góc nghiêng và thêm giá trị này vào biến angle_pitch.
angle_roll += gyro_roll * 0.0000611;
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) 0,000001066 = 0,0000611 * (3,142 (PI) / 180degr) Hàm Arduino sin tính bằng radian
7
angle_pitch -= angle_roll * sin(gyro_yaw * 0.000001066); //Nếu IMU đã nhận,chuyển góc raw sang góc nghiêng.
angle_roll += angle_pitch * sin(gyro_yaw * 0.000001066);
//Tính toán góc gia tốc kế acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Tính tổng vectơ gia tốc kế. if(abs(acc_y) < acc_total_vector){ angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Tính góc tung độ. } if(abs(acc_x) < acc_total_vector){ angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; }
//Đặt mức linh kiện MPU-6050 và ghi lại các giá trị trong hai dòng sau để hiệu chuẩn.
angle_pitch_acc -= 0.0; //Giá trị hiệu chỉnh gia tốc kế cho trục pitch
angle_roll_acc -= 0.0; //Giá trị hiệu chuẩn gia tốc kế cho trục roll
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Hiệu chỉnh độ lệch góc của con quay hồi chuyển bằng góc của gia tốc kế. angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
pitch_level_adjust = angle_pitch * 15; //Tính toán hiệu chỉnh góc pitch
roll_level_adjust = angle_roll * 15; //Tính toán hiệu chỉnh góc roll
if(!auto_level){ //Nếu quadcopter không ở chế độ tự động cấp
8 pitch_level_adjust = 0; //Đặt hiệu chỉnh góc cao độ thành 0. roll_level_adjust = 0; } if ( start == 0){ esc_1 = 0; esc_2 = 0; esc_3 = 0; esc_4 = 0; }
//Để khởi động động cơ: vặn ga thấp và di chuyển sang trái (bước 1). if(receiver_input_channel_3 < 1050 && receiver_input_channel_4 < 1050)start = 1;
//Khi cần gạt trở lại vị trí trung tâm, khởi động động cơ (bước 2). if(start == 1 && receiver_input_channel_3 < 1050 &&
receiver_input_channel_4 > 1450){ start = 2;
angle_pitch = angle_pitch_acc; //Đặt góc bước của con quay hồi chuyển bằng với góc bước của gia tốc kế khi khởi động quadcopter.
angle_roll = angle_roll_acc; // Đặt góc cuộn của