Revision 0:115db3c33311, committed 2018-10-24

Comitter:

yurindes

Date:

Wed Oct 24 11:58:36 2018 +0000

Commit message:

controlador vertical :)

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalController.cpp	Wed Oct 24 11:58:36 2018 +0000
@@ -0,0 +1,24 @@
+# include "mbed.h"
+# include "VerticalController.h"
+// Class constructor
+VerticalController :: VerticalController ()
+{
+    f_t=0;
+    z_e_last=0;
+    
+    float os=0.5f;
+    float t_s=2.0f;
+    float zeta=abs(log(os/100.0f))/sqrt(pow(log(os/100.0f)),2)+pow(3.1415f,2));
+    float w_n=4.0f/(t_s*zeta);
+    kp=pow(w_n, 2);
+    kd=2.0f*zeta*w_n;
+}
+// Control thrust force (N) given vertical position (m) and velocity (m/s)
+void VerticalController :: control ( float z_r , float z, float w)
+{
+    float erro_e=z_r-z;
+    float erro_e_ponto=(erro_e-z_e_last)/0.002f;
+    z_e_last=erro;
+    float z_r_2pontos=kp*erro_e+kd*erro_e_ponto;
+    f_t=z_r_2pontos*30e-3;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalController.h	Wed Oct 24 11:58:36 2018 +0000
@@ -0,0 +1,20 @@
+# ifndef VerticalController_h
+# define VerticalController_h
+# include "mbed.h"
+# include "Parameters.h"
+// Vertical controller class
+class VerticalController
+{
+    public:
+        // Class constructor
+        VerticalController () ;
+        // Control total thrust force given vertical position reference and estimation
+        void control ( float z_r , float z, float w);
+        // Thrust force (N)
+        float f_t;
+    private:
+        // Last vertical position (m) error
+        float z_e_last ;
+        float kp, kd;
+};
+# endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 24 11:58:36 2018 +0000
@@ -0,0 +1,68 @@
+# include "mbed.h"
+# include "CrazyflieController.h"
+// Crazyflie controller objects
+//dfu-util -d 0483:df11 -a 0 -s 0x08000000 -D love.bin
+AttitudeEstimator att_est ;
+AttitudeController att_cont ;
+VerticalEstimator ver_est ;
+VerticalController ver_cont;
+Mixer mixer;
+// Timer and tickers
+Timer tim;
+Ticker tic;
+Ticker tic_range;
+// Interrupt flags
+bool flag = false;
+bool flag_range = false;
+// Callback function
+void callback()
+{
+    flag = true;
+}
+// Callback function ( range correct )
+void callback_range()
+{
+    flag_range = true;
+}
+// Current time
+float current_time;
+// Main program
+int main ()
+    {
+    // Set references
+    float z_r = 0.1f;
+    float phi_r = 0.0f;
+    float theta_r = 0.0f;
+    float psi_r = 0.0f;
+    // Wait 5s for safety
+    wait (5);
+    // Initialize estimators objects
+    att_est.init();
+    ver_est.init();
+    // Initialize interrupts
+    tic.attach(&callback,dt);
+    tic_range.attach(&callback_range ,dt_range);
+    // Start timer
+    tim.start();
+    while (tim.read()<=5.0f)
+    {
+        // Estimate and control
+        if (flag)
+        {
+            flag = false;
+            att_est.estimate();
+            ver_est.predict();
+            ver_cont.control(z_r,ver_est.z,ver_est.w);
+            att_cont.control(phi_r,theta_r,psi_r,att_est.phi,att_est.theta,att_est.psi,att_est.p,att_est.q,att_est.r);
+            mixer.actuate(ver_cont.f_t,att_cont.tau_phi,att_cont.tau_theta,att_cont.tau_ps);
+        }
+        // Correct vertical estimation
+        if (flag_range)
+        {
+            flag_range = false;
+            ver_est.correct(att_est.phi,att_est.theta);
+    }
+    // Turn off all motors
+    mixer.actuate(0.0f ,0.0f ,0.0f ,0.0f);
+    while(1);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Oct 24 11:58:36 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file