yuri
Dependencies: mbed CrazyflieController CrazyflieSensors USBDevice
main.cpp@5:d8e1cf0be63d, 2018-11-21 (annotated)
- Committer:
- yurindes
- Date:
- Wed Nov 21 10:10:08 2018 +0000
- Revision:
- 5:d8e1cf0be63d
- Parent:
- 1:0f858d1630a2
yu
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yvesyuzo | 0:cc252ccf8183 | 1 | #include "mbed.h" |
yvesyuzo | 0:cc252ccf8183 | 2 | #include "CrazyflieController.h" |
yurindes | 5:d8e1cf0be63d | 3 | // Crazyflie controller objects |
yurindes | 5:d8e1cf0be63d | 4 | //dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D controladoatitude.NUCLEO_F446ZE.bin |
yurindes | 5:d8e1cf0be63d | 5 | AttitudeEstimator att_est ; |
yurindes | 5:d8e1cf0be63d | 6 | AttitudeController att_cont ; |
yurindes | 5:d8e1cf0be63d | 7 | Mixer mixer ; |
yurindes | 5:d8e1cf0be63d | 8 | // Timer and tickers |
yurindes | 5:d8e1cf0be63d | 9 | Timer tim; |
yurindes | 5:d8e1cf0be63d | 10 | Ticker tic ; |
yurindes | 5:d8e1cf0be63d | 11 | // Interrupt flag |
yurindes | 5:d8e1cf0be63d | 12 | bool flag = false ; |
yurindes | 5:d8e1cf0be63d | 13 | // Callback function |
yurindes | 5:d8e1cf0be63d | 14 | void callback () |
yurindes | 5:d8e1cf0be63d | 15 | { |
yurindes | 5:d8e1cf0be63d | 16 | flag = true ; |
yurindes | 5:d8e1cf0be63d | 17 | } |
yurindes | 5:d8e1cf0be63d | 18 | // Current time |
yurindes | 5:d8e1cf0be63d | 19 | float current_time ; |
yurindes | 5:d8e1cf0be63d | 20 | // Main program |
yurindes | 5:d8e1cf0be63d | 21 | int main () |
yurindes | 5:d8e1cf0be63d | 22 | { |
yurindes | 5:d8e1cf0be63d | 23 | // Set references |
yurindes | 5:d8e1cf0be63d | 24 | float f_t = 1.0f * m * g; |
yurindes | 5:d8e1cf0be63d | 25 | float phi_r = 0.0f; |
yurindes | 5:d8e1cf0be63d | 26 | float theta_r = 0.0f; |
yurindes | 5:d8e1cf0be63d | 27 | float psi_r = 0.0f; |
yvesyuzo | 0:cc252ccf8183 | 28 | // Wait 5s for safety |
yvesyuzo | 0:cc252ccf8183 | 29 | wait (5); |
yurindes | 5:d8e1cf0be63d | 30 | // Initialize estimator object |
yurindes | 5:d8e1cf0be63d | 31 | att_est.init () ; |
yurindes | 5:d8e1cf0be63d | 32 | // Initialize interrupts |
yurindes | 5:d8e1cf0be63d | 33 | tic.attach (&callback , dt); |
yvesyuzo | 0:cc252ccf8183 | 34 | // Start timer |
yurindes | 5:d8e1cf0be63d | 35 | tim.start () ; |
yurindes | 5:d8e1cf0be63d | 36 | while ( current_time <=10.0f) |
yvesyuzo | 0:cc252ccf8183 | 37 | { |
yurindes | 5:d8e1cf0be63d | 38 | // Update current time |
yurindes | 5:d8e1cf0be63d | 39 | current_time = tim.read () ; |
yurindes | 5:d8e1cf0be63d | 40 | // Estimate and control |
yurindes | 5:d8e1cf0be63d | 41 | if ( flag ) |
yvesyuzo | 0:cc252ccf8183 | 42 | { |
yurindes | 5:d8e1cf0be63d | 43 | flag = false ; |
yurindes | 5:d8e1cf0be63d | 44 | att_est.estimate () ; |
yurindes | 5:d8e1cf0be63d | 45 | att_cont.control (phi_r , theta_r ,psi_r , att_est.phi , att_est.theta , att_est.psi); |
yurindes | 5:d8e1cf0be63d | 46 | mixer.actuate (f_t ,0.0f, att_cont.tau_theta ,0.0f); |
yvesyuzo | 0:cc252ccf8183 | 47 | } |
yvesyuzo | 0:cc252ccf8183 | 48 | } |
yvesyuzo | 0:cc252ccf8183 | 49 | // Turn off all motors |
yvesyuzo | 0:cc252ccf8183 | 50 | mixer.actuate (0.0f ,0.0f ,0.0f ,0.0f); |
yurindes | 5:d8e1cf0be63d | 51 | while (1) ; |
yurindes | 5:d8e1cf0be63d | 52 | } |