clc;
clear;
%motor modeling   
Ra=1.11;
La=200e-6;
Ct=0.0364;
Ue=24;
Ce=Ct;
J=67.7e-7;
aaa=9.54929658/(Ce*30);    %轉速：脈沖數(shù)/ms
bbb=J*La/(Ce*Ct);
ccc=J*Ra/(Ce*Ct);
ts=0.001;
sys=tf(aaa,[bbb,ccc,1]);
dsys=c2d(sys,ts,'tusin');
[num,den]=tfdata(dsys,'v');
u_1=0.0;
u_2=0.0;
y_1=0.0;
y_2=0.0;
e_3=0;
e_2=0;
e_1=0;
se_2=0;
se_1=0;
se_3=0;
uu=0;
vv=0;
ss=0;
sss=0;
se=0;
x=[0 0 0]';
%%%%%%%%%%% 位置環(huán)參數(shù) %%%%%%%%%%
%skp=0.2;
%ski=4;
%skp=0.38;
%ski=0.15;
skp=0.3;
ski=0.55;
skd=0.00000;
p0=skp*(1+0.001/ski+skd/0.001);      %增量式PID
p1=-skp*(1+2*skd/0.001);
p2=skp*skd/0.01;
%%%%%%%%%%% 速度環(huán)參數(shù) %%%%%%%%%%
kp=0.135;
ki=0.005;
kd=0;
q0=kp*(1+0.001/ki+kd/0.001);      %增量式PID
q1=-kp*(1+2*kd/0.001);
q2=kp*kd/0.01;
sexp=[1.6667	6.6667	15	26.667	41.667	60	81.667	106.67	135	166.67	201.67	240	281.67	326.67	375	426.67	481.67	540	601.67	666.67	735	806.67	881.67	960	1041.7	1126.7	1215	1306.7	1401.7	1500	1600	1700	1800	1900	2000	2100	2200	2300	2400	2500	2600	2700	2800	2900	3000	3100	3200	3300	3400	3500	3600	3700	3800	3900	4000	4100	4200	4300	4400	4500	4600	4700	4800	4900	5000	5100	5200	5300	5400	5500	5600	5700	5800	5900	6000	6100	6200	6300	6400	6500	6600	6700	6800	6900	7000	7100	7200	7300	7400	7500	7600	7700	7800	7900	8000	8100	8200	8300	8400	8500	8600	8700	8800	8900	9000	9100	9200	9300	9400	9500	9600	9700	9800	9900	10000	10100	10200	10300	10400	10500	10600	10700	10800	10900	11000	11100	11200	11300	11400	11500	11600	11700	11800	11900	12000	12100	12200	12300	12400	12500	12600	12700	12800	12900	13000	13100	13200	13300	13400	13500	13600	13700	13800	13900	14000	14100	14200	14300	14400	14500	14600	14700	14800	14900	15000	15100	15200	15300	15400	15500	15598	15693	15785	15873	15958	16040	16118	16193	16265	16333	16398	16460	16518	16573	16625	16673	16718	16760	16798	16833	16865	16893	16918	16940	16958	16973	16985	16993	16998	17000	17000];
vexp=[3.3333	6.6667	10	13.333	16.667	20	23.333	26.667	30	33.333	36.667	40	43.333	46.667	50	53.333	56.667	60	63.333	66.667	70	73.333	76.667	80	83.333	86.667	90	93.333	96.667	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	100	96.667	93.333	90	86.667	83.333	80	76.667	73.333	70	66.667	63.333	60	56.667	53.333	50	46.667	43.333	40	36.667	33.333	30	26.667	23.333	20	16.667	13.333	10	6.6667	3.3333	0 0];
for k=1:201
time1(k)=k*ts;
end
for k=1:200
    time(k)=k*ts;
%%%%%%%%%%% 位置環(huán)增量式PID %%%%%%%%%%  
vvexp(k+1)=p0*se_1+p1*se_2+p2*se_3+vv;
%vvexp(k+1)=skp*se_1+vv;
if vvexp(k+1)>=200
   vvexp(k+1)=200;
end
if vvexp(k+1)<=-200
   vvexp(k+1)=-200;
end
vv=vvexp(k+1);
%%%%%%%%%%% 速度環(huán)增量式PID %%%%%%%%%% 
u(k)=q0*e_1+e_2*q1+e_3*q2+uu;
%%%%%%%%%%%%   Adding disturbance  %%%%%%%%%%%
%if k==100               
%   u(k)=u(k)+4.0;
%end
%%%%%%%%%%% 積分飽和 %%%%%%%%%%
if u(k)>=24
   u(k)=24;
end
if u(k)<=-24
   u(k)=-24;
end
uu=u(k);
%%%%%%%%%%% 實際速度 %%%%%%%%%%
yout(k)=-den(2)*y_1-den(3)*y_2+num(1)*u(k)+num(2)*u_1+num(3)*u_2;
%%%%%%%%%% Adding disturbance %%%%%%%%%
%if k==125               
%   yout(k)=yout(k)+100;
%end
u_2=u_1;
u_1=u(k);
y_2=y_1;
y_1=yout(k);
%%%%%%%%%%% 實際位移 %%%%%%%%%%
sact(k)=yout(k)+sss;
sss=sact(k);
%%%%%%%%%%% 速度環(huán)誤差計算 %%%%%%%
error(k)=vvexp(k+1)-yout(k);
e_3=e_2;
e_2=e_1;
e_1=error(k);
%%%%%%%%%%% 位置環(huán)誤差計算 %%%%%%%
serror(k)=sexp(k+1)-sact(k);
se_3=se_2;
se_2=se_1;
se_1=serror(k);
end
%plot(time,yout,time1,vvexp)    %速度環(huán)
%plot(time1,sexp,time,sact)     
plot(time1,vvexp,time1,vexp)     %位置環(huán)
%plot(time,serror,time1,sexp,time,sact)             %累積位置誤差
%plot(time,u)
%plot(time1,vvexp,time1,vexp,time,yout)
%plot(time,serror)