Rodrigo Gikas / Mbed 2 deprecated Drone_Controlador_Atitude

Crazyflie Attittude Controller

Dependencies:   mbed CrazyflieController CrazyflieSensors USBDevice

main.cpp@1:65d0334fc83a, 2018-10-09 (annotated)

Committer:
IgneousGuikas
Date:
Tue Oct 09 18:02:04 2018 +0000
Revision:
1:65d0334fc83a
Parent:
0:c5109053e100
Child:
2:6ca1b4dcb9f7
Vers?o atualizada com par?metros de seguran?a.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IgneousGuikas 0:c5109053e100 1 #include "mbed.h"
IgneousGuikas 1:65d0334fc83a 2 #include "CrazyflieController.h"
IgneousGuikas 0:c5109053e100 3
IgneousGuikas 0:c5109053e100 4 //Declare attitude estimator object
IgneousGuikas 0:c5109053e100 5 AttitudeEstimator att_est;
IgneousGuikas 0:c5109053e100 6 //Declare attitude controller object
IgneousGuikas 0:c5109053e100 7 AttitudeController att_cont;
IgneousGuikas 0:c5109053e100 8 // Declare mixer
IgneousGuikas 0:c5109053e100 9 Mixer mixer;
IgneousGuikas 0:c5109053e100 10 // Declare timer object
IgneousGuikas 0:c5109053e100 11 Timer timer;
IgneousGuikas 1:65d0334fc83a 12
IgneousGuikas 1:65d0334fc83a 13 Ticker tic;
IgneousGuikas 1:65d0334fc83a 14
IgneousGuikas 1:65d0334fc83a 15 bool flag = false;
IgneousGuikas 1:65d0334fc83a 16
IgneousGuikas 1:65d0334fc83a 17 void callback()
IgneousGuikas 1:65d0334fc83a 18 {
IgneousGuikas 1:65d0334fc83a 19 flag = true;
IgneousGuikas 1:65d0334fc83a 20 }
IgneousGuikas 0:c5109053e100 21 // Last interrupt time
IgneousGuikas 1:65d0334fc83a 22 float current_time = 0.0f;
IgneousGuikas 0:c5109053e100 23
IgneousGuikas 0:c5109053e100 24 int main()
IgneousGuikas 0:c5109053e100 25 {
IgneousGuikas 0:c5109053e100 26 // Wait 5s for safety
IgneousGuikas 0:c5109053e100 27 wait(5);
IgneousGuikas 0:c5109053e100 28 //Initialize attitude estimator
IgneousGuikas 0:c5109053e100 29 att_est.init();
IgneousGuikas 0:c5109053e100 30 //Start timer
IgneousGuikas 1:65d0334fc83a 31 tic.attach(&callback,dt);
IgneousGuikas 1:65d0334fc83a 32
IgneousGuikas 0:c5109053e100 33 timer.start();
IgneousGuikas 0:c5109053e100 34 //Run controller for only 5s
IgneousGuikas 1:65d0334fc83a 35 while( current_time <=10.0f)
IgneousGuikas 0:c5109053e100 36 {
IgneousGuikas 0:c5109053e100 37 // Wait 5ms (200 Hz) to perfom next estimate and control update
IgneousGuikas 1:65d0334fc83a 38 current_time = timer.read();
IgneousGuikas 1:65d0334fc83a 39
IgneousGuikas 1:65d0334fc83a 40 if(flag)
IgneousGuikas 0:c5109053e100 41 {
IgneousGuikas 1:65d0334fc83a 42 flag = false;
IgneousGuikas 1:65d0334fc83a 43 // Estimate attitude
IgneousGuikas 1:65d0334fc83a 44 att_est.estimate();
IgneousGuikas 1:65d0334fc83a 45 // Calculate torques (N.m) given estimated attitude ( rad and rad /s)
IgneousGuikas 1:65d0334fc83a 46 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 1:65d0334fc83a 47 // Actuate motors with given force and calculated torques
IgneousGuikas 1:65d0334fc83a 48 mixer.actuate(0.25f, att_cont.tau_phi, att_cont.tau_theta, 0.0f);
IgneousGuikas 0:c5109053e100 49 }
IgneousGuikas 1:65d0334fc83a 50 if(att_est.phi >= pi/4.0f || att_est.theta >= pi/4.0f || att_est.p >= 4.0f*pi || att_est.q >= 4.0f*pi)
IgneousGuikas 1:65d0334fc83a 51 {
IgneousGuikas 1:65d0334fc83a 52 break;
IgneousGuikas 1:65d0334fc83a 53 }
IgneousGuikas 0:c5109053e100 54 }
IgneousGuikas 0:c5109053e100 55 //Turn off all motors
IgneousGuikas 0:c5109053e100 56 mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f);
IgneousGuikas 1:65d0334fc83a 57 while (1);
IgneousGuikas 0:c5109053e100 58 }