Yuri De Stefani
/
VerticalController
Controlador vertical
VerticalController.cpp
- Committer:
- yurindes
- Date:
- 2018-10-24
- Revision:
- 0:115db3c33311
File content as of revision 0:115db3c33311:
# 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; }