global angl1k;
global angl1k1;
global angl2k;
global angl2k1;
global velo1;
global velo1k1;
global velo2k;
global velo2k1;
global acce1k;
global acce1k1;
global acce2k;
global acce2k1;
global t;
angl1k1=0;
angl2k1=0;
velo1k1=0;
velo2k1=0;
acce1k1=0;
acce2k1=0;
erro1k=0;
erro2k=0;
erro1k_1=0;
erro2k_1=0;
t=0.0005;
t1k=0;
t2k=0;
rule=[-5,-5,-5,-5,-5,-5,-5,-4,-3,-2,0,0,0;
-5,-5,-5,-5,-5,-5,-5,-4,-3,-2,0,0,0;
-5,-5,-5,-5,-5,-5,-5,-3,-3,-2,0,0,0;
-4,-4,-4,-4,-4,-4,-4,-3,-2,-1,1,1,1;
-4,-4,-4,-4,-4,-4,-4,-2,-1,0,2,2,2;
-4,-4,-4,-3,-3,-3,-3,-1,2,2,3,3,3;
-4,-4,-4,-3,-3,-1,0,1,3,3,4,4,4;
-3,-3,-3,-2,-2,1,3,3,3,3,4,4,4;
-2,-2,0,0,1,2,4,4,4,4,4,4,4;
-1,-1,0,1,2,3,4,4,4,4,4,4,4;
0,0,1,2,3,4,5,5,5,5,5,5,5;
0,0,1,2,3,4,5,5,5,5,5,5,5;
0,0,1,2,3,4,5,5,5,5,5,5,5];
ke1=200;
kec1=1;
kcw1=600;
ke2=200;
kec2=1;
kcw2=600/6;
x=0;
A1=[];
A2=[];
DA1=[];
DA2=[];
ERR1=[];
ERR2=[];
WN1=[];
WN2=[];
X=[];
load 'waveletpara.mat';
%main
for i=0:1999
erro1k_1=erro1k;
erro2k_1=erro2k;
angl1k=angl1k1;
angl2k=angl2k1;
velo1k=velo1k1;
velo2k=velo2k1;
acce1k=acce1k1;
acce2k=acce2k1;
x=x+t;
de_angl1k=sin(2*pi*x);
de_angl2k=sin(2*pi*x);
%%%%%%%%%%%%%%%%%%%%%%%
erro1k=de_angl1k-angl1k;
derro1k=(erro1k-erro1k_1)/t;
erro2k=de_angl2k-angl2k;
derro2k=(erro2k-erro2k_1)/t;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
E1=round(erro1k*ke1);
if E1>6
E1=6;
elseif E1<-6
E1=-6;
end
EC1=round(derro1k*kec1);
if EC1>6
EC1=5;
elseif EC1<-6
EC1=-6;
end
%%%%%%%%%%%%%%%%%%%%%
E2=round(erro2k*ke2);
if E2>6
E2=6;
elseif E2<-6
E2=-6;
end
EC2=round(derro2k*kec2);
if EC2>6
EC2=5;
elseif EC2<-6
EC2=-6;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%%%%%%% Wavelet Net
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
out1_one(1)=E1/6;
out1_one(2)=EC1/6;
out1_two(1)=E2/6;
out1_two(2)=EC2/6;
%%%%%%%%%network1
%%%% second layer,getting wavelet membership
for i=1:2
for j=1:3
to(i,j)=(out1_one(i)-bo(i,j))/ao(i,j);
out2_one(i,j)=cos(0.5*to(i,j))*exp(-to(i,j)^2/2);
end
end
% third layer,'and operation'
for i=1:3
for j=1:3
out3_one(i,j)=out2_one(1,i)*out2_one(2,j);
end
end
% forth layer defuzziation
sumone=0;
wholesumone=0;
for i=1:3
for j=1:3
sumone=sumone+out3_one(i,j);
wholesumone=wholesumone+out3_one(i,j)*weight3_one(i,j);
end
end
netout(1)=wholesumone/sumone;
%network2
%second layer
for i=1:2
for j=1:3
tt(i,j)=(out1_two(i)-bt(i,j))/at(i,j);
out2_two(i,j)=cos(0.5*tt(i,j))*exp(-tt(i,j)^2/2);
end
end
% third layer, 'and operation'
for i=1:3
for j=1:3
out3_two(i,j)=out2_two(1,i)*out2_two(2,j);
end
end
%%%%% forth layer defuzzization
sumtwo=0;
wholesumtwo=0;
for i=1:3
for j=1:3
sumtwo=sumtwo+out3_two(i,j);
wholesumtwo=wholesumtwo+out3_two(i,j)*weight3_two(i,j);
end
end
netout(2)=wholesumtwo/sumtwo;
%%%%%%%%%% fifth layer
wn(1)=weight4(1,1)*netout(1)+weight4(1,2)*netout(2);
wn(2)=weight4(2,1)*netout(1)+weight4(2,2)*netout(2);
t1k=wn(1)*kcw1*6;
t2k=wn(2)*kcw2*6;
WN1=[WN1,wn(1)];
WN2=[WN2,wn(2)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[angl1k1,angl2k1,velo1k1,velo2k1,acce1k1,acce2k1]=robot(t1k,t2k,angl1k,angl2k,velo1k,velo2k,acce1k,acce2k,t);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A1=[A1,angl1k];
A2=[A2,angl2k];
DA1=[DA1,de_angl1k];
DA2=[DA2,de_angl2k];
ERR1=[ERR1,erro1k];
ERR2=[ERR2,erro2k];
X=[X,x];
end
figure(1)
plot(X,A1,'-',X,DA1,'-');
figure(2)
plot(X,A2,'-',X,DA2,'-');
figure(3)
plot(X,ERR1,'-');
axis([0 1 -0.15 0.15]);
figure(4)
plot(X,WN1,'-');
figure(6)
plot(X,WN2,'-'); |