Revision 8:c96125e9ac70, committed 2018-11-21

Comitter:

yurindes

Date:

Wed Nov 21 10:07:00 2018 +0000

Branch:

yuri

Parent:

6:7a447d4ae677

Commit message:

teste controlador atitude;

Changed in this revision

diff -r 7a447d4ae677 -r c96125e9ac70 AttitudeController/AttitudeController.cpp
--- a/AttitudeController/AttitudeController.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/AttitudeController/AttitudeController.cpp	Wed Nov 21 10:07:00 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 c96125e9ac70 AttitudeController/AttitudeController.h
--- a/AttitudeController/AttitudeController.h	Wed Oct 17 12:18:29 2018 +0000
+++ b/AttitudeController/AttitudeController.h	Wed Nov 21 10:07:00 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 c96125e9ac70 AttitudeEstimator/AttitudeEstimator.cpp
--- a/AttitudeEstimator/AttitudeEstimator.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Wed Nov 21 10:07:00 2018 +0000
@@ -26,14 +26,17 @@
     q_bias = 0.0f;
     r_bias = 0.0f;
 
-    for(count = 1; count <= taxa; ++count) 
+    for(count = 1; count <= taxa; count++) 
     {
         imu.read();
-        p_bias += imu.gx/200.0f;
-        q_bias += imu.gy/200.0f;
-        r_bias += imu.gz/200.0f;
+        p_bias += imu.gx;
+        q_bias += imu.gy;
+        r_bias += imu.gz;
         wait(0.005);
     }
+    p_bias = p_bias/200.0f;
+    q_bias = q_bias/200.0f;
+    r_bias = r_bias/200.0f;
 }
 
 // Estimate Euler angles (rad ) from accelerometer data
@@ -92,10 +95,7 @@
     imu.read();
     estimate_accel();
     estimate_gyro();
-    phi = rho*phi_accel + (1-rho)*phi_gyro;
-    theta = rho*theta_accel + (1-rho)*theta_gyro;
+    phi = rho*phi_accel + (1.0f-rho)*phi_gyro;
+    theta = rho*theta_accel + (1.0f-rho)*theta_gyro;
     psi = psi_gyro;
-}
-
-
-
+}
\ No newline at end of file

diff -r 7a447d4ae677 -r c96125e9ac70 Library/Library.h
--- a/Library/Library.h	Wed Oct 17 12:18:29 2018 +0000
+++ b/Library/Library.h	Wed Nov 21 10:07:00 2018 +0000
@@ -1,26 +1,43 @@
 #ifndef Library_h
 #define Library_h
+#include <math.h>
+
+ float const pi = 3.14159265f;
 //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 Ts = 2.0f;//0.3f;
+float const OS = 0.5f;
+
+float const zeta = sqrt(log(OS/100.0f)*log(OS/100.0f))/(sqrt(log(OS/100.0f)*log(OS/100.0f)+pi*pi));
+float const omega_n = 4.0f/(Ts*zeta);
+
+float const Kp_phi = omega_n*omega_n;
 float const Kp_theta = Kp_phi;
-float const Td_phi = 1.96;
+float const Td_phi = 1/(zeta*2*Kp_phi*sqrt(Kp_phi));
 float const Td_theta = Td_phi;
-float const Kp_psi = 2.57*0.81;
-float const Td_psi = 0.81;
+
+float const Ts2 = 0.6f;
+float const OS2= 0.5f;
+
+float const zeta2 = sqrt(log(OS2/100.0f)*log(OS2/100.0f))/(sqrt(log(OS2/100.0f)*log(OS2/100.0f)+3.1415f*3.1415f));
+float const omega_n2 = 4.0f/(Ts2*zeta2);
+
+float const Kp_psi = omega_n2*omega_n2;
+float const Td_psi = 4.0f/(Ts2*zeta2);
 // 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;
 // Attitude Estimator constants
- float const pi = 3.14159265f;
- float const dt = 0.005f;
- float const rho = 0.05f;
+ float const dt = 0.002f;
+ float const rho = 0.01f;
  // Mixer
 const float alpha = 1.081E-7;
 const float beta = 2.678E-11;
 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;
 # endif

diff -r 7a447d4ae677 -r c96125e9ac70 Mixer/Mixer.cpp
--- a/Mixer/Mixer.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ b/Mixer/Mixer.cpp	Wed Nov 21 10:07:00 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 c96125e9ac70 VerticalEstimator/VerticalEstimator.cpp
--- a/VerticalEstimator/VerticalEstimator.cpp	Wed Oct 17 12:18:29 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,31 +0,0 @@
-#include "mbed.h"
-#include "VerticalEstimator.h"
-// Class constructor hhh
-VerticalEstimator :: VerticalEstimator() : range (PB_7 , PB_6 )
-{
-    z=0;
-    w=0;
-    z_m_last=0;
-}
-// Initialize class
-void VerticalEstimator :: init()
-{
-    range.init();
-}
-// Predict vertical position and velocity from model
-void VerticalEstimator :: predict()
-{
-    z=z+0.05f*w;
-    w=w;
-}
-// Correct vertical position and velocity with measurement
-void VerticalEstimator :: correct ( float phi , float theta )
-{
-    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;
-}

diff -r 7a447d4ae677 -r c96125e9ac70 VerticalEstimator/VerticalEstimator.h
--- a/VerticalEstimator/VerticalEstimator.h	Wed Oct 17 12:18:29 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,27 +0,0 @@
-# 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 ;
-};
-# endif