Codigo COmpleto
Dependencies: CrazyflieController CrazyflieSensors USBDevice mbed
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?
User | Revision | Line number | New 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 | } |