%　ランプ指令に対する移動平均処理（2回）

clear
format;

%移動平均処理前のランプ指令

x0 = 0;         %初期位置[m]
x_fin = 0.010;  %終了位置[m]

vel1 = 0.1;      %速度1[m/s] 
vel2 = -0.1;      %速度2[m/s] 
Tstop1 = 0.5;    %停止時間1[sec]
Tstop2 = 0.5;    %停止時間2[sec]
Tstop3 = 0.5;    %停止時間3[sec]

Tave1 = 0.020;  %移動平均時間1[sec]
Tave2 = 0.020;  %移動平均時間2[sec]

dTref = 1e-3;      %サンプリング時間[sec]

Xref = [];
Tref = [];

for Tsim = 0:dTref:Tstop1
    Xref = [Xref
            x0  ];            
    Tref = [Tref
            Tsim];
end

for Tsim = (Tstop1+dTref):dTref:(Tstop1+abs((x_fin-x0)/vel1))
    
    Xref = [Xref
            Xref(size(Xref,1)) + vel1*dTref  ];    
    Tref = [Tref
            Tsim];      
end

for Tsim = (Tstop1+dTref+abs((x_fin-x0)/vel1)):dTref:(Tstop1+Tstop2+abs((x_fin-x0)/vel1))
    Xref = [Xref
            x_fin  ];            
    Tref = [Tref
            Tsim];
end

for Tsim = (Tstop1+Tstop2+abs((x_fin-x0)/vel1)+dTref):dTref:(Tstop1+Tstop2+2*abs((x_fin-x0)/vel1))
    Xref = [Xref
             Xref(size(Xref,1)) - vel1*dTref ];            
    Tref = [Tref
            Tsim];
end

for Tsim = (Tstop1+Tstop2+2*abs((x_fin-x0)/vel1)+dTref):dTref:(Tstop1+Tstop2+Tstop3+2*abs((x_fin-x0)/vel1))
    Xref = [Xref
            x0 ];            
    Tref = [Tref
            Tsim];
end

%移動平均処理1回目

nave1 = Tave1/dTref;
Xref_Ave1 = [];

for Tsim = 0:dTref:max(Tref)
 Xref_Ave1_tmp = 0;
    for  counter = 0:(nave1-1)
        if ( (Tsim-counter*dTref) - Tave1) < 0
             Xref_Ave1_tmp2 = Xref(1);
         else
             Xref_Ave1_tmp2 = Xref(round(Tsim/dTref)-counter+1);
        end
        
        Xref_Ave1_tmp = Xref_Ave1_tmp + Xref_Ave1_tmp2/nave1;
        
    end
     Xref_Ave1 = [Xref_Ave1
                    Xref_Ave1_tmp];
 end
 
 %移動平均処理2回目

nave2 = Tave2/dTref;
Xref_Ave2 = [];

for Tsim = 0:dTref:max(Tref)
 Xref_Ave2_tmp = 0;
    for  counter = 0:(nave2-1)
        if ( (Tsim-counter*dTref) - Tave2) < 0
             Xref_Ave2_tmp2 = Xref_Ave1(1);
         else
             Xref_Ave2_tmp2 = Xref_Ave1(round(Tsim/dTref)-counter+1);
        end
        
        Xref_Ave2_tmp = Xref_Ave2_tmp + Xref_Ave2_tmp2/nave2;
        
    end
     Xref_Ave2 = [Xref_Ave2
                    Xref_Ave2_tmp];
 end
 
%グラフ 
plot(Tref,Xref,'b-');
hold on
plot(Tref,Xref_Ave1,'g-');
plot(Tref,Xref_Ave2,'r-');
grid on



