Rodrigo Gikas / Mbed 2 deprecated Drone_Controlador_Atitude

Crazyflie Attittude Controller

Dependencies:   mbed CrazyflieController CrazyflieSensors USBDevice

main.cpp@0:c5109053e100, 2018-10-08 (annotated)

Committer:
IgneousGuikas
Date:
Mon Oct 08 11:08:56 2018 +0000
Revision:
0:c5109053e100
Child:
1:65d0334fc83a
Drone Controle de Atitude

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IgneousGuikas 0:c5109053e100 1 #include "mbed.h"
IgneousGuikas 0:c5109053e100 2 #include "AttitudeEstimator.h"
IgneousGuikas 0:c5109053e100 3 #include "AttitudeController.h"
IgneousGuikas 0:c5109053e100 4 #include "Mixer.h"
IgneousGuikas 0:c5109053e100 5
IgneousGuikas 0:c5109053e100 6 //Declare attitude estimator object
IgneousGuikas 0:c5109053e100 7 AttitudeEstimator att_est;
IgneousGuikas 0:c5109053e100 8 //Declare attitude controller object
IgneousGuikas 0:c5109053e100 9 AttitudeController att_cont;
IgneousGuikas 0:c5109053e100 10 // Declare mixer
IgneousGuikas 0:c5109053e100 11 Mixer mixer;
IgneousGuikas 0:c5109053e100 12 // Declare timer object
IgneousGuikas 0:c5109053e100 13 Timer timer;
IgneousGuikas 0:c5109053e100 14 // Last interrupt time
IgneousGuikas 0:c5109053e100 15 float last_time = 0.0f;
IgneousGuikas 0:c5109053e100 16
IgneousGuikas 0:c5109053e100 17 int main()
IgneousGuikas 0:c5109053e100 18 {
IgneousGuikas 0:c5109053e100 19 // Wait 5s for safety
IgneousGuikas 0:c5109053e100 20 wait(5);
IgneousGuikas 0:c5109053e100 21 //Initialize attitude estimator
IgneousGuikas 0:c5109053e100 22 att_est.init();
IgneousGuikas 0:c5109053e100 23 //Start timer
IgneousGuikas 0:c5109053e100 24 timer.start();
IgneousGuikas 0:c5109053e100 25 //Run controller for only 5s
IgneousGuikas 0:c5109053e100 26 while( timer.read() <=5.0f)
IgneousGuikas 0:c5109053e100 27 {
IgneousGuikas 0:c5109053e100 28 // Wait 5ms (200 Hz) to perfom next estimate and control update
IgneousGuikas 0:c5109053e100 29 while(( timer.read()-last_time ) <=0.005f)
IgneousGuikas 0:c5109053e100 30 {
IgneousGuikas 0:c5109053e100 31 }
IgneousGuikas 0:c5109053e100 32 // Reset timmer
IgneousGuikas 0:c5109053e100 33 last_time = timer.read();
IgneousGuikas 0:c5109053e100 34 // Estimate attitude
IgneousGuikas 0:c5109053e100 35 att_est.estimate();
IgneousGuikas 0:c5109053e100 36 // Calculate torques (N.m) given estimated attitude ( rad and rad /s)
IgneousGuikas 0:c5109053e100 37 att_cont.control(0.0f, 0.0f, 0.0f, att_est.phi, att_est.theta, att_est.psi, att_est.p, att_est.q, att_est.r);
IgneousGuikas 0:c5109053e100 38 // Actuate motors with given force and calculated torques
IgneousGuikas 0:c5109053e100 39 mixer.actuate(0.25f, att_cont.tau_phi, att_cont.tau_theta, att_cont.tau_psi);
IgneousGuikas 0:c5109053e100 40 }
IgneousGuikas 0:c5109053e100 41 //Turn off all motors
IgneousGuikas 0:c5109053e100 42 mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f);
IgneousGuikas 0:c5109053e100 43 while (1)
IgneousGuikas 0:c5109053e100 44 {
IgneousGuikas 0:c5109053e100 45
IgneousGuikas 0:c5109053e100 46 }
IgneousGuikas 0:c5109053e100 47 }