Yuri De Stefani / CrazyflieController_final

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers VerticalController.cpp Source File

VerticalController.cpp

00001 # include "mbed.h"
00002 # include "VerticalController.h"
00003 // Class constructor
00004 VerticalController :: VerticalController ()
00005 {
00006     f_t=0;
00007     z_e_last=0;
00008     
00009     float os=0.5f;
00010     float t_s=2.0f;
00011     float zeta=sqrt(log(os/100.0f)*log(os/100.0f))/sqrt(log(os/100.0f)*log(os/100.0f)+3.1415f*3.1415f);
00012     float w_n=4.0f/(t_s*zeta);
00013     kp=pow(w_n, 2);
00014     kd=2.0f*zeta*w_n;
00015 }
00016 // Control thrust force (N) given vertical position (m) and velocity (m/s)
00017 void VerticalController :: control ( float z_r , float z, float w)
00018 {
00019     float erro_e=z_r-z;
00020     float erro_e_ponto=(erro_e-z_e_last)/0.002f;
00021     z_e_last=erro_e;
00022     float z_r_2pontos=kp*erro_e+kd*erro_e_ponto;
00023     f_t=z_r_2pontos*0.03f;
00024 }
Generated on Fri Jul 15 2022 13:09:41 by  doxygen 1.7.2