Yuri De Stefani / Mbed 2 deprecated VerticalController

Controlador vertical

Dependencies:   mbed

VerticalController.cpp@0:115db3c33311, 2018-10-24 (annotated)

Committer:
yurindes
Date:
Wed Oct 24 11:58:36 2018 +0000
Revision:
0:115db3c33311
controlador vertical :)

Who changed what in which revision?

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