Yuri De Stefani / CrazyflieController_final

programa final

AttitudeController/AttitudeController.cpp@2:54fabc943f25, 2018-10-16 (annotated)

Committer:
yvesyuzo
Date:
Tue Oct 16 21:57:19 2018 +0000
Revision:
2:54fabc943f25
Parent:
0:3871dc7bedf7
Child:
8:1ad52489f6f3
n

Who changed what in which revision?

UserRevisionLine numberNew 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
yvesyuzo 0:3871dc7bedf7 6 // Class constructor
yvesyuzo 0:3871dc7bedf7 7 AttitudeController :: AttitudeController ()
yvesyuzo 0:3871dc7bedf7 8 {
yvesyuzo 0:3871dc7bedf7 9 }
yvesyuzo 0:3871dc7bedf7 10 // Control torques given reference angles and current angles and angular velocities
yvesyuzo 0:3871dc7bedf7 11 void AttitudeController::control ( float phi_r, float theta_r, float psi_r, float phi, float theta, float psi)
yvesyuzo 0:3871dc7bedf7 12 {
yvesyuzo 2:54fabc943f25 13 tau_phi = single_axis_control ( phi_r , phi , Kp_phi , Td_phi , I_xx, last_erro_phi);
yvesyuzo 2:54fabc943f25 14
yvesyuzo 2:54fabc943f25 15 last_erro_phi = last_erro;
yvesyuzo 2:54fabc943f25 16
yvesyuzo 2:54fabc943f25 17 tau_theta = single_axis_control ( psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi);
yvesyuzo 2:54fabc943f25 18
yvesyuzo 2:54fabc943f25 19 last_erro_theta = last_erro;
yvesyuzo 2:54fabc943f25 20
yvesyuzo 2:54fabc943f25 21 tau_psi = single_axis_control ( theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta);
yvesyuzo 2:54fabc943f25 22
yvesyuzo 2:54fabc943f25 23 last_erro_psi = last_erro;
yvesyuzo 0:3871dc7bedf7 24 }
yvesyuzo 0:3871dc7bedf7 25 // 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 26 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 27 {
yvesyuzo 2:54fabc943f25 28 float erro = angle_r - angle;
yvesyuzo 2:54fabc943f25 29 float erro_p = (erro - last_erro_angle)/dt2;
yvesyuzo 2:54fabc943f25 30 last_erro = erro;
yvesyuzo 0:3871dc7bedf7 31 float angle_2p = K_angle*(erro+1/K_rate*erro_p);
yvesyuzo 0:3871dc7bedf7 32 return I*angle_2p;
yvesyuzo 0:3871dc7bedf7 33 }