qjones sin trajectory work in progress

sin_traj.m

function [y,yd,ydd] = straj(D, Vmax, Amax, dt, V0, Vf)
%TODO: Pass move time
%TODO: Short moves

%Calculate Additional params
Sgn = sign(D);
Df = abs(D);

% Compute Accel/Decel Times
T1 = 2*(Vmax-V0)/Amax;
T3 = 2*(Vmax-Vf)/Amax;

% Compute Accel/Decel Distances
L1 = 1/4*Amax*T1^2 + V0*T1;
L3 = 1/4*Amax*T3^2 + Vf*T3;
L2 = Df - L1 - L3;

%TODO: Short moves support
if Df < L1 + L3
   error('Short Move');
   % Compute Max Velocity for move blah
   %Recompute Times
end

Vc = 1/2 * Amax * T1 + V0;
T2 = (L2)/Vc;

omega_L1 = 2*pi/T1;
omega_L3 = 2*pi/T3;

Tt = T1+T2+T3; % Total Move Time

t1 = T1;
t2 = T2 + t1;
t3 = Tt;

t_traj = [0:dt:Tt];

y = zeros(length(t_traj), 1);
y_hat = y;

yd = y;
yd_hat = y;
 
ydd = y;
ydd_hat = y;

for i = 1:length(t_traj)
    t = t_traj(i);
    if(t <=0)
        y_hat(i) = 0;
        yd_hat(i) = V0;
        ydd_hat(i) = 0;
    elseif(0 < t && t <= t1) %Accel
        y_hat(i) =  Amax/2*(1/omega_L1)^2*(1/2*(omega_L1*t)^2-(1-cos(omega_L1*t)))+V0*t;
        yd_hat(i) = Amax/(2*omega_L1)*(omega_L1*t-sin(omega_L1*t)) + V0;
        ydd_hat(i) = Amax/2*(1-cos(omega_L1*t));
    elseif(t1 < t && t <= t2) % Constant velocity
        y_hat(i) = L1 + Vc * (t-t1);
        yd_hat(i) = Vc;
        ydd_hat(i) = 0;
    elseif(t > t2) % Decel
        D = (1-cos(2*pi/T3*(t-t2)));
        C = 1/2*(2*pi*(t-t2)/T3)^2;
        B = ((2*pi)^2*(t-t2))/T3;

        y_hat(i) =  Vf*(t-t2)+L1+L2+ Amax/2*(T3/(2*pi))^2*(B-C+D);
        yd_hat(i) = (Amax*(sin(omega_L3*(t - t2)) - omega_L3*(t-t2)))/(2*omega_L3) + Vc;
        ydd_hat(i) = -Amax/2*(1-cos(omega_L3*(t-t2)));
    end
end

% Adjust for move direction.
y = y_hat * Sgn;
yd = yd_hat * Sgn;
ydd = ydd_hat * Sgn;

clf
subplot(311)
hold on

plot(t_traj,y);

subplot(312)
plot(t_traj,yd);

% Plot the diff of 'y' to make sure derivations of 'y_d' is correct
hold on
dy=diff(y)./diff(t_traj);
plot(t_traj(2:end),dy, '--', 'LineWidth', 2);

subplot(313)
plot(t_traj,ydd);