Yuri De Stefani / CrazyflieController_final

programa final

VerticalController/VerticalController.cpp

Committer:
yurindes
Date:
2018-11-30
Branch:
yuri
Revision:
8:1ad52489f6f3

File content as of revision 8:1ad52489f6f3:

# 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;
}