load ball.dat;

m=0.226;
t=0.004*5*(1:length(ball));
theta4=ball(:,1);
effort4=ball(:,2);
theta5=ball(:,3);
effort5=ball(:,4);
y=ball(:,5);
y_dot=ball(:,6);
z=ball(:,7);
z_dot=ball(:,8);

tt=[];
yy=[];
zz=[];
zz_dot=[];
yy_dot=[];
count=0;
for i=1:length(ball)
         if  y(i)~=0
                tt=[tt t(i)];
                yy=[yy y(i)];
                zz=[zz z(i)];
                yy_dot=[yy_dot y_dot(i)];
                zz_dot=[zz_dot z_dot(i)];
                count=count+1;
         end
end

TL=1;
TM=0.8;
TS=0.6;

x1_scaling=0.1;
x2_scaling=0.1;

for j=1:(count-1)

        x1=zz(j)/x1_scaling;
        x2=zz_dot(j)/x2_scaling;

        %    Membership
        if (x1<-1)
                A1N=1;              %x1:N
                A1Z=0;              %x1:Z
                A1P=0;              %x1:P
        elseif(x1>=-1 &  x1<0)
                A1N=-x1;
                A1Z=(x1+1);
                A1P=0;
        elseif(x1>=0 & x1<=1)
                A1N=0;
                A1Z=-(x1-1);
                A1P=x1;
        elseif (x1>1)
                A1N=0;
                A1Z=0;
                A1P=1;
        end

        if (x2<-1)
                A2N=1;              %x2:N
                A2Z=0;              %x2:Z
                A2P=0;              %x2:P
        elseif(x2>=-1 & x2<0)
                A2N=-x2;
                A2Z=(x2+1);
                A2P=0;
        elseif(x2>=0 & x2<=1)
                A2N=0;
                A2Z=-(x2-1);
                A2P=x2;
        elseif (x2>1)
                A2N=0;
                A2Z=0;
                A2P=1;
        end


        mA1=min(A1N,A2N);  %RAle1:  x1:N   x2:N
        mA2=min(A1N,A2Z);  %RAle2:  x1:N   x2:Z
        mA3=min(A1N,A2P);  %RAle3:  x1:N   x2:P
        mA4=min(A1Z,A2N);  %RAle4:  x1:Z   x2:N
        mA5=min(A1Z,A2Z);  %RAle5:  x1:Z   x2:Z
        mA6=min(A1Z,A2P);  %RAle6:  x1:Z   x2:P
        mA7=min(A1P,A2N);  %RAle7:  x1:P   x2:N
        mA8=min(A1P,A2Z);  %RAle8:  x1:P   x2:Z
        mA9=min(A1P,A2P);  %RAle9:  x1:P   x2:P

        y1=TL;
        y2=(TL+TM)/2;
        y3=TM;
        y4=(TM+TS)/2;
        y5=TS;
        y6=(TM+TS)/2;
        y7=TM;
        y8=(TL+TM)/2;
        y9=TL;

        A=mA1+mA2+mA3+mA4+mA5+mA6+mA7+mA8+mA9;
        Treg=(mA1*y1+mA2*y2+mA3*y3+mA4*y4+mA5*y5+mA6*y6+mA7*y7+mA8*y8+mA9*y9)/A;

        T1=Treg;
        T2=T1;
        
        Fz(j)=-zz(j+1)*14/5*m/(T1+T2)^2;   % system uncertainty estimate

end

Uz=[zz(1:(count-1))'  zz_dot(1:(count-1))'  Fz']

fid=fopen('d:\user\853737\robot\ball_bal\learn4.dat','at');
for j=1:(count-1)
        fprintf(fid,'%6.4f    %6.4f    %6.4f\n',zz(j),zz_dot(j),Fz(j));
end
fclose(fid);              
