Diff: main.cpp

Revision:

0:c5109053e100

Child:

1:65d0334fc83a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 08 11:08:56 2018 +0000
@@ -0,0 +1,47 @@
+#include "mbed.h"
+#include "AttitudeEstimator.h"
+#include "AttitudeController.h"
+#include "Mixer.h"
+
+//Declare attitude estimator object
+AttitudeEstimator att_est;
+//Declare attitude controller object
+AttitudeController att_cont;
+// 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
+    att_est.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
+        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);
+    }
+    //Turn off all motors
+    mixer.actuate(0.0f, 0.0f, 0.0f, 0.0f);
+    while (1)
+    {
+    
+    }
+}
\ No newline at end of file