Diff: main.cpp

Revision:

0:cc252ccf8183

Child:

1:0f858d1630a2

--- /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)
+    {
+    }
+}