kaibin is one of my project group member. he is designing the code of filter, and me is designing the code of positive input shaping.. actually our project is divided into two stage .. first stage, we are use the matlab to design the simulation of flexible manipulator model, and also the positive input algorithm ... below is my simulation
stage 1 was done .. now, we are doing the stage 2.. The positive input shaping algorithm is embedded into dsPIC.. and the positive input shaping block in matlab is replace by the dsPIC... this also means that the flexible manipulator simulation block in matlab will " calculate " the angle , displacement, valocity, etc from the dsPIC, which is embedded with positive input shaping and filter .
here is my positive input shaping code in matlab
starting_time=0;
simulation_time=4;
sampling_time=0.001;
t=starting_time:sampling_time:simulation_time;
for i=1😔0.2/sampling_time)+1
u(i,1)=0;
end
for i=(0.2/sampling_time)+1😔0.5/sampling_time)+1
u(i,1)=0.3;
end
for i=(0.5/sampling_time)+1😔0.8/sampling_time)+1
u(i,1)=-0.3;
end
for i=(0.8/sampling_time)+1😔4/sampling_time)+1
u(i,1)=0;
end
% Positive ZV shaper (2-impulse)
% mode 1
pi=22/7;
z1=0.0086;
k1=exp((-z1*pi)/((1-z1^2)^0.5));
wn1=94;
wd1=wn1*(sqrt(1-z1^2));
%Determine the amplitudes and time location of the input shaper
ip=2;
tt1a(1)=0;
tt1a(2)=pi/wd1;
A1a(1)=1/(1+k1);
A1a(2)=A1a(1)*k1;
tt1a=tt1a./0.001;
tt1a=round(tt1a);
tt1a=tt1a.*0.001;
for i=1
v1a(i,1)=A1a(1);
end
ni=1;
while ni<=ip-1
for i=((tt1a(ni)/sampling_time)+2)😔(tt1a(ni+1)/sampling_time)+1)
v1a(i,1)=0;
end
for i=(tt1a(ni+1)/sampling_time)+1
v1a(i,1)=A1a(ni+1);
end
ni=ni+1;
end
for i=((tt1a(ni)/sampling_time)+2)😔(4/sampling_time)+1)
v1a(i,1)=0;
end
%mode 2
pi=22/7;
z2=0.0086;
k2=exp((-z2*pi)/((1-z2^2)^0.5));
wn2=344.70;
wd2=wn2*(sqrt(1-z2^2));
%Determine the amplitudes and time location of the input shaper
ip=2;
tt2a(1)=0;
tt2a(2)=pi/wd2;
A2a(1)=1/(1+k2);
A2a(2)=A2a(1)*k2;
tt2a=tt2a./0.001;
tt2a=round(tt2a);
tt2a=tt2a.*0.001;
for i=1
v2a(i,1)=A2a(1);
end
ni=1;
while ni<=ip-1
for i=((tt2a(ni)/sampling_time)+2)😔(tt2a(ni+1)/sampling_time)+1)
v2a(i,1)=0;
end
for i=(tt2a(ni+1)/sampling_time)+1
v2a(i,1)=A2a(ni+1);
end
ni=ni+1;
end
for i=((tt2a(ni)/sampling_time)+2)😔(4/sampling_time)+1)
v2a(i,1)=0;
end
imp1a=conv(v1a,v2a);
impa=imp1a(1😔4/sampling_time)+1);
shp1a=conv(impa,u);
PZV=shp1a(1😔4/sampling_time)+1);