%% 梯形速度曲线
%% 假定初始速度和最终速度为零，给定最大的加速度、运行时间、开始角度 、结束角度


function trapezoid()

%给定初始条件
t0=0;
tf=10;
q0=0;
qf=20;
a_max=1;

if a_max>= 4*(qf-q0)/(tf-t0)^2
% 先判断能否求解到t_f，给定的最大加速度是否满足条件   
% t_f 加速至匀速阶段的时间
    t_f=tf/2-0.5*sqrt( ( tf^2*a_max-4*(qf-q0)) / (a_max));
    i=1;
    t=t0:0.1:tf;
    n=size(t);

    qt=zeros(n(2),1);
     for t=t0:0.1:tf
         if t<=t_f
             qt(i)=q0+0.5*a_max*t*t;
         elseif t_f<t && t<=tf-t_f
             qt(i)=q0+0.5*a_max*t_f*t_f+a_max*t_f*(t-t_f);
         else 
             qt(i)=qf-0.5*a_max*(t-tf)^2;
         end
         i=i+1;
     end
     
     figure(1)
     plot(qt);
     legend('位置曲线')
     figure(2)
     plot(diff(qt))
     legend('速度曲线')
     figure(3)
     plot(diff(diff(qt)))
     legend('加速度曲线')
else
    disp('给定的加速度太小，不能求解出轨迹');
end

end

%% 梯形速度曲线
%% 给定开始角度 、结束角度、匀速阶段的速度、给定最大的加速度


function trapezoid_2()

%给定初始条件
t0=0;
q0=0;
qf=20;
v_max=3;   %这两个值不同，得到的轨迹曲线也就不一样
a_max=0.5;


t_c=v_max/a_max;
q_c=0.5*a_max*t_c^2;
t_f=(qf-2*q_c)/v_max+2*t_c;

t=t0:0.1:t_f;
n=size(t);
qt=zeros(n(2),1);

i=1;
if t_f-2*t_c>0  % 达到最大速度，也即速度曲线为梯形
     for t=t0:0.1:t_f
         if t<=t_c
             qt(i)=q0+0.5*a_max*t*t;
         elseif t_c<t && t<=t_f-t_c
             qt(i)=q0+0.5*a_max*t_c*t_c+a_max*t_c*(t-t_c);
         else 
             qt(i)=qf-0.5*a_max*(t_f-t)^2;
         end
         i=i+1;
     end
else  % 未达到最大速度，也即速度曲线为三角形
    t_c=sqrt( (qf-q0)/a_max);
    t_f=2*t_c;
         for t=t0:0.1:t_f
             if t<=t_c
                 qt(i)=q0+0.5*a_max*t*t;
             else 
                 qt(i)=qf-0.5*a_max*(t_f-t)^2;
             end
             i=i+1;
        end
    
end
     figure(1)
     plot(qt);
     legend('位置曲线')
     figure(2)
     plot(diff(qt))
     legend('速度曲线')
     figure(3)
     plot(diff(diff(qt)))
     legend('加速度曲线')

end