Torktan Açı Hesaplama ve Diferansiyel

Iyi Gunler;

Yazdigim programda kuvvet degerlerimiz ve kosullari dogru olup, tork ve torktan açıyı bulma kisminda sikinti yasiyorum. Konu ile ilgili yardimci olup tavsiye verebilirseniz cok sevinirim.
Tesekkurler

clc,clear,close all
dt=0.0001;
t=0:dt:6;
ndata=size(t,2);
g=-9.81;
T=10; %Thrust N
m=10; %weight kg
Iy=2.161/2; %Inertia kgm^2
pF= -0.75+0.6; %position of front foil respect to G
pB= 0.6;  %position of back foil respect to G
%% Initial Conditions
uy(1)=0.00001;
ux(1)=0.00001;
y(1)=-0.18;
x(1)=0;
d=y(1);
a(1)=0;
ro(1)=0;
rou(1)=0;
datd(1)=0;
inrd(1)=0;
lff(1)=0;
lfb(1)=0;
dff(1)=0;
dfb(1)=0;
df=0;
db=0;
dfd(1)=0;
dbd(1)=0;
ang(1)=0;
Tord(1)=0;

%%
for i=2:ndata
    if d<0
    T=0;
else
    T=T;
end
%     df=d;
%     db=d;
    %Moment equations
    Tf=lift_force_front(ang(i-1),df*10^3,ux(i-1))*pF; %Nm !!!!!!
    Tb=lift_force_back(ang(i-1),db*10^3,ux(i-1))*pB +drag_force_back(ang(i-1),db*10^3,ux(i-1))*0.108+drag_force_front(ang(i-1),df*10^3,ux(i-1))*0.130;  %Nm !!!!!!Drag acts as the same with this torque
    TorT=-0.12*T;
    Tor=Tf+Tb+TorT;
    Tord(i)=Tor;
    %Lift force Drag equations
    dragf=drag_force_front(ang(i-1),df*10^3,ux(i-1))+drag_force_back(ang(i-1),db*10^3,ux(i-1)); %N
    drag=viscous_res(ux(i-1))+induced_res(ux(i-1)); %N
    liftf=lift_force_front(ang(i-1),df*10^3,ux(i-1))+lift_force_back(ang(i-1),db*10^3,ux(i-1)); %N
    %Force components respect to angle of attack
    fy=liftf*cosd(a(i-1))-dragf*sind(a(i-1))-induced_res(uy(i-1));
    fx=dragf*cosd(a(i-1))+liftf*sind(a(i-1));
    lift=buoyancy_Force(d*10^3)+fy;
    %Velocity equations
    uy(i)=uy(i-1)+(lift+m*g)*dt/m;
    ux(i)=ux(i-1)+(T-(drag+fx))*dt/m;
    rou(i)=rou(i-1)+(Tor/Iy)*dt/m;
    %Location equations
    y(i)=y(i-1)+uy(i)*dt;
    x(i)=x(i-1)+ux(i)*dt;
    ro(i)=ro(i-1)+rou(i)*dt;
    a(i)=atand(uy(i-1)/ux(i-1));
    %Coverting to degrees
    roud(i)=rad2deg(rou(i));
    rod(i)=rad2deg(ro(i));
    ang(i)=rod(i)+a(i);
    %Storing some data
    d=-y(i);
    datd(i)=d;
    inrd(i)=induced_res(uy(i-1));
    lff(i)=lift_force_front(ang(i-1),df*10^3,ux(i-1));
    lfb(i)=lift_force_back(ang(i-1),db*10^3,ux(i-1));
    dff(i)=drag_force_front(ang(i-1),df*10^3,ux(i-1));
    dfb(i)=drag_force_back(ang(i-1),db*10^3,ux(i-1));
    df=0.15*sind(ang(i))+d;
    db=0.6*sind(ang(i))+d;
    dfd(i)=df;
    dbd(i)=db;

end