Yuri De Stefani / CrazyflieController_final

programa final

VerticalController/VerticalController.cpp@8:1ad52489f6f3, 2018-11-30 (annotated)

Committer:
yurindes
Date:
Fri Nov 30 19:23:29 2018 +0000
Branch:
yuri
Revision:
8:1ad52489f6f3
final;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yurindes 8:1ad52489f6f3 1 # include "mbed.h"
yurindes 8:1ad52489f6f3 2 # include "VerticalController.h"
yurindes 8:1ad52489f6f3 3 // Class constructor
yurindes 8:1ad52489f6f3 4 VerticalController :: VerticalController ()
yurindes 8:1ad52489f6f3 5 {
yurindes 8:1ad52489f6f3 6 f_t=0;
yurindes 8:1ad52489f6f3 7 z_e_last=0;
yurindes 8:1ad52489f6f3 8
yurindes 8:1ad52489f6f3 9 float os=0.5f;
yurindes 8:1ad52489f6f3 10 float t_s=2.0f;
yurindes 8:1ad52489f6f3 11 float zeta=sqrt(log(os/100.0f)*log(os/100.0f))/sqrt(log(os/100.0f)*log(os/100.0f)+3.1415f*3.1415f);
yurindes 8:1ad52489f6f3 12 float w_n=4.0f/(t_s*zeta);
yurindes 8:1ad52489f6f3 13 kp=pow(w_n, 2);
yurindes 8:1ad52489f6f3 14 kd=2.0f*zeta*w_n;
yurindes 8:1ad52489f6f3 15 }
yurindes 8:1ad52489f6f3 16 // Control thrust force (N) given vertical position (m) and velocity (m/s)
yurindes 8:1ad52489f6f3 17 void VerticalController :: control ( float z_r , float z, float w)
yurindes 8:1ad52489f6f3 18 {
yurindes 8:1ad52489f6f3 19 float erro_e=z_r-z;
yurindes 8:1ad52489f6f3 20 float erro_e_ponto=(erro_e-z_e_last)/0.002f;
yurindes 8:1ad52489f6f3 21 z_e_last=erro_e;
yurindes 8:1ad52489f6f3 22 float z_r_2pontos=kp*erro_e+kd*erro_e_ponto;
yurindes 8:1ad52489f6f3 23 f_t=z_r_2pontos*0.03f;
yurindes 8:1ad52489f6f3 24 }