h=5;
a = -0.15;
history = @(t)[-0.2;0.1;0.1];
% Define the Rh function based on the given conditions
Rh = @(t) (0 <= t & t < h) .* 0 + ...
(h <= t & t <= 2*h) .* ((t-h).^5.*(2*h-t).^5) + ...
(t > 2*h) .* 0;
% Define the PDF gain function based on Rh
W_c = integral(@(s) Rh(s) .* exp(2 * a * s), h, 2 * h);
W = 1 / W_c;
K_a_h = @(t) Rh(t) .* W .* exp(-a * (h - 2 * t));
% Define the delay differential equation
dde = @(t,x,Z)manipulator_dynamics(t,x,Z,K_a_h);
sol = dde23(dde, h, history, [0 40]);
% Plot results
figure;
subplot(3,1,1);
plot(sol.x, sol.y(1,:));
xlabel('Time (s)');
ylabel('x1 (error in q)');
title('State x1 vs Time');
subplot(3,1,2);
plot(sol.x, sol.y(2,:));
xlabel('Time (s)');
ylabel('x2 (error in dq)');
title('State x2 vs Time');
subplot(3,1,3);
plot(sol.x, sol.y(3,:));
xlabel('Time (s)');
ylabel('x3 (error in I)');
title('State x3 vs Time');
% DDE function
function dxdt = manipulator_dynamics(t, x, Z, K_a_h)
x1_d = 0;
x2_d = 0;
x3_d = 0;
a = -0.15; % Example value
tau = 81/83; % Example value
h = 5; % Example value
J = 1.625e-3; % kg·m²
m = 0.506; % kg
L0 = 0.305; % m
R0 = 0.023; % m
B0 = 16.25e-3; % N·m·s/rad
L = 25e-3; % H
R = 5; % Ω
K_tau = 0.9; % N·m/A
G = 9.81; % m/s²
M0 = 0.434; % kg
% Derived parameters
M = J / K_tau + m * L0^2 / (3 * K_tau) + M0 * L0^2 / K_tau + 2 * M0 * R0^2 / (5 * K_tau);
N = m * L0 * G / (2 * K_tau) + M0 * L0 * G / K_tau;
B = B0 / K_tau;
K_B = 0.9;
Vp = 0.1 * sin(50 * pi *t);
z1 = x(1) - x1_d;
z2 = x(2) - x2_d;
z3 = x(3) - x3_d;
z3lag = Z(3,1) - x3_d;
% Control input
u = L * ((-a / (2 * (1 - tau))) * z3 + x3_d ...
+ R / L * x(3) + K_B / L * x(2) ...
- 5 * sign(z3) ...
- (K_a_h(t) / (2 * (1 - tau))) * (sig(z3 * (abs(z3lag)^(2 * (1 - tau)))))^(2 * tau - 1));
% Desired trajectory
q_d = (pi / 2) * sin(t) * (1 - exp(-0.1 * t^2));
% Dynamics
dxdt = [x(2);
(N / M) * sin(q_d) - (N / M) * sin(x(1) + q_d) - (B / M) * x(2) + (1 / M) * x(3);
-(R / L) * x(3) - (K_B / L) * x(2) + (1 / L) * u + Vp / L];
end
function sigx = sig(x)
tau = 81/83;
sigx = sign(x) .* abs(x).^(2 * tau - 1);
end