% 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 = -1;
Joystick_gain_y = 1;

PORT = 4;

%%%%%%%%%%% 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 :)
J_e = m_c*l_c^2 + 2*m_p*l_h^2;
J_p = 2*m_p*l_p^2;
J_lamb = J_e + J_p;


L_2=l_c*m_c + l_h*2*m_p;
L_3=l_h;
L_4=l_h;

K_vso = 20;
F_s = (l_c/l_h)*g*m_c + 2*g*m_p;
V_so = L_3*K_vso/L_2;

K_f = F_s/V_so;

L_1 = l_p/K_f;

K_1 = L_1/J_p;
K_2 = L_3/J_e;
K_3 = L_4/J_lamb;


%Pitch controller param
lambda_p1 = -1;
lambda_p2 = -4;
K_pd = -(lambda_p1 + lambda_p2) / K_1;
K_pp = lambda_p1 * lambda_p2 / K_1;
