Yves Yuzo Sakamoto / Mbed 2 deprecated Drones-Controlador

Codigo COmpleto

Dependencies:   CrazyflieController CrazyflieSensors USBDevice mbed

main.cpp@4:8734045fab59, 2018-11-05 (annotated)

Committer:
yvesyuzo
Date:
Mon Nov 05 11:42:49 2018 +0000
Revision:
4:8734045fab59
Parent:
1:0f858d1630a2 
f

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yvesyuzo 0:cc252ccf8183 1 #include "mbed.h"
yvesyuzo 0:cc252ccf8183 2 #include "CrazyflieController.h"
yvesyuzo 0:cc252ccf8183 3
yvesyuzo 0:cc252ccf8183 4 //cmd command to upload code to drone
yvesyuzo 0:cc252ccf8183 5 //dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D love.bin
yvesyuzo 0:cc252ccf8183 6
yvesyuzo 0:cc252ccf8183 7 // Declare attitude estimator object
yvesyuzo 0:cc252ccf8183 8 AttitudeEstimator AttitudeEstimator;
yvesyuzo 0:cc252ccf8183 9 // Declare attitude controller object
yvesyuzo 0:cc252ccf8183 10 AttitudeController AttitudeController;
yvesyuzo 0:cc252ccf8183 11 // Declare mixer
yvesyuzo 0:cc252ccf8183 12 Mixer mixer;
yvesyuzo 0:cc252ccf8183 13 // Declare timer object
yvesyuzo 0:cc252ccf8183 14 Timer timer;
yvesyuzo 0:cc252ccf8183 15 // Last interrupt time
yvesyuzo 0:cc252ccf8183 16 float last_time = 0.0f;
yvesyuzo 0:cc252ccf8183 17 int main()
yvesyuzo 4:8734045fab59 18 {
yvesyuzo 0:cc252ccf8183 19 // Wait 5s for safety
yvesyuzo 0:cc252ccf8183 20 wait (5);
yvesyuzo 0:cc252ccf8183 21 // Initialize attitude estimator
yvesyuzo 0:cc252ccf8183 22 AttitudeEstimator.init();
yvesyuzo 0:cc252ccf8183 23 // Start timer
yvesyuzo 0:cc252ccf8183 24 timer.start();
yvesyuzo 0:cc252ccf8183 25 // Run controller for only 5s
yvesyuzo 4:8734045fab59 26 while (timer.read () <=5.0f) {
yvesyuzo 0:cc252ccf8183 27 // Wait 5ms (200 Hz) to perfom next estimate and control update
yvesyuzo 4:8734045fab59 28 while (( timer.read () -last_time ) <= 0.005f) {
yvesyuzo 0:cc252ccf8183 29 }
yvesyuzo 0:cc252ccf8183 30 // Reset timmer
yvesyuzo 0:cc252ccf8183 31 last_time = timer.read ();
yvesyuzo 0:cc252ccf8183 32 // Estimate attitude
yvesyuzo 0:cc252ccf8183 33 AttitudeEstimator.estimate ();
yvesyuzo 0:cc252ccf8183 34 // Calculate torques (N.m) given estimated attitude ( rad and rad /s)
yvesyuzo 0:cc252ccf8183 35 AttitudeController.control (0.0f , 0.0f , 0.0f, AttitudeEstimator.phi , AttitudeEstimator.theta , AttitudeEstimator.psi) ;
yvesyuzo 0:cc252ccf8183 36 // Actuate motors with given force and calculated torques
yvesyuzo 0:cc252ccf8183 37 mixer.actuate (0.0f ,0.0f, AttitudeController.tau_theta ,0.0f);
yvesyuzo 0:cc252ccf8183 38 }
yvesyuzo 0:cc252ccf8183 39 // Turn off all motors
yvesyuzo 0:cc252ccf8183 40 mixer.actuate (0.0f ,0.0f ,0.0f ,0.0f);
yvesyuzo 4:8734045fab59 41 while (1) {
yvesyuzo 0:cc252ccf8183 42 }
yvesyuzo 0:cc252ccf8183 43 }