% FOR HELICOPTER NR 3-10
% This file contains the initialization for the helicopter assignment in
% the course TTK4115. Run this file before you execute QuaRC_ -> Build 
% to build the file heli_q8.mdl.

% Oppdatert høsten 2006 av Jostein Bakkeheim
% Oppdatert høsten 2008 av Arnfinn Aas Eielsen
% Oppdatert høsten 2009 av Jonathan Ronen
% Updated fall 2010, Dominik Breu
% Updated fall 2013, Mark Haring
% Updated spring 2015, Mark Haring


%%%%%%%%%%% Calibration of the encoder and the hardware for the specific
%%%%%%%%%%% helicopter
Joystick_gain_x = 0.7;
Joystick_gain_y = 0.7;


%%%%%%%%%%% Physical constants
g = 9.81; % gravitational constant [m/s^2]
l_c = 0.46; % distance elevation axis to counterweight [m]
l_h = 0.66; % distance elevation axis to helicopter head [m]
l_p = 0.175; % distance pitch axis to motor [m]
m_c = 1.92; % Counterweight mass [kg]
m_p = 0.72; % Motor mass [kg]

%%%%%%%%%%% Constants found at the lab
e_offset = -0.454;

vs_0 = 7.2;

p_offset = 0;

k_f = -(m_c*g*l_c - 2*m_p*g*l_h)/(vs_0*l_h);

j_p = 2 * m_p * l_p^2;
j_e = m_c * l_c^2 + 2 * m_p * l_h^2;
j_l = m_c * l_c^2 + 2 * m_p * (l_h^2 + l_p^2);

k_1 = l_p * k_f /j_p;

k_2 = l_h * k_f / j_e;

k_3 = (l_h* k_f) / j_l;

%%%%%%%%%%% LQR Regulator
A = [
  [ 0  1  0  0  0];
  [ 0  0  0  0  0];
  [ 0  0  0  0  0];
  [-1  0  0  0  0];
  [ 0  0 -1  0  0];
];

B = [
  [  0   0];
  [  0 k_1];
  [k_2   0];
  [  0   0];
  [  0   0];
];

%C = [
%  [0 1 0];
%  [0 0 1];
%];

% Q11 = Pitch error straff (høyere verdi gir tillater mere pitch)
% Q22 = Pitch rate error straff (høyere verdi gir mere demping på pitch)
% Q33 = Elevation rate?
% Q44 = Pitch integral?
% Q55 = Elevation integral?

Q = diag([1000 500 500 50 100]);

R = diag([1 1]);

K = lqr(A, B, Q, R);

%F = inv(C*inv(B*K-A)*B)

F = [
   [K(1, 1) K(1, 3)];
   [K(2, 1) K(2, 3)];
]; % ?????

% Input control
x_unit_step_length = 3;
x_unit_step_start_time = 6;
x_unit_step_gain = 0.4;

y_unit_step_length = 3;
y_unit_step_start_time = 0;
y_unit_step_gain = 0.35;

unit_input_disable_delay = 10;

% IMU Config
PORT = 4;


% Luenberger Observer

radius = min(eig(A-B*K))*15;

p = radius*[
    exp(-30*pi/180*1j)
    exp(-15*pi/180*1j)
    exp(  0*pi/180*1j)
    exp( 15*pi/180*1j)
    exp( 30*pi/180*1j)
];

A_l = [
    0 1 0 0 0;
    0 0 0 0 0; 
    0 0 0 1 0;
    0 0 0 0 0;
  k_3 0 0 0 0;
];

B_l = [
    0   0;
    0   k_1;
    0   0;
    k_2 0;
    0   0
];

C_l = diag([0 0 1 0 1]);

L_l = place(A_l', C_l', p)';

