controlador de atitude
AttitudeController/AttitudeController.cpp@8:c96125e9ac70, 2018-11-21 (annotated)
- Committer:
- yurindes
- Date:
- Wed Nov 21 10:07:00 2018 +0000
- Branch:
- yuri
- Revision:
- 8:c96125e9ac70
- Parent:
- 2:54fabc943f25
teste controlador atitude;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yvesyuzo | 0:3871dc7bedf7 | 1 | #include "mbed.h" |
yvesyuzo | 0:3871dc7bedf7 | 2 | #include "AttitudeController.h" |
yvesyuzo | 0:3871dc7bedf7 | 3 | #include "Library.h" |
yvesyuzo | 0:3871dc7bedf7 | 4 | |
yvesyuzo | 0:3871dc7bedf7 | 5 | // Class constructor |
yvesyuzo | 0:3871dc7bedf7 | 6 | AttitudeController :: AttitudeController () |
yvesyuzo | 0:3871dc7bedf7 | 7 | { |
yvesyuzo | 0:3871dc7bedf7 | 8 | } |
yvesyuzo | 0:3871dc7bedf7 | 9 | // Control torques given reference angles and current angles and angular velocities |
yvesyuzo | 0:3871dc7bedf7 | 10 | void AttitudeController::control ( float phi_r, float theta_r, float psi_r, float phi, float theta, float psi) |
yvesyuzo | 0:3871dc7bedf7 | 11 | { |
yvesyuzo | 2:54fabc943f25 | 12 | tau_phi = single_axis_control ( phi_r , phi , Kp_phi , Td_phi , I_xx, last_erro_phi); |
yurindes | 8:c96125e9ac70 | 13 | |
yvesyuzo | 2:54fabc943f25 | 14 | last_erro_phi = last_erro; |
yurindes | 8:c96125e9ac70 | 15 | |
yurindes | 8:c96125e9ac70 | 16 | tau_theta = single_axis_control ( theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta); |
yurindes | 8:c96125e9ac70 | 17 | |
yvesyuzo | 2:54fabc943f25 | 18 | last_erro_theta = last_erro; |
yurindes | 8:c96125e9ac70 | 19 | |
yurindes | 8:c96125e9ac70 | 20 | tau_psi = single_axis_control ( psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi); |
yurindes | 8:c96125e9ac70 | 21 | |
yvesyuzo | 2:54fabc943f25 | 22 | last_erro_psi = last_erro; |
yvesyuzo | 0:3871dc7bedf7 | 23 | } |
yvesyuzo | 0:3871dc7bedf7 | 24 | // Control torque of a single axis given reference angles and current angles and angular velocities ( with given gains and /or time constants and moments of inertia ) |
yvesyuzo | 0:3871dc7bedf7 | 25 | float AttitudeController :: single_axis_control ( float angle_r , float angle , float K_angle , float K_rate , float I, float last_erro_angle) |
yvesyuzo | 0:3871dc7bedf7 | 26 | { |
yvesyuzo | 2:54fabc943f25 | 27 | float erro = angle_r - angle; |
yurindes | 8:c96125e9ac70 | 28 | float erro_p = (erro - last_erro_angle)/dt_alti; |
yvesyuzo | 2:54fabc943f25 | 29 | last_erro = erro; |
yvesyuzo | 0:3871dc7bedf7 | 30 | float angle_2p = K_angle*(erro+1/K_rate*erro_p); |
yvesyuzo | 0:3871dc7bedf7 | 31 | return I*angle_2p; |
yvesyuzo | 0:3871dc7bedf7 | 32 | } |