Yuri De Stefani
/
VerticalController
Controlador vertical
VerticalController.cpp@0:115db3c33311, 2018-10-24 (annotated)
- Committer:
- yurindes
- Date:
- Wed Oct 24 11:58:36 2018 +0000
- Revision:
- 0:115db3c33311
controlador vertical :)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yurindes | 0:115db3c33311 | 1 | # include "mbed.h" |
yurindes | 0:115db3c33311 | 2 | # include "VerticalController.h" |
yurindes | 0:115db3c33311 | 3 | // Class constructor |
yurindes | 0:115db3c33311 | 4 | VerticalController :: VerticalController () |
yurindes | 0:115db3c33311 | 5 | { |
yurindes | 0:115db3c33311 | 6 | f_t=0; |
yurindes | 0:115db3c33311 | 7 | z_e_last=0; |
yurindes | 0:115db3c33311 | 8 | |
yurindes | 0:115db3c33311 | 9 | float os=0.5f; |
yurindes | 0:115db3c33311 | 10 | float t_s=2.0f; |
yurindes | 0:115db3c33311 | 11 | float zeta=abs(log(os/100.0f))/sqrt(pow(log(os/100.0f)),2)+pow(3.1415f,2)); |
yurindes | 0:115db3c33311 | 12 | float w_n=4.0f/(t_s*zeta); |
yurindes | 0:115db3c33311 | 13 | kp=pow(w_n, 2); |
yurindes | 0:115db3c33311 | 14 | kd=2.0f*zeta*w_n; |
yurindes | 0:115db3c33311 | 15 | } |
yurindes | 0:115db3c33311 | 16 | // Control thrust force (N) given vertical position (m) and velocity (m/s) |
yurindes | 0:115db3c33311 | 17 | void VerticalController :: control ( float z_r , float z, float w) |
yurindes | 0:115db3c33311 | 18 | { |
yurindes | 0:115db3c33311 | 19 | float erro_e=z_r-z; |
yurindes | 0:115db3c33311 | 20 | float erro_e_ponto=(erro_e-z_e_last)/0.002f; |
yurindes | 0:115db3c33311 | 21 | z_e_last=erro; |
yurindes | 0:115db3c33311 | 22 | float z_r_2pontos=kp*erro_e+kd*erro_e_ponto; |
yurindes | 0:115db3c33311 | 23 | f_t=z_r_2pontos*30e-3; |
yurindes | 0:115db3c33311 | 24 | } |