Yuri De Stefani
/
VerticalController
Controlador vertical
main.cpp@0:115db3c33311, 2018-10-24 (annotated)
- Committer:
- yurindes
- Date:
- Wed Oct 24 11:58:36 2018 +0000
- Revision:
- 0:115db3c33311
controlador vertical :)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yurindes | 0:115db3c33311 | 1 | # include "mbed.h" |
yurindes | 0:115db3c33311 | 2 | # include "CrazyflieController.h" |
yurindes | 0:115db3c33311 | 3 | // Crazyflie controller objects |
yurindes | 0:115db3c33311 | 4 | //dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D love.bin |
yurindes | 0:115db3c33311 | 5 | AttitudeEstimator att_est ; |
yurindes | 0:115db3c33311 | 6 | AttitudeController att_cont ; |
yurindes | 0:115db3c33311 | 7 | VerticalEstimator ver_est ; |
yurindes | 0:115db3c33311 | 8 | VerticalController ver_cont; |
yurindes | 0:115db3c33311 | 9 | Mixer mixer; |
yurindes | 0:115db3c33311 | 10 | // Timer and tickers |
yurindes | 0:115db3c33311 | 11 | Timer tim; |
yurindes | 0:115db3c33311 | 12 | Ticker tic; |
yurindes | 0:115db3c33311 | 13 | Ticker tic_range; |
yurindes | 0:115db3c33311 | 14 | // Interrupt flags |
yurindes | 0:115db3c33311 | 15 | bool flag = false; |
yurindes | 0:115db3c33311 | 16 | bool flag_range = false; |
yurindes | 0:115db3c33311 | 17 | // Callback function |
yurindes | 0:115db3c33311 | 18 | void callback() |
yurindes | 0:115db3c33311 | 19 | { |
yurindes | 0:115db3c33311 | 20 | flag = true; |
yurindes | 0:115db3c33311 | 21 | } |
yurindes | 0:115db3c33311 | 22 | // Callback function ( range correct ) |
yurindes | 0:115db3c33311 | 23 | void callback_range() |
yurindes | 0:115db3c33311 | 24 | { |
yurindes | 0:115db3c33311 | 25 | flag_range = true; |
yurindes | 0:115db3c33311 | 26 | } |
yurindes | 0:115db3c33311 | 27 | // Current time |
yurindes | 0:115db3c33311 | 28 | float current_time; |
yurindes | 0:115db3c33311 | 29 | // Main program |
yurindes | 0:115db3c33311 | 30 | int main () |
yurindes | 0:115db3c33311 | 31 | { |
yurindes | 0:115db3c33311 | 32 | // Set references |
yurindes | 0:115db3c33311 | 33 | float z_r = 0.1f; |
yurindes | 0:115db3c33311 | 34 | float phi_r = 0.0f; |
yurindes | 0:115db3c33311 | 35 | float theta_r = 0.0f; |
yurindes | 0:115db3c33311 | 36 | float psi_r = 0.0f; |
yurindes | 0:115db3c33311 | 37 | // Wait 5s for safety |
yurindes | 0:115db3c33311 | 38 | wait (5); |
yurindes | 0:115db3c33311 | 39 | // Initialize estimators objects |
yurindes | 0:115db3c33311 | 40 | att_est.init(); |
yurindes | 0:115db3c33311 | 41 | ver_est.init(); |
yurindes | 0:115db3c33311 | 42 | // Initialize interrupts |
yurindes | 0:115db3c33311 | 43 | tic.attach(&callback,dt); |
yurindes | 0:115db3c33311 | 44 | tic_range.attach(&callback_range ,dt_range); |
yurindes | 0:115db3c33311 | 45 | // Start timer |
yurindes | 0:115db3c33311 | 46 | tim.start(); |
yurindes | 0:115db3c33311 | 47 | while (tim.read()<=5.0f) |
yurindes | 0:115db3c33311 | 48 | { |
yurindes | 0:115db3c33311 | 49 | // Estimate and control |
yurindes | 0:115db3c33311 | 50 | if (flag) |
yurindes | 0:115db3c33311 | 51 | { |
yurindes | 0:115db3c33311 | 52 | flag = false; |
yurindes | 0:115db3c33311 | 53 | att_est.estimate(); |
yurindes | 0:115db3c33311 | 54 | ver_est.predict(); |
yurindes | 0:115db3c33311 | 55 | ver_cont.control(z_r,ver_est.z,ver_est.w); |
yurindes | 0:115db3c33311 | 56 | att_cont.control(phi_r,theta_r,psi_r,att_est.phi,att_est.theta,att_est.psi,att_est.p,att_est.q,att_est.r); |
yurindes | 0:115db3c33311 | 57 | mixer.actuate(ver_cont.f_t,att_cont.tau_phi,att_cont.tau_theta,att_cont.tau_ps); |
yurindes | 0:115db3c33311 | 58 | } |
yurindes | 0:115db3c33311 | 59 | // Correct vertical estimation |
yurindes | 0:115db3c33311 | 60 | if (flag_range) |
yurindes | 0:115db3c33311 | 61 | { |
yurindes | 0:115db3c33311 | 62 | flag_range = false; |
yurindes | 0:115db3c33311 | 63 | ver_est.correct(att_est.phi,att_est.theta); |
yurindes | 0:115db3c33311 | 64 | } |
yurindes | 0:115db3c33311 | 65 | // Turn off all motors |
yurindes | 0:115db3c33311 | 66 | mixer.actuate(0.0f ,0.0f ,0.0f ,0.0f); |
yurindes | 0:115db3c33311 | 67 | while(1); |
yurindes | 0:115db3c33311 | 68 | } |