HorizontalController.cpp

00001 #include "mbed.h"
00002 #include "HorizontalController.h"
00003 // Class constructor
00004 
00005 
00006 // na biblioteca
00007 HorizontalController :: HorizontalController ()
00008 {
00009     
00010     phi_r=0;
00011     theta_r=0;
00012     kp_hori = 1.35f;
00013     kd_hori = 2;
00014     x_e_last=0;
00015     y_e_last=0;
00016 }
00017 // Control reference roll and pitch angles given reference horizontal positions and current horizontal positions and velocities
00018 void HorizontalController :: control ( float x_r , float y_r , float x, float y, float u, float v)
00019 {
00020     theta_r = (1 / g)* control_state_regulator (x_r , x , x_e_last , kp_hori , kd_hori);
00021     x_e_last = pos_e_last;
00022     phi_r = - (1 / g)* control_state_regulator(y_r , y , y_e_last , kp_hori , kd_hori);
00023     y_e_last = pos_e_last;
00024 }
00025 // Control acceleration given reference position and current position and velocity with given controller gains
00026 float HorizontalController :: control_state_regulator ( float pos_r , float pos , float pos_e_last, float kp,  float kd)
00027 {
00028     float pos_e_hori = pos_r - pos;
00029     float vel = (pos_e_hori - pos_e_last) / delta_hori;
00030     pos_e_last = pos_e_hori;
00031     float pos_e_hori_2ponto = kp * pos_e_hori + kd * vel;  
00032     return pos_e_hori_2ponto;
00033 }