Yuri De Stefani / Mbed 2 deprecated VerticalController

Controlador vertical

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }