Yuri De Stefani
/
VerticalController
Controlador vertical
Diff: VerticalController.cpp
- Revision:
- 0:115db3c33311
--- /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