Diff: HorizontalController/HorizontalController.cpp

Branch:

yuri

Revision:

8:1ad52489f6f3

diff -r 7a447d4ae677 -r 1ad52489f6f3 HorizontalController/HorizontalController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HorizontalController/HorizontalController.cpp	Fri Nov 30 19:23:29 2018 +0000
@@ -0,0 +1,33 @@
+#include "mbed.h"
+#include "HorizontalController.h"
+// Class constructor
+
+
+// na biblioteca
+HorizontalController :: HorizontalController ()
+{
+    
+    phi_r=0;
+    theta_r=0;
+    kp_hori = 1.35f;
+    kd_hori = 2;
+    x_e_last=0;
+    y_e_last=0;
+}
+// Control reference roll and pitch angles given reference horizontal positions and current horizontal positions and velocities
+void HorizontalController :: control ( float x_r , float y_r , float x, float y, float u, float v)
+{
+    theta_r = (1 / g)* control_state_regulator (x_r , x , x_e_last , kp_hori , kd_hori);
+    x_e_last = pos_e_last;
+    phi_r = - (1 / g)* control_state_regulator(y_r , y , y_e_last , kp_hori , kd_hori);
+    y_e_last = pos_e_last;
+}
+// Control acceleration given reference position and current position and velocity with given controller gains
+float HorizontalController :: control_state_regulator ( float pos_r , float pos , float pos_e_last, float kp,  float kd)
+{
+    float pos_e_hori = pos_r - pos;
+    float vel = (pos_e_hori - pos_e_last) / delta_hori;
+    pos_e_last = pos_e_hori;
+    float pos_e_hori_2ponto = kp * pos_e_hori + kd * vel;  
+    return pos_e_hori_2ponto;
+}
\ No newline at end of file