clc;
clear;
fuzzTab=[-6?-4?-2??0??2??4??6]
%?????????[NB?NM?NS?ZO?PS?PM?PB]

NB=fuzzTab（1）;
NM=fuzzTab（2）;?
NS=fuzzTab（3）;
Z0=fuzzTab（4）;?
PS=fuzzTab（5）;?
PM=fuzzTab（6）;?
PB=fuzzTab（7）;

%?模糊規(guī)則表
PID1.pTab=[NB?NB?NM?NM?NS?Z0?Z0;
??????????NB?NB?NM?NS?NS?Z0?Z0;
??????????NB?NM?NS?NS?Z0?PS?PS;
??????????NM?NM?NS?Z0?PS?PM?PM;
??????????NM?NS?Z0?PS?PS?PM?PB;
??????????Z0?Z0?PS?PS?PM?PB?PB;
??????????Z0?Z0?PS?PM?PM?PB?PB];
PID1.iTab=[NB?NB?NM?NM?NS?Z0?Z0;
??????????NB?NB?NM?NS?NS?Z0?Z0;
??????????NB?NM?NS?NS?Z0?PS?PS;
??????????NM?NM?NS?Z0?PS?PM?PM;
??????????NM?NS?Z0?PS?PS?PM?PB;
??????????Z0?Z0?PS?PS?PM?PB?PB;
??????????Z0?Z0?PS?PM?PM?PB?PB];
PID1.dTab=[PS?NS?NB?NB?NB?NM?PS;
??????????PS?NS?NB?NM?NM?NS?Z0;
??????????Z0?NS?NM?NM?NS?NS?Z0;
??????????Z0?NS?NS?NS?NS?NS?Z0;
??????????Z0?Z0?Z0?Z0?Z0?Z0?Z0
??????????PB?NS?PS?PS?PS?PS?PB;
??????????PB?PM?PM?PM?PS?PS?PB];
??????


%?模糊PID控制器?1
PID1.ref=0;?%?期望值
PID1.Kp=10;?%比例
PID1.Ki=2;?%積分
PID1.Kd=4;?%微分
PID1.err=0;%偏差
PID1.derr=0;

PID1.max=pi;%最大測量值?
PID1.min=-pi;%最小測量值?

PID1.maxDltKp=10;%Kp上限
PID1.minDltKp=-5;%Kp下限
PID1.scalKp=0.2;%Kp?權重系數(shù)

PID1.maxDltKi=1;%Ki上限
PID1.minDltKi=-2;%Ki下限
PID1.scalKi=0.5;%Ki?權重系數(shù)

PID1.maxDltKd=12;%Kd上限
PID1.minDltKd=-4;%Kd下限
PID1.scalKd=0.15;%Kd?權重系數(shù)


%?模糊PID控制器?2
PID2=PID1;
PID2.Kp=8;?%比例
PID2.Ki=1;?%積分
PID2.Kd=2;?%微分
PID2.maxDltKp=8;%Kp上限
PID2.minDltKp=-2;%Kp下限
PID2.scalKp=0.9;%Kp?權重系數(shù)

PID2.maxDltKi=1;%Ki上限
PID2.minDltKi=-1;%Ki下限
PID2.scalKi=0.5;%Ki?權重系數(shù)

PID2.maxDltKd=8;%Kd上限
PID2.minDltKd=-2;%Kd下限
PID2.scalKd=0.15;%Kd?權重系數(shù)



%圓心坐標
x0=110;
y0=110;
%半徑
R=40;
%連桿長度
a=100;
b=100;
t=1;

%?控制周期?
dt=0.05?%?秒
Time=[0];?%?當前時間
aimTheta=[0];%?關節(jié)1目標角度
aimPhi=[0];%關節(jié)2目標角度
realTheta=[0];%?關節(jié)1實際角度
realPhi=[0];%關節(jié)2實際角度
errTehta=[0];?%?關節(jié)1?角度誤差
errPhi=[0];?%?關節(jié)2?角度誤差
errThetaSum=0;%關節(jié)1累積誤差
errPhiSum=0;%關節(jié)2累積誤差
derrTheta=0;%關節(jié)1?本次角度誤差與上一次角度誤差的差值
derrPhi=0;%關節(jié)2?本次角度誤差與上一次角度誤差的差值
%?PID?參數(shù)（關節(jié)1）
%?Kp1=10;
%?Ki1=2;
%?Kd1=4;
%?%?PID?參數(shù)（關節(jié)2）
%?Kp2=8;
%?Ki2=1;
%?Kd2=2;

Kp1=8;
Ki1=2;
Kd1=4;
%?PID?參數(shù)（關節(jié)2）
Kp2=6;
Ki2=1;
Kd2=2;


saveW1=[];
saveW2=[];

for?i=0:0.1:2*pi+0.1
???
%?????theta2=i/150*2*pi;
%?????phi=i/150*pi;
????x=x0+R*cos（i）;
????y=y0+R*sin（i）;
????theta1=atan2（yx）;?
????%?theta1=acos（x/sqrt（x*x+y*y））;
????c=sqrt（x*x+y*y）;?%?末端到原點的距離
????theta3=acos（（c*c+a*a-b*b）/（2*a*c））;
????theta2=theta1-theta3;?%?關節(jié)1?角度
????phi=pi-acos（（a*a+b*b-c*c）/（2*a*b））;?%關節(jié)2角度
????aimTheta（end+1）=theta2;
????aimPhi（end+1）=phi;
????
????%連桿?P?位置
????P=Rot（theta2‘z‘）*[a;0;0];
????
????%?連桿末端位置（正運動學驗證）
????E=P+Rot（theta2‘z‘）*Rot（phi‘z‘）*[b;0;0];
????
????%?PID?偏差
?????PID1.err=theta2-realTheta（end）;
?????PID2.err=phi-realPhi（end）;
????
?????deltPID1=Fuzzy2（PID1realTheta（end）fuzzTab）;
?????deltPID2=Fuzzy2（PID2realPhi（end）fuzzTab）;
?????
?????PID1.Kp=Kp1+deltPID1（1）*PID1.scalKp;
?????PID1.Ki=Ki1+deltPID1（2）*PID1.scalKi;
?????PID1.Kd=Kd1+deltPID1（

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????4660??2020-05-11?23:37??ctrl.m
?????文件????????2965??2020-05-11?23:26??Fuzzy2.m
?????文件?????????860??2020-02-21?08:27??main.m
?????文件?????????305??2020-02-21?08:51??plotrobot.m
?????文件?????????366??2020-02-19?08:36??Rot.m