Yuri De Stefani / CrazyflieController_final

programa final

AttitudeController/AttitudeController.h@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 #ifndef AttitudeController_h
yvesyuzo 0:3871dc7bedf7 2 #define AttitudeController_h
yvesyuzo 0:3871dc7bedf7 3
yvesyuzo 0:3871dc7bedf7 4 #include "mbed.h"
yvesyuzo 0:3871dc7bedf7 5 #include "Library.h"
yvesyuzo 0:3871dc7bedf7 6
yvesyuzo 0:3871dc7bedf7 7 /*
yvesyuzo 0:3871dc7bedf7 8 float const dt2 = 0.005f;
yvesyuzo 0:3871dc7bedf7 9 // Controller gains and /or time constants
yvesyuzo 0:3871dc7bedf7 10 float const Kp_phi = 3.63*1.96;
yvesyuzo 0:3871dc7bedf7 11 float const Kp_theta = Kp_phi;
yvesyuzo 0:3871dc7bedf7 12 float const Td_phi = 1.96;
yvesyuzo 0:3871dc7bedf7 13 float const Td_theta = Td_phi;
yvesyuzo 0:3871dc7bedf7 14
yvesyuzo 0:3871dc7bedf7 15 float const Kp_psi = 2.57*0.81;
yvesyuzo 0:3871dc7bedf7 16 float const Td_psi = 0.81;
yvesyuzo 0:3871dc7bedf7 17
yvesyuzo 0:3871dc7bedf7 18
yvesyuzo 0:3871dc7bedf7 19 // Quadcopter moments of inertia (kg.m ^2)
yvesyuzo 0:3871dc7bedf7 20 float const I_xx = 16.0e-6f;
yvesyuzo 0:3871dc7bedf7 21 float const I_yy = 16.0e-6f;
yvesyuzo 0:3871dc7bedf7 22 float const I_zz = 29.0e-6f;
yvesyuzo 0:3871dc7bedf7 23 */
yvesyuzo 0:3871dc7bedf7 24
yvesyuzo 0:3871dc7bedf7 25 class AttitudeController
yvesyuzo 0:3871dc7bedf7 26 {
yvesyuzo 0:3871dc7bedf7 27 public :
yvesyuzo 0:3871dc7bedf7 28 // Class constructor
yvesyuzo 0:3871dc7bedf7 29 AttitudeController () ;
yvesyuzo 0:3871dc7bedf7 30 // Control torques given reference angles and current angles and angular velocities
yvesyuzo 0:3871dc7bedf7 31 void control(float phi_r, float theta_r, float psi_r, float phi, float theta, float psi);
yvesyuzo 0:3871dc7bedf7 32 // Torques (N.m)
yvesyuzo 0:3871dc7bedf7 33 float tau_phi, tau_theta, tau_psi;
yvesyuzo 2:54fabc943f25 34
yvesyuzo 0:3871dc7bedf7 35 private :
yvesyuzo 0:3871dc7bedf7 36 // 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 37 float single_axis_control(float angle_r, float angle, float K_angle, float K_rate , float I, float last_erro_angle);
yvesyuzo 2:54fabc943f25 38
yvesyuzo 0:3871dc7bedf7 39 float last_erro_phi, last_erro_theta, last_erro_psi;
yvesyuzo 2:54fabc943f25 40
yvesyuzo 0:3871dc7bedf7 41 float last_erro;
yvesyuzo 0:3871dc7bedf7 42 };
yvesyuzo 0:3871dc7bedf7 43 # endif
yvesyuzo 0:3871dc7bedf7 44