Tải bản đầy đủ - 0 (trang)
9 Kết quả mô hình

9 Kết quả mô hình

Tải bản đầy đủ - 0trang

Trang 18/33



Hình 3-3 Mơ hình máy bay



• Lắp ráp hồn chỉnh, kết nối các thành phần điều khiển, điều khiển được bằng bộ thu

phát RF 2.4Ghz.

• Máy bay còn chưa cân bằng do phần cứng chưa cân bằng và sai sót trong khâu hiệu

chỉnh.



Thiết kế máy bay điều khiển



Trang 19/33



CHƯƠNG 4.



KẾT LUẬN



1.10 Kết luận

• Hiểu biết cách hoạt động, nguyên lý bay của máy bay điều khiển.

• Sai số phần cứng cùng với chưa giảm bớt được nhiễu từ cảm biến nên máy bay khi

bay còn chưa được cân bằng hồn chỉnh.

• Chưa có kinh nghiệm điều khiển nên khi điều khiển còn sai sót, dễ gây nguy hiểm

cho xung quanh và hư hỏng phần cứng. Cần thời gian nghiên cứu và luyện tập kỹ

năng điều khiển.

1.11 Hướng phát triển

• Nghiên cứu, hiệu chỉnh cân bằng tốt hơn để máy bay hồn thiện, an tồn.

• Gắn thêm các cảm biến tăng độ an toàn cho máy bay và người xung quanh, GPS giữ

máy bay trong khoảng không gian xác định, camera quay phim, chụp hình .



Thiết kế máy bay điều khiển



Trang 20/33



TÀI LIỆU THAM KHẢO

Tiếng Việt:

[1] https://www.arduino.vn/en/Main/ArduinoBoardNano.

[2] http://playground.arduino.cc/Main/MPU-6050.

[3] https://oscarliang.com/kiss-24a-esc-re/.

[4] http://quanphongrc.vn/ct/may-bay-dieu-khien/1366/himodel-xxd-22121000kv-dau-dan.html.

[5] Datasheet MPU6050.

[6] Data sheet Atmega328p.

Tiếng Anh:

[7] https://www.arduino.cc/en/Main/ArduinoBoardNano.

[8] https://www.youtube.com/user/MacPuffdog.

[9] http://mydronelab.com/blog/arduino-quadcopter.html.



Thiết kế máy bay điều khiển



Trang 21/33



PHỤ LỤC

#include

#include

float Kp_roll = 2.5; //Gain setting for the roll P-controller

float Ki_roll = 0.03;//Gain setting for the roll I-controller

float Kd_roll = 22.5;//Gain setting for the roll D-controller

int pid_max_roll = 300; //Maximum output of the PID-controller (+/-)

float Kp_pitch = Kp_roll;

float Ki_pitch = Ki_roll;

float Kd_pitch = Kd_roll;

int pid_max_pitch = pid_max_roll;

float Kp_yaw = 2.8;

float Ki_yaw = 0.02;

float Kd_yaw = 0.05;

int pid_max_yaw = 300;

boolean mode_auto = true;//Auto level on (true)

byte last_channel_1, last_channel_2, last_channel_3, last_channel_4;

byte eeprom_data[36];

byte highByte, lowByte;

volatile int tin_hieu_kenh1, tin_hieu_kenh2, tin_hieu_kenh3, tin_hieu_kenh4;

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 tin_hieu_thu[5];

int temperature;

int truc_giatoc[4], truc_vantocgoc[4];



Thiết kế máy bay điều khiển



Trang 22/33



float roll_dieuchinh, pitch_dieuchinh;

long giatoc_x, giatoc_y, giatoc_z, tong_giatoc;

unsigned long timer_kenh1, timer_kenh2, timer_kenh3, timer_kenh4, 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 truc_vantocgoc_cal[4];

float loi_pid;

float tong_pid_i_roll, roll_setpoint, gyro_roll_input, pid_output_roll,

last_roll_d_error;

float tong_pid_i_pitch, pitch_setpoint, gyro_pitch_input, pid_output_pitch,

last_pitch_d_error;

float tong_pid_i_yaw, yaw_setpoint, gyro_yaw_input, pid_output_yaw,

last_yaw_d_error;

float goc_roll_acc, goc_pitch_acc, goc_pitch, goc_roll;

boolean gyro_angles_set;

void setup(){

//Copy the EEPROM data for fast access data.

for(start = 0; start <= 35; start++)eeprom_data[start] = EEPROM.read(start);

start = 0; //Set start back to zero.

gyro_address = eeprom_data[32]; //Store the gyro address in the variable.

Wire.begin();

TWBR = 12;//Set the I2C clock speed to 400kHz.

DDRD |= B11110000;//Configure digital poort 4, 5, 6 and 7 as output.

DDRB |= B00110000;//Configure digital poort 12 and 13 as output.

digitalWrite(12,HIGH);//Turn on the warning led.

//Check the EEPROM signature, make sure that the setup program is executed.



Thiết kế máy bay điều khiển



Trang 23/33



while(eeprom_data[33] != 'J' || eeprom_data[34] != 'M' || eeprom_data[35] !=

'B')delay(10);

//If setup is completed without MPU-6050 stop the flight controller program

if(eeprom_data[31] == 2 || eeprom_data[31] == 3)delay(10);

set_gyro_registers();//Set the specific gyro registers.

for (cal_int = 0; cal_int < 1250 ; cal_int ++){//Wait 5 seconds before continuing.

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.

delayMicroseconds(3000);

}

//multiple gyro data samples so we can determine the average gyro offset

(calibration).

for (cal_int = 0; cal_int < 2000 ; cal_int ++){//Take 2000 readings for calibration.

if(cal_int % 15 == 0)digitalWrite(12, !digitalRead(12));

gyro_signalen();//Read the gyro output.

truc_vantocgoc_cal[1] += truc_vantocgoc[1];

truc_vantocgoc_cal[2] += truc_vantocgoc[2];

truc_vantocgoc_cal[3] += truc_vantocgoc[3]; //Ad yaw value to

truc_vantocgoc_cal.

//We don't want the esc's to be beeping annoyingly.

PORTD |= B11110000;//Set digital poort 4, 5, 6 and 7 high.

delayMicroseconds(1000);

PORTD &= B00001111;//Set digital poort 4, 5, 6 and 7 low.

delay(3);

}

//devide by 2000 to get the average gyro offset.

truc_vantocgoc_cal[1] /= 2000;//Divide the roll total by 2000.

truc_vantocgoc_cal[2] /= 2000;



Thiết kế máy bay điều khiển



Trang 24/33



truc_vantocgoc_cal[3] /= 2000;

PCICR |= (1 << PCIE0); //Set PCIE0 to enable PCMSK0 scan.

PCMSK0 |= (1 << PCINT0); //Set PCINT0(digital input 8) ngắt khi trạng thái thay

đổi.

PCMSK0 |= (1 << PCINT1);

PCMSK0 |= (1 << PCINT2);

PCMSK0 |= (1 << PCINT3);

//Wait until the receiver is active and the throtle is set to the lower position.

while(tin_hieu_kenh3 < 990 || tin_hieu_kenh3 > 1020 || tin_hieu_kenh4 < 1400){

tin_hieu_kenh3 = convert_receiver_channel(3);//Convert the actual receiver

signals for throttle to the standard 1000 - 2000us

tin_hieu_kenh4 = convert_receiver_channel(4);

start ++; //While waiting increment start whith every loop.

PORTD |= B11110000;

delayMicroseconds(1000);

PORTD &= B00001111;

delay(3);

if(start == 125){ // 125 loops (500ms).

digitalWrite(12, !digitalRead(12));//Change the led status.

start = 0;

}

}

start = 0; //Set start về 0.

//Load the battery voltage to the battery_voltage variable.

//65 is the voltage compensation for the diode.

//12.6V equals ~5V @ Analog 0.

//12.6V equals 1023 analogRead(0).

//1260 / 1023 = 1.2317.

//The variable battery_voltage holds 1050 if the battery voltage is 10.5V.



Thiết kế máy bay điều khiển



Trang 25/33



battery_voltage = (analogRead(0) + 65) * 1.2317;

loop_timer = micros(); //Set the timer for the next loop.

//When everything is done, turn off the led.

digitalWrite(12,LOW);

}

void loop(){

//65.5 = 1 deg/sec (check the datasheet of the MPU-6050 for more information).

gyro_roll_input = (gyro_roll_input * 0.8) + ((gyro_roll / 65.5) * 0.2); // pid input

deg/sec.

gyro_pitch_input = (gyro_pitch_input * 0.8) + ((gyro_pitch / 65.5) * 0.2);// pid

input deg/sec.

gyro_yaw_input = (gyro_yaw_input * 0.8) + ((gyro_yaw / 65.5) * 0.2);



// pid



input deg/sec.

//tính góc

//0.0000611 = 1/(250Hz / 65.5)

goc_pitch += gyro_pitch * 0.0000611; //Calculate the traveled pitch angle and add

this to the goc_pitch variable.

goc_roll += gyro_roll * 0.0000611;

//0.000001066 = 0.0000611*(3.142(PI)/180degr) The Arduino sin function is in

radians

goc_pitch -= goc_roll * sin(gyro_yaw * 0.000001066);//If the IMU has yawed

transfer the roll angle to the pitch angel.

goc_roll += goc_pitch * sin(gyro_yaw * 0.000001066);

//tính gia tốc

tong_giatoc = sqrt((giatoc_x*giatoc_x)+(giatoc_y*giatoc_y)+

(giatoc_z*giatoc_z));

if((giatoc_y) < tong_giatoc){

goc_pitch_acc = asin((float)giatoc_y/tong_giatoc)* 57.296;//tính góc trục pitch

}



Thiết kế máy bay điều khiển



Trang 26/33



if((giatoc_x) < tong_giatoc){ //Prevent the asin function to produce a NaN

goc_roll_acc = asin((float)giatoc_x/tong_giatoc)* -57.296;//tính góc trục roll

}

//Place the MPU-6050 spirit level and note the values in the following two lines

for calibration.

goc_pitch_acc -= 0.0;//Accelerometer calibration value for pitch.

goc_roll_acc -= 0.0;

goc_pitch = goc_pitch * 0.9996 + goc_pitch_acc * 0.0004;

goc_roll = goc_roll * 0.9996 + goc_roll_acc * 0.0004;//Correct the drift of the

gyro roll angle with the accelerometer roll angle.

pitch_dieuchinh = goc_pitch * 15;

roll_dieuchinh = goc_roll * 15;//Calculate the roll angle correction

if(!mode_auto){

pitch_dieuchinh = 0;

roll_dieuchinh = 0;//Set the roll angle correcion to zero.

}

if(tin_hieu_kenh3 < 1050 && tin_hieu_kenh4 < 1050)start = 1;

//quay motor

if(start == 1 && tin_hieu_kenh3 < 1050 && tin_hieu_kenh4 > 1450){

start = 2;

goc_pitch = goc_pitch_acc;

goc_roll = goc_roll_acc;//Set the gyro roll angle equal to the accelerometer roll

angle when the quadcopter is started.

gyro_angles_set = true;//Set the IMU started flag.

//Reset the PID

tong_pid_i_roll = 0;

last_roll_d_error = 0;

tong_pid_i_pitch = 0;

last_pitch_d_error = 0;



Thiết kế máy bay điều khiển



Trang 27/33



tong_pid_i_yaw = 0;

last_yaw_d_error = 0;

}

//dừng motor

if(start == 2 && tin_hieu_kenh3 < 1050 && tin_hieu_kenh4 > 1950)start = 0;

//The PID set point in degrees per second is determined by the roll receiver input.

//In the case of deviding by 3 the max roll rate is aprox 164 degrees per second

( (500-8)/3 = 164d/s ).

roll_setpoint = 0;

if(tin_hieu_kenh1 > 1508)roll_setpoint = tin_hieu_kenh1 - 1508;

else if(tin_hieu_kenh1 < 1492)roll_setpoint = tin_hieu_kenh1 - 1492;

roll_setpoint -= roll_dieuchinh;//Subtract the angle correction from the

standardized receiver roll input value.

roll_setpoint /= 3.0;//Divide the setpoint for the PID roll controller by 3 to get

angles in degrees.

pitch_setpoint = 0;

if(tin_hieu_kenh2 > 1508)pitch_setpoint = tin_hieu_kenh2 - 1508;

else if(tin_hieu_kenh2 < 1492)pitch_setpoint = tin_hieu_kenh2 - 1492;

pitch_setpoint -= pitch_dieuchinh;

pitch_setpoint /= 3.0;

yaw_setpoint = 0;

if(tin_hieu_kenh3 > 1050){

if(tin_hieu_kenh4 > 1508)yaw_setpoint = (tin_hieu_kenh4 - 1508)/3.0;

else if(tin_hieu_kenh4 < 1492)yaw_setpoint = (tin_hieu_kenh4 - 1492)/3.0;

}

calculate_pid();



//PID inputs are known. So we can



calculate the pid output.

//A complementary filter is used to reduce noise.

//0.09853 = 0.08 * 1.2317.



Thiết kế máy bay điều khiển



Trang 28/33



battery_voltage = battery_voltage * 0.92 + (analogRead(0) + 65) * 0.09853;

if(battery_voltage < 1000 && battery_voltage > 600)digitalWrite(12, HIGH);

throttle = tin_hieu_kenh3;// throttle signal as a base signal.

if (start == 2){

if (throttle > 1800) throttle = 1800;//giới hạn throttle

esc_1 = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw;//(trướcphải - CCW)

esc_2 = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw;//(sauphải - CW)

esc_3 = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw;//(sau-trái

- CCW)

esc_4 = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw;//(trướctrái - CW)

if (esc_1 < 1150) esc_1 = 1150;//giữ motor quay ở dưới đất.

if (esc_2 < 1150) esc_2 = 1150;

if (esc_3 < 1150) esc_3 = 1150;

if (esc_4 < 1150) esc_4 = 1150;

if(esc_1 > 1900)esc_1 = 1900;//giới hạn tốc độ

if(esc_2 > 1900)esc_2 = 1900;

if(esc_3 > 1900)esc_3 = 1900;

if(esc_4 > 1900)esc_4 = 1900;

}

else{

esc_1 = 1000;//nếu start != 2 giữ tốc độ 1000ms

esc_2 = 1000;

esc_3 = 1000;

esc_4 = 1000;

}



Thiết kế máy bay điều khiển



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

9 Kết quả mô hình

Tải bản đầy đủ ngay(0 tr)

×