Codigo COmpleto
Dependencies: CrazyflieController CrazyflieSensors USBDevice mbed
Diff: main.cpp
- Revision:
- 0:cc252ccf8183
- Child:
- 1:0f858d1630a2
diff -r 000000000000 -r cc252ccf8183 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 10 11:28:53 2018 +0000 @@ -0,0 +1,46 @@ +#include "mbed.h" +#include "CrazyflieController.h" + +//cmd command to upload code to drone +//dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D love.bin + +// Declare attitude estimator object +AttitudeEstimator AttitudeEstimator; +// Declare attitude controller object +AttitudeController AttitudeController; +// Declare mixer +Mixer mixer; +// Declare timer object +Timer timer; +// Last interrupt time +float last_time = 0.0f; +int main() + { + // Wait 5s for safety + wait (5); + // Initialize attitude estimator + AttitudeEstimator.init(); + // Start timer + timer.start(); + // Run controller for only 5s + while (timer.read () <=5.0f) + { + // Wait 5ms (200 Hz) to perfom next estimate and control update + while (( timer . read () -last_time ) <= 0.005f) + { + } + // Reset timmer + last_time = timer.read (); + // Estimate attitude + AttitudeEstimator.estimate (); + // Calculate torques (N.m) given estimated attitude ( rad and rad /s) + AttitudeController.control (0.0f , 0.0f , 0.0f, AttitudeEstimator.phi , AttitudeEstimator.theta , AttitudeEstimator.psi) ; + // Actuate motors with given force and calculated torques + mixer.actuate (0.0f ,0.0f, AttitudeController.tau_theta ,0.0f); + } + // Turn off all motors + mixer.actuate (0.0f ,0.0f ,0.0f ,0.0f); + while (1) + { + } +}