clear all;
close all;



xiteP=5;
xiteI=2.2;
xiteD=2.4;

i1_1=-10;i2_1=-10;i3_1=-50;i4_1=-10;
p3_1=-10;p1_1=-10;p2_1=-10;p4_1=-10;
d1_1=-0;d2_1=-0;d3_1=-0;d4_1=-0;

error_1=0;
error_2=0; 


e1_1=0;e2_1=0;e3_1=0;e4_1=0;
e1_2=0;e2_2=0;e3_2=0;e4_2=0;
u_1=0;
xk=[-0.1,0,0.0005,0];   %Initial state

ts=0.02;

for k=1:1:700
time(k)=k*ts;
Tspan=[0 ts];
para=u_1;
%para=0;
[t,x]=ode45('nn_pidf',Tspan,xk,[],para);
xk=x(length(x),:);

r1(k)=0.0;    
r2(k)=0.0;    
r3(k)=0.0;    
r4(k)=0.0;    
x1(k)=xk(1);
x2(k)=xk(2);
x3(k)=xk(3);
x4(k)=xk(4);

e1(k)=r1(k)-x1(k);
e2(k)=r2(k)-x2(k);
e3(k)=r3(k)-x3(k);
e4(k)=r4(k)-x4(k);
error(k)=0.2*e1(k)+0.8*e3(k);

 xx(1)=error(k)-error_1;             %P
 xx(2)=error(k);                     %I
 xx(3)=error(k)-2*error_1+error_2;   %D

i1(k)=i1_1+xiteI*xx(2);
p1(k)=p1_1+xiteP*xx(1);
d1(k)=d1_1+xiteD*xx(3);
i2(k)=i2_1+xiteI*xx(2);
p2(k)=p2_1+xiteP*xx(1);
d2(k)=d2_1+xiteD*xx(3);
i3(k)=i3_1+xiteI*xx(2);
p3(k)=p3_1+xiteP*xx(1);
d3(k)=d3_1+xiteD*xx(3);
i4(k)=i4_1+xiteI*xx(2);
p4(k)=p4_1+xiteP*xx(1);
d4(k)=d4_1+xiteD*xx(3);

  

    de1(k)=e1(k)-e1_1;
    ce1(k)=e1(k)-2*e1_1+e1_2;
   u1(k)=i1(k)*e1(k)+p1(k)*de1(k)+d1(k)*ce1(k);
  
   de2(k)=e2(k)-e2_1;
    ce2(k)=e2(k)-2*e2_1+e2_2;
   u2(k)=i2(k)*e2(k)+p2(k)*de2(k)+d2(k)*ce2(k);
   
   de3(k)=e3(k)-e3_1;
    ce3(k)=e3(k)-2*e3_1+e3_2;
   u3(k)=i3(k)*e3(k)+p3(k)*de3(k)+d3(k)*ce3(k);
   
   de4(k)=e4(k)-e4_1;
   ce4(k)=e4(k)-2*e4_1+e4_2;
   u4(k)=i4(k)*e4(k)+p4(k)*de4(k)+d4(k)*ce4(k);
   u(k)=u1(k)+u2(k)+u3(k)+u4(k);
   
   %u(k)=(11*u1(k)+20*u2(k)+96.5*u3(k)+35.5*u4(k))/50;
u(k)=u1(k)+u2(k)+u3(k)+u4(k);

if k==400
    u(k)=u(k)+1
end

if u(k)>=10
   u(k)=10;
elseif u(k)<=-10
  	u(k)=-10;
end

error_2=error_1;
error_1=error(k);



i1_1=i1(k);i2_1=i2(k);i3_1=i3(k);i4_1=i4(k);
p1_1=p1(k);p2_1=p2(k);p3_1=p3(k);p4_1=p4(k);
d1_1=d1(k);d2_1=d2(k);d3_1=d3(k);d4_1=d4(k);

	e1_2=e1_1;e1_1=e1(k);
	e2_2=e2_1;e2_1=e2(k);
	e3_2=e3_1;e3_1=e3(k);
	e4_2=e4_1;e4_1=e4(k);
	u_1=u(k);
end




figure(1);
subplot(411);
plot(time,r1,'r',time,x1);      
xlabel('time(s)');ylabel('cart position');
subplot(412);
plot(time,r2,'r',time,x2);     
xlabel('time(s)');ylabel('cart rate');
subplot(413);
plot(time,r3,'r',time,x3);     
xlabel('time(s)');ylabel('angle position');
subplot(414);
plot(time,r4,'r',time,x4);     
xlabel('time(s)');ylabel('angle rate');
figure(5);
plot(time,u);                 
xlabel('time(s)');ylabel('Force');

x1_1=x1;
x1_2=x2;
x1_3=x3;
x1_4=x4;