programa final
HorizontalController/HorizontalController.cpp@8:1ad52489f6f3, 2018-11-30 (annotated)
- Committer:
- yurindes
- Date:
- Fri Nov 30 19:23:29 2018 +0000
- Branch:
- yuri
- Revision:
- 8:1ad52489f6f3
final;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yurindes | 8:1ad52489f6f3 | 1 | #include "mbed.h" |
yurindes | 8:1ad52489f6f3 | 2 | #include "HorizontalController.h" |
yurindes | 8:1ad52489f6f3 | 3 | // Class constructor |
yurindes | 8:1ad52489f6f3 | 4 | |
yurindes | 8:1ad52489f6f3 | 5 | |
yurindes | 8:1ad52489f6f3 | 6 | // na biblioteca |
yurindes | 8:1ad52489f6f3 | 7 | HorizontalController :: HorizontalController () |
yurindes | 8:1ad52489f6f3 | 8 | { |
yurindes | 8:1ad52489f6f3 | 9 | |
yurindes | 8:1ad52489f6f3 | 10 | phi_r=0; |
yurindes | 8:1ad52489f6f3 | 11 | theta_r=0; |
yurindes | 8:1ad52489f6f3 | 12 | kp_hori = 1.35f; |
yurindes | 8:1ad52489f6f3 | 13 | kd_hori = 2; |
yurindes | 8:1ad52489f6f3 | 14 | x_e_last=0; |
yurindes | 8:1ad52489f6f3 | 15 | y_e_last=0; |
yurindes | 8:1ad52489f6f3 | 16 | } |
yurindes | 8:1ad52489f6f3 | 17 | // Control reference roll and pitch angles given reference horizontal positions and current horizontal positions and velocities |
yurindes | 8:1ad52489f6f3 | 18 | void HorizontalController :: control ( float x_r , float y_r , float x, float y, float u, float v) |
yurindes | 8:1ad52489f6f3 | 19 | { |
yurindes | 8:1ad52489f6f3 | 20 | theta_r = (1 / g)* control_state_regulator (x_r , x , x_e_last , kp_hori , kd_hori); |
yurindes | 8:1ad52489f6f3 | 21 | x_e_last = pos_e_last; |
yurindes | 8:1ad52489f6f3 | 22 | phi_r = - (1 / g)* control_state_regulator(y_r , y , y_e_last , kp_hori , kd_hori); |
yurindes | 8:1ad52489f6f3 | 23 | y_e_last = pos_e_last; |
yurindes | 8:1ad52489f6f3 | 24 | } |
yurindes | 8:1ad52489f6f3 | 25 | // Control acceleration given reference position and current position and velocity with given controller gains |
yurindes | 8:1ad52489f6f3 | 26 | float HorizontalController :: control_state_regulator ( float pos_r , float pos , float pos_e_last, float kp, float kd) |
yurindes | 8:1ad52489f6f3 | 27 | { |
yurindes | 8:1ad52489f6f3 | 28 | float pos_e_hori = pos_r - pos; |
yurindes | 8:1ad52489f6f3 | 29 | float vel = (pos_e_hori - pos_e_last) / delta_hori; |
yurindes | 8:1ad52489f6f3 | 30 | pos_e_last = pos_e_hori; |
yurindes | 8:1ad52489f6f3 | 31 | float pos_e_hori_2ponto = kp * pos_e_hori + kd * vel; |
yurindes | 8:1ad52489f6f3 | 32 | return pos_e_hori_2ponto; |
yurindes | 8:1ad52489f6f3 | 33 | } |