Diff: main.cpp

Revision:

1:65d0334fc83a

Parent:

0:c5109053e100

Child:

2:6ca1b4dcb9f7

--- a/main.cpp	Mon Oct 08 11:08:56 2018 +0000
+++ b/main.cpp	Tue Oct 09 18:02:04 2018 +0000
@@ -1,7 +1,5 @@
 #include "mbed.h"
-#include "AttitudeEstimator.h"
-#include "AttitudeController.h"
-#include "Mixer.h"
+#include "CrazyflieController.h"
 
 //Declare attitude estimator object
 AttitudeEstimator att_est;
@@ -11,8 +9,17 @@
 Mixer mixer;
 // Declare timer object
 Timer timer;
+
+Ticker tic;
+
+bool flag = false;
+
+void callback()
+{
+    flag = true;
+}
 // Last interrupt time
-float last_time = 0.0f;
+float current_time = 0.0f;
 
 int main()
 {
@@ -21,27 +28,31 @@
     //Initialize attitude estimator
     att_est.init();
     //Start timer
+    tic.attach(&callback,dt);
+    
     timer.start();
     //Run controller for only 5s
-    while( timer.read() <=5.0f)
+    while( current_time <=10.0f)
     {
         // Wait 5ms (200 Hz) to perfom next estimate and control update
-        while(( timer.read()-last_time ) <=0.005f)
+        current_time = timer.read();
+        
+        if(flag)
         {
+            flag = false;
+            // Estimate attitude
+            att_est.estimate();
+            // Calculate torques (N.m) given estimated attitude ( rad and rad /s)
+            att_cont.control(0.0f, 0.0f, 0.0f, att_est.phi, att_est.theta, att_est.psi, att_est.p, att_est.q, att_est.r);
+            // Actuate motors with given force and calculated torques
+            mixer.actuate(0.25f, att_cont.tau_phi, att_cont.tau_theta, 0.0f);
         }
-        // Reset timmer
-        last_time = timer.read();
-        // Estimate attitude
-        att_est.estimate();
-        // Calculate torques (N.m) given estimated attitude ( rad and rad /s)
-        att_cont.control(0.0f, 0.0f, 0.0f, att_est.phi, att_est.theta, att_est.psi, att_est.p, att_est.q, att_est.r);
-        // Actuate motors with given force and calculated torques
-        mixer.actuate(0.25f, att_cont.tau_phi, att_cont.tau_theta, att_cont.tau_psi);
+        if(att_est.phi >= pi/4.0f || att_est.theta >= pi/4.0f || att_est.p >= 4.0f*pi || att_est.q >= 4.0f*pi)
+        {
+            break;
+        }
     }
     //Turn off all motors
     mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f);
-    while (1)
-    {
-    
-    }
+    while (1);
 }
\ No newline at end of file