Crazyflie Attittude Controller
Dependencies: mbed CrazyflieController CrazyflieSensors USBDevice
Diff: main.cpp
- Revision:
- 1:65d0334fc83a
- Parent:
- 0:c5109053e100
- Child:
- 2:6ca1b4dcb9f7
diff -r c5109053e100 -r 65d0334fc83a main.cpp --- a/main.cpp Mon Oct 08 11:08:56 2018 +0000 +++ b/main.cpp Tue Oct 09 18:02:04 2018 +0000 @@ -1,7 +1,5 @@ #include "mbed.h" -#include "AttitudeEstimator.h" -#include "AttitudeController.h" -#include "Mixer.h" +#include "CrazyflieController.h" //Declare attitude estimator object AttitudeEstimator att_est; @@ -11,8 +9,17 @@ Mixer mixer; // Declare timer object Timer timer; + +Ticker tic; + +bool flag = false; + +void callback() +{ + flag = true; +} // Last interrupt time -float last_time = 0.0f; +float current_time = 0.0f; int main() { @@ -21,27 +28,31 @@ //Initialize attitude estimator att_est.init(); //Start timer + tic.attach(&callback,dt); + timer.start(); //Run controller for only 5s - while( timer.read() <=5.0f) + while( current_time <=10.0f) { // Wait 5ms (200 Hz) to perfom next estimate and control update - while(( timer.read()-last_time ) <=0.005f) + current_time = timer.read(); + + if(flag) { + flag = false; + // Estimate attitude + att_est.estimate(); + // Calculate torques (N.m) given estimated attitude ( rad and rad /s) + 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); + // Actuate motors with given force and calculated torques + mixer.actuate(0.25f, att_cont.tau_phi, att_cont.tau_theta, 0.0f); } - // Reset timmer - last_time = timer.read(); - // Estimate attitude - att_est.estimate(); - // Calculate torques (N.m) given estimated attitude ( rad and rad /s) - 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); - // Actuate motors with given force and calculated torques - mixer.actuate(0.25f, att_cont.tau_phi, att_cont.tau_theta, att_cont.tau_psi); + 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) + { + break; + } } //Turn off all motors mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f); - while (1) - { - - } + while (1); } \ No newline at end of file