Diff: VerticalController/VerticalController.cpp

Branch:

yuri

Revision:

8:1ad52489f6f3

diff -r 7a447d4ae677 -r 1ad52489f6f3 VerticalController/VerticalController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalController/VerticalController.cpp	Fri Nov 30 19:23:29 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=sqrt(log(os/100.0f)*log(os/100.0f))/sqrt(log(os/100.0f)*log(os/100.0f)+3.1415f*3.1415f);
+    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_e;
+    float z_r_2pontos=kp*erro_e+kd*erro_e_ponto;
+    f_t=z_r_2pontos*0.03f;
+}
\ No newline at end of file