Diff: main.cpp

Revision:

5:d8e1cf0be63d

Parent:

1:0f858d1630a2

--- a/main.cpp	Wed Oct 17 12:13:57 2018 +0000
+++ b/main.cpp	Wed Nov 21 10:10:08 2018 +0000
@@ -1,46 +1,52 @@
 #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()
-    {
+// Crazyflie controller objects
+//dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D controladoatitude.NUCLEO_F446ZE.bin
+AttitudeEstimator att_est ;
+AttitudeController att_cont ;
+Mixer mixer ;
+// Timer and tickers
+Timer tim;
+Ticker tic ;
+// Interrupt flag
+bool flag = false ;
+// Callback function
+void callback ()
+{
+    flag = true ;
+}
+// Current time
+float current_time ;
+// Main program
+int main ()
+{
+    // Set references
+    float f_t = 1.0f * m * g;
+    float phi_r = 0.0f;
+    float theta_r = 0.0f;
+    float psi_r = 0.0f;
     // Wait 5s for safety
     wait (5);
-    // Initialize attitude estimator
-    AttitudeEstimator.init();
+    // Initialize estimator object
+    att_est.init () ;
+    // Initialize interrupts
+    tic.attach (&callback , dt);
     // Start timer
-    timer.start();
-    // Run controller for only 5s
-    while (timer.read () <=5.0f)
+    tim.start () ;
+    while ( current_time <=10.0f)
     {
-        // Wait 5ms (200 Hz) to perfom next estimate and control update
-        while (( timer.read () -last_time ) <= 0.005f)
+        // Update current time
+        current_time = tim.read () ;
+        // Estimate and control
+        if ( flag )
         {
+            flag = false ;
+            att_est.estimate () ;
+            att_cont.control (phi_r , theta_r ,psi_r , att_est.phi , att_est.theta , att_est.psi);
+            mixer.actuate (f_t ,0.0f, att_cont.tau_theta ,0.0f);
         }
-        // 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)
-    {
-    }
-}
+    while (1) ;
+}
\ No newline at end of file