26 lines
1.1 KiB
Mathematica
26 lines
1.1 KiB
Mathematica
|
function [q,qd,q2d] = mixed_traj(t,C,A,B,w,N)
|
||
|
|
% ---------------------------------------------------------------------
|
||
|
|
% This function computes "mixed trajectory" meaning the trajectory
|
||
|
|
% that consistes of finite fourier series and fifth order polynomial
|
||
|
|
% Inputs:
|
||
|
|
% t - time instants at which trajectory should be evaluated
|
||
|
|
% C - coefficients of the fifth order polynomail
|
||
|
|
% A - coeffincients of the sine in finite fourier series
|
||
|
|
% B - coefficinets of the cosine in finitne fourier series
|
||
|
|
% w - fundamental frequency
|
||
|
|
% N - number of harmonics
|
||
|
|
% ---------------------------------------------------------------------
|
||
|
|
% finite fourier series
|
||
|
|
[qh,qhd,qh2d] = fourier_series_traj(t,zeros(6,1),A,B,w,N);
|
||
|
|
|
||
|
|
% fifth order polynomail trajectory
|
||
|
|
qp = C(:,1) + C(:,2).*t + C(:,3).*t.^2 + C(:,4).*t.^3 + ...
|
||
|
|
C(:,5).*t.^4 + C(:,6).*t.^5;
|
||
|
|
qpd = C(:,2) + 2*C(:,3).*t + 3*C(:,4).*t.^2 + ...
|
||
|
|
4*C(:,5).*t.^3 + 5*C(:,6).*t.^4;
|
||
|
|
qp2d = 2*C(:,3) + 6*C(:,4).*t + 12*C(:,5).*t.^2 + 20*C(:,6).*t.^3;
|
||
|
|
|
||
|
|
q = qh+qp;
|
||
|
|
qd = qhd+qpd;
|
||
|
|
q2d = qh2d+qp2d;
|