Revision 8:1ad52489f6f3, committed 2018-11-30

Comitter:

yurindes

Date:

Fri Nov 30 19:23:29 2018 +0000

Branch:

yuri

Parent:

6:7a447d4ae677

Commit message:

final;

Changed in this revision

diff -r 7a447d4ae677 -r 1ad52489f6f3 AttitudeController/AttitudeController.cpp
--- a/AttitudeController/AttitudeController.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/AttitudeController/AttitudeController.cpp	Fri Nov 30 19:23:29 2018 +0000
@@ -2,7 +2,6 @@
 #include "AttitudeController.h"
 #include "Library.h"
 
-
 // Class constructor
 AttitudeController :: AttitudeController ()
 {
@@ -11,22 +10,22 @@
 void AttitudeController::control ( float phi_r, float theta_r, float psi_r, float phi, float theta, float psi)
 {
     tau_phi = single_axis_control ( phi_r , phi , Kp_phi , Td_phi , I_xx, last_erro_phi);
-    
+
     last_erro_phi = last_erro;
-    
-    tau_theta = single_axis_control (  psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi);
-    
+
+    tau_theta = single_axis_control (  theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta);
+
     last_erro_theta = last_erro;
-    
-    tau_psi = single_axis_control ( theta_r , theta , Kp_theta , Td_theta , I_yy, last_erro_theta);
-    
+
+    tau_psi = single_axis_control ( psi_r , psi , Kp_psi , Td_psi , I_zz, last_erro_psi);
+
     last_erro_psi = last_erro;
 }
 // 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 )
 float AttitudeController :: single_axis_control ( float angle_r , float angle , float K_angle , float K_rate , float I, float last_erro_angle)
 {
     float erro = angle_r - angle;
-    float erro_p = (erro - last_erro_angle)/dt2;
+    float erro_p = (erro - last_erro_angle)/dt_alti;
     last_erro = erro;
     float angle_2p = K_angle*(erro+1/K_rate*erro_p);
     return I*angle_2p;

diff -r 7a447d4ae677 -r 1ad52489f6f3 AttitudeController/AttitudeController.h
--- a/AttitudeController/AttitudeController.h	Wed Oct 17 12:18:29 2018 +0000
+++ b/AttitudeController/AttitudeController.h	Fri Nov 30 19:23:29 2018 +0000
@@ -4,24 +4,6 @@
 #include "mbed.h"
 #include "Library.h"
 
-/*
-float const dt2 = 0.005f;
-// Controller gains and /or time constants
-float const Kp_phi = 3.63*1.96;
-float const Kp_theta = Kp_phi;
-float const Td_phi = 1.96;
-float const Td_theta = Td_phi;
-
-float const Kp_psi = 2.57*0.81;
-float const Td_psi = 0.81;
-
-
-// Quadcopter moments of inertia (kg.m ^2)
-float const I_xx = 16.0e-6f;
-float const I_yy = 16.0e-6f;
-float const I_zz = 29.0e-6f;
-*/
-
 class AttitudeController
 {
     public :

diff -r 7a447d4ae677 -r 1ad52489f6f3 AttitudeEstimator/AttitudeEstimator.cpp
--- a/AttitudeEstimator/AttitudeEstimator.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Fri Nov 30 19:23:29 2018 +0000
@@ -95,7 +95,4 @@
     phi = rho*phi_accel + (1-rho)*phi_gyro;
     theta = rho*theta_accel + (1-rho)*theta_gyro;
     psi = psi_gyro;
-}
-
-
-
+}
\ No newline at end of file

diff -r 7a447d4ae677 -r 1ad52489f6f3 CrazyflieController.h
--- a/CrazyflieController.h	Wed Oct 17 12:18:29 2018 +0000
+++ b/CrazyflieController.h	Fri Nov 30 19:23:29 2018 +0000
@@ -5,5 +5,6 @@
 #include "AttitudeController.h"
 #include "Mixer.h"
 #include "Library.h"
-
+#include "VerticalEstimator.h"
+#include "VerticalController.h"
 #endif

diff -r 7a447d4ae677 -r 1ad52489f6f3 HorizontalController/HorizontalController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HorizontalController/HorizontalController.cpp	Fri Nov 30 19:23:29 2018 +0000
@@ -0,0 +1,33 @@
+#include "mbed.h"
+#include "HorizontalController.h"
+// Class constructor
+
+
+// na biblioteca
+HorizontalController :: HorizontalController ()
+{
+    
+    phi_r=0;
+    theta_r=0;
+    kp_hori = 1.35f;
+    kd_hori = 2;
+    x_e_last=0;
+    y_e_last=0;
+}
+// Control reference roll and pitch angles given reference horizontal positions and current horizontal positions and velocities
+void HorizontalController :: control ( float x_r , float y_r , float x, float y, float u, float v)
+{
+    theta_r = (1 / g)* control_state_regulator (x_r , x , x_e_last , kp_hori , kd_hori);
+    x_e_last = pos_e_last;
+    phi_r = - (1 / g)* control_state_regulator(y_r , y , y_e_last , kp_hori , kd_hori);
+    y_e_last = pos_e_last;
+}
+// Control acceleration given reference position and current position and velocity with given controller gains
+float HorizontalController :: control_state_regulator ( float pos_r , float pos , float pos_e_last, float kp,  float kd)
+{
+    float pos_e_hori = pos_r - pos;
+    float vel = (pos_e_hori - pos_e_last) / delta_hori;
+    pos_e_last = pos_e_hori;
+    float pos_e_hori_2ponto = kp * pos_e_hori + kd * vel;  
+    return pos_e_hori_2ponto;
+}
\ No newline at end of file

diff -r 7a447d4ae677 -r 1ad52489f6f3 HorizontalController/HorizontalController.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HorizontalController/HorizontalController.h	Fri Nov 30 19:23:29 2018 +0000
@@ -0,0 +1,22 @@
+#ifndef HorizontalController_h
+#define HorizontalController_h
+#include "mbed.h"
+#include "Library.h"
+// Horizontal controller class
+class HorizontalController
+{
+    public :
+        // Class constructor
+        HorizontalController () ;
+        // Control reference roll and pitch angles given reference horizontal positions and current horizontal positions and velocities
+        void control ( float x_r , float y_r , float x, float y, float u, float v);
+        // Reference roll and pitch angles ( rad)
+        float phi_r , theta_r ;
+    private :
+        // Control acceleration given reference position and current position and velocity with given controller gains
+        float control_single ( float pos_r , float pos , float vel , float kp_hori , float kd_hori);
+        // Last horizontal positions error (m)
+        float x_e_last , y_e_last ;
+        float pos_e_last;
+};
+# endif
\ No newline at end of file

diff -r 7a447d4ae677 -r 1ad52489f6f3 Library/Library.h
--- a/Library/Library.h	Wed Oct 17 12:18:29 2018 +0000
+++ b/Library/Library.h	Fri Nov 30 19:23:29 2018 +0000
@@ -1,14 +1,14 @@
 #ifndef Library_h
 #define Library_h
 //Attitude Controller VARIABLES
-float const dt2 = 0.005f;
+float const dt_alti = 0.005f;
 // Controller gains and /or time constants
-float const Kp_phi = 3.63*1.96;
+float const Kp_phi = 200;
 float const Kp_theta = Kp_phi;
-float const Td_phi = 1.96;
+float const Td_phi = 30;
 float const Td_theta = Td_phi;
-float const Kp_psi = 2.57*0.81;
-float const Td_psi = 0.81;
+float const Kp_psi = 2.57*0.81*2.5*10;
+float const Td_psi = 0.81*10*1.2;
 // Quadcopter moments of inertia (kg.m ^2)
 float const I_xx = 16.0e-6f;
 float const I_yy = 16.0e-6f;
@@ -23,4 +23,16 @@
 const float kl = 2.69E-8;
 const float kd = 1.59E-10;
 const float l = 33E-3;
+const float m = 0.03f;
+const float g = 9.81f;
+//verrtical
+const float p_verti = 0.3f;
+const float dt_ver_pre=0.002f;
+const float dt_ver_cor=0.05f;
+const float dt_range=0.02f;
+
+//horizontal
+float kp_hori = 1.35f;
+float kd_hori = 2;
+
 # endif

diff -r 7a447d4ae677 -r 1ad52489f6f3 Mixer/Mixer.cpp
--- a/Mixer/Mixer.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/Mixer/Mixer.cpp	Fri Nov 30 19:23:29 2018 +0000
@@ -19,7 +19,7 @@
     float const beta = 2.678e-11;
     
     //calcula as velocidades angulares conforme a relacao entre toque e forca e velocidade angular
-    w1 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) - ((1/(4*kl))* tau_theta) - ((1/(4*kd))* tau_psi)  ;
+    w1 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) - ((1/(4*kl)) * tau_theta) - ((1/(4*kd))* tau_psi)  ;
     w2 = ((1/(4*kl))* f_t) - ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
     w3 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) + ((1/(4*kl*l))* tau_theta) - ((1/(4*kd))* tau_psi);
     w4 = ((1/(4*kl))* f_t) + ((1/(4*kl*l))* tau_phi) - ((1/(4*kl*l))* tau_theta) + ((1/(4*kd))* tau_psi);
@@ -50,10 +50,4 @@
     motor [3] = alpha *pow(w4 ,2) + beta * w4;
     
 
-}
-
-
-
-
-
-
+}
\ No newline at end of file

diff -r 7a447d4ae677 -r 1ad52489f6f3 VerticalController/VerticalController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalController/VerticalController.cpp	Fri Nov 30 19:23:29 2018 +0000
@@ -0,0 +1,24 @@
+# include "mbed.h"
+# include "VerticalController.h"
+// Class constructor
+VerticalController :: VerticalController ()
+{
+    f_t=0;
+    z_e_last=0;
+    
+    float os=0.5f;
+    float t_s=2.0f;
+    float zeta=sqrt(log(os/100.0f)*log(os/100.0f))/sqrt(log(os/100.0f)*log(os/100.0f)+3.1415f*3.1415f);
+    float w_n=4.0f/(t_s*zeta);
+    kp=pow(w_n, 2);
+    kd=2.0f*zeta*w_n;
+}
+// Control thrust force (N) given vertical position (m) and velocity (m/s)
+void VerticalController :: control ( float z_r , float z, float w)
+{
+    float erro_e=z_r-z;
+    float erro_e_ponto=(erro_e-z_e_last)/0.002f;
+    z_e_last=erro_e;
+    float z_r_2pontos=kp*erro_e+kd*erro_e_ponto;
+    f_t=z_r_2pontos*0.03f;
+}
\ No newline at end of file

diff -r 7a447d4ae677 -r 1ad52489f6f3 VerticalController/VerticalController.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VerticalController/VerticalController.h	Fri Nov 30 19:23:29 2018 +0000
@@ -0,0 +1,19 @@
+# ifndef VerticalController_h
+# define VerticalController_h
+# include "mbed.h"
+// Vertical controller class
+class VerticalController
+{
+    public:
+        // Class constructor
+        VerticalController () ;
+        // Control total thrust force given vertical position reference and estimation
+        void control ( float z_r , float z, float w);
+        // Thrust force (N)
+        float f_t;
+    private:
+        // Last vertical position (m) error
+        float z_e_last ;
+        float kp, kd;
+};
+# endif
\ No newline at end of file

diff -r 7a447d4ae677 -r 1ad52489f6f3 VerticalEstimator/VerticalEstimator.cpp
--- a/VerticalEstimator/VerticalEstimator.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.cpp	Fri Nov 30 19:23:29 2018 +0000
@@ -1,31 +1,49 @@
 #include "mbed.h"
 #include "VerticalEstimator.h"
-// Class constructor hhh
-VerticalEstimator :: VerticalEstimator() : range (PB_7 , PB_6 )
+//#include <math.h>
+#include "Library.h"
+
+// Class constructor
+VerticalEstimator::VerticalEstimator():range (PB_7 , PB_6 )
+{
+}
+
+// Initialize class
+void VerticalEstimator::init ()
 {
-    z=0;
-    w=0;
-    z_m_last=0;
-}
-// Initialize class
-void VerticalEstimator :: init()
-{
-    range.init();
+    VL53L0X range (PB_7 , PB_6 );
+    range.init() ;
+    
+    z = 0.0f;//leitura da distancia vertical para função predict
+    zm = 0.0f;//definido uma leitura para distancia vertical
+    zml = 0.0f;//definindo uma segunda leitura para calcular a velocidade vertical
+    z_est = 0.0f;
+    
+    w = 0.0f;//definido a velocidade vertical para função predict
+    wm = 0.0f;
+    w_est = 0.0f;  
+    
 }
 // Predict vertical position and velocity from model
-void VerticalEstimator :: predict()
+void VerticalEstimator::predict ()
 {
-    z=z+0.05f*w;
-    w=w;
+    range.read();
+    z = range.d;
+    z = z + w*dt_ver_pre;//velocidade prevista, como a leitura é feita com 500Hz 0.002f sao os 2ms equivalentes
 }
+
 // Correct vertical position and velocity with measurement
-void VerticalEstimator :: correct ( float phi , float theta )
+void VerticalEstimator::correct ( float phi , float theta )//phi e theta são os angulos de euler estimados
 {
-    p=0.5f;
-    range.read;
-    float z_m = range.z;
-    float w_m = (z-z_m)/0.05f;
-    z=(1-p)*z+p*z_m;
-    w=(1-p)*w+p*w_m;
-    z_m_last = z_m;
-}
+
+    zm = range.d;
+    zm = zm*cos(phi)*cos(theta);//zm é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
+    wait(dt_ver_cor);
+    zml = range.d;
+    zm = zml*cos(phi)*cos(theta);//d é a distancia medida e phi e theta sao os angulos de Euler para correcao da medida
+    wm = (zm-zml)/dt_ver_cor;
+
+    z_est = (1-p_verti)*z + p_verti*zm;
+    w_est = (1-p_verti)*w + p_verti*wm;
+
+}
\ No newline at end of file

diff -r 7a447d4ae677 -r 1ad52489f6f3 VerticalEstimator/VerticalEstimator.h
--- a/VerticalEstimator/VerticalEstimator.h	Wed Oct 17 12:18:29 2018 +0000
+++ b/VerticalEstimator/VerticalEstimator.h	Fri Nov 30 19:23:29 2018 +0000
@@ -1,27 +1,31 @@
-# ifndef VerticalEstimator_h
+#ifndef VerticalEstimator_h
 #define VerticalEstimator_h
+
 #include "mbed.h"
 #include "Library.h"
 #include "VL53L0X.h"
+
+
 // Vertical estimator class
 class VerticalEstimator
 {
-    public:
-        
-        // Class constructor
-        VerticalEstimator () ;
-        // Initialize class
-        void init () ;
-        // Predict vertical position and velocity from model
-        void predict () ;
-        // Correct vertical position and velocity with measurement
-        void correct ( float phi , float theta );
-        // Vertical position (m) and velocity (m/s) estimation
-        float z, w;
-    private:
-        // Range sensor object
-        VL53L0X range ;
-        // Last vertical position (m) measurement
-        float z_m_last ;
+public :
+//Class constructor
+    VerticalEstimator () ;
+// Initialize class
+    void init () ;
+// Predict vertical position and velocity from model
+    void predict () ;
+// Correct vertical position and velocity with measurement
+    void correct (float phi, float theta);
+// Vertical position (m) and velocity (m/s) estimation
+    float w_est, z_est;
+
+private :
+// Range sensor object
+    VL53L0X range ;
+// Last vertical position (m) measurement
+    float z, w, zml, wm, zm;
 };
+
 # endif