Yuri De Stefani / Mbed 2 deprecated controladoatitude_cteste

yuri

Dependencies:   mbed CrazyflieController CrazyflieSensors USBDevice

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

Committer:
yvesyuzo
Date:
Wed Oct 10 11:28:53 2018 +0000
Revision:
0:cc252ccf8183
Child:
1:0f858d1630a2
added cmd command to make it easier to upload code to drone;

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 0:cc252ccf8183 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 0:cc252ccf8183 26 while (timer.read () <=5.0f)
yvesyuzo 0:cc252ccf8183 27 {
yvesyuzo 0:cc252ccf8183 28 // Wait 5ms (200 Hz) to perfom next estimate and control update
yvesyuzo 0:cc252ccf8183 29 while (( timer . read () -last_time ) <= 0.005f)
yvesyuzo 0:cc252ccf8183 30 {
yvesyuzo 0:cc252ccf8183 31 }
yvesyuzo 0:cc252ccf8183 32 // Reset timmer
yvesyuzo 0:cc252ccf8183 33 last_time = timer.read ();
yvesyuzo 0:cc252ccf8183 34 // Estimate attitude
yvesyuzo 0:cc252ccf8183 35 AttitudeEstimator.estimate ();
yvesyuzo 0:cc252ccf8183 36 // Calculate torques (N.m) given estimated attitude ( rad and rad /s)
yvesyuzo 0:cc252ccf8183 37 AttitudeController.control (0.0f , 0.0f , 0.0f, AttitudeEstimator.phi , AttitudeEstimator.theta , AttitudeEstimator.psi) ;
yvesyuzo 0:cc252ccf8183 38 // Actuate motors with given force and calculated torques
yvesyuzo 0:cc252ccf8183 39 mixer.actuate (0.0f ,0.0f, AttitudeController.tau_theta ,0.0f);
yvesyuzo 0:cc252ccf8183 40 }
yvesyuzo 0:cc252ccf8183 41 // Turn off all motors
yvesyuzo 0:cc252ccf8183 42 mixer.actuate (0.0f ,0.0f ,0.0f ,0.0f);
yvesyuzo 0:cc252ccf8183 43 while (1)
yvesyuzo 0:cc252ccf8183 44 {
yvesyuzo 0:cc252ccf8183 45 }
yvesyuzo 0:cc252ccf8183 46 }