Yuri De Stefani / CrazyflieController_final

programa final

HorizontalController/HorizontalController.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 "HorizontalController.h"
yurindes 8:1ad52489f6f3 3 // Class constructor
yurindes 8:1ad52489f6f3 4
yurindes 8:1ad52489f6f3 5
yurindes 8:1ad52489f6f3 6 // na biblioteca
yurindes 8:1ad52489f6f3 7 HorizontalController :: HorizontalController ()
yurindes 8:1ad52489f6f3 8 {
yurindes 8:1ad52489f6f3 9
yurindes 8:1ad52489f6f3 10 phi_r=0;
yurindes 8:1ad52489f6f3 11 theta_r=0;
yurindes 8:1ad52489f6f3 12 kp_hori = 1.35f;
yurindes 8:1ad52489f6f3 13 kd_hori = 2;
yurindes 8:1ad52489f6f3 14 x_e_last=0;
yurindes 8:1ad52489f6f3 15 y_e_last=0;
yurindes 8:1ad52489f6f3 16 }
yurindes 8:1ad52489f6f3 17 // Control reference roll and pitch angles given reference horizontal positions and current horizontal positions and velocities
yurindes 8:1ad52489f6f3 18 void HorizontalController :: control ( float x_r , float y_r , float x, float y, float u, float v)
yurindes 8:1ad52489f6f3 19 {
yurindes 8:1ad52489f6f3 20 theta_r = (1 / g)* control_state_regulator (x_r , x , x_e_last , kp_hori , kd_hori);
yurindes 8:1ad52489f6f3 21 x_e_last = pos_e_last;
yurindes 8:1ad52489f6f3 22 phi_r = - (1 / g)* control_state_regulator(y_r , y , y_e_last , kp_hori , kd_hori);
yurindes 8:1ad52489f6f3 23 y_e_last = pos_e_last;
yurindes 8:1ad52489f6f3 24 }
yurindes 8:1ad52489f6f3 25 // Control acceleration given reference position and current position and velocity with given controller gains
yurindes 8:1ad52489f6f3 26 float HorizontalController :: control_state_regulator ( float pos_r , float pos , float pos_e_last, float kp, float kd)
yurindes 8:1ad52489f6f3 27 {
yurindes 8:1ad52489f6f3 28 float pos_e_hori = pos_r - pos;
yurindes 8:1ad52489f6f3 29 float vel = (pos_e_hori - pos_e_last) / delta_hori;
yurindes 8:1ad52489f6f3 30 pos_e_last = pos_e_hori;
yurindes 8:1ad52489f6f3 31 float pos_e_hori_2ponto = kp * pos_e_hori + kd * vel;
yurindes 8:1ad52489f6f3 32 return pos_e_hori_2ponto;
yurindes 8:1ad52489f6f3 33 }