close all, clear all
% Tillst?nd
T = 20;
beta = zeros(1, T);
beta(1) = pi/2;
betaPrick = zeros(1, T);

theta = zeros(1, T)
theta(1) = ((pi/2)-beta(1))*56/pi;
thetaPrick = zeros(1,T);

% Parametrar
Tvrid = 1.8;
g = 9.81;
mskar = 0.77058;   % massa skär
Fmax = 80;
Js = 0.00315;
Jm = 0.00000053;
m = mskar;
g = 9.81;
L = 0.2;

% Kraft
dt = 0.001;
r = 0.01;
h = L;


for k = 1 : T-1
  % Krafterna

  x = L * cos(beta(k));
  y = h - L * sin(beta(k));
  lBeta = sqrt(x^2 + y^2);

  phi = acos((L^2 + lBeta^2 - L^2)/(2 * L * lBeta));
  psi = pi/2 - phi;

  S = (m * g * cos(beta(k)) / 2) / cos(psi);
%   Tvrid = 20;
  F = cos(psi) * Tvrid / r;
  % Derivatorna
  derivataBetaPrick = m*g*cos(beta(k)) * L / 2 - F * L;
  derivataThetaPrick = Tvrid + fs(beta(k)) * r;

  % Ekvationerna f?r beta
  betaPrick(k+1) = betaPrick(k) + dt * derivataBetaPrick / Js;
  beta(k+1) = beta(k) + dt * betaPrick(k);

  % Ekvationerna f?r theta
  thetaPrick(k+1) = thetaPrick(k) + dt * derivataThetaPrick / Jm;
  theta(k+1) = theta(k) + dt * thetaPrick(k);
end

figure
subplot(1,2,1)
plot(beta*180/pi)
axis([0 T 0 90])
title('beta')
subplot(1,2,2)
plot(theta/pi/10)
title('theta')