Scribd
Upload
32 views3 pages

Trajectory

The document outlines the parameters and simulation of a Cartesian Gantry Robot, including link lengths, initial and desired positions, and PID controller gains. It describes the trajectory generation using PID control over a specified simulation duration, updating the robot's position iteratively. Additionally, it includes a section for creating a 3D animation to visualize the robot's movement along the generated trajectory.

Uploaded by

ademasfaw222
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
32 views3 pages

Trajectory

The document outlines the parameters and simulation of a Cartesian Gantry Robot, including link lengths, initial and desired positions, and PID controller gains. It describes the trajectory generation using PID control over a specified simulation duration, updating the robot's position iteratively. Additionally, it includes a section for creating a 3D animation to visualize the robot's movement along the generated trajectory.

Uploaded by

ademasfaw222
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 3

% Cartesian Gantry Robot Parameters

L1 = 800; % Length of first link (mm)


L2 = 800; % Length of second link (mm)
L3 = 400; % Length of third link (mm)

% Initial and Desired Positions


x_init = 450; % Initial x-position (mm)
y_init = 400; % Initial y-position (mm)
z_init = 130; % Initial z-position (mm)

x_des = 1250; % Desired x-position (mm)


y_des = 1000; % Desired y-position (mm)
z_des = 140; % Desired z-position (mm)

% PID Controller Gains


Kp = [50 50 50]; % Proportional gains
Ki = [0.5 0.5 0.5]; % Integral gains
Kd = [5 5 5]; % Derivative gains

% Simulation Parameters
t_final = 5; % Simulation duration (s)
dt = 0.01; % Time step (s)
t = 0:dt:t_final;
steps = length(t);

% Initialize Trajectory and Error Vectors


x_traj = zeros(steps, 1);
y_traj = zeros(steps, 1);
z_traj = zeros(steps, 1);

x_err = zeros(steps, 1);


y_err = zeros(steps, 1);
z_err = zeros(steps, 1);

x_dot = zeros(steps, 1);


y_dot = zeros(steps, 1);
z_dot = zeros(steps, 1);

% Initialize Integral and Derivative Terms


x_int = 0;
y_int = 0;
z_int = 0;

x_prev = x_init;
y_prev = y_init;
z_prev = z_init;

% Generate Trajectory using PID Control


for i = 1:steps
% Calculate Position Errors
x_err(i) = x_des - x_init;
y_err(i) = y_des - y_init;
z_err(i) = z_des - z_init;

% Calculate Integral and Derivative Terms


x_int = x_int + x_err(i) * dt;
y_int = y_int + y_err(i) * dt;
z_int = z_int + z_err(i) * dt;

x_dot(i) = (x_init - x_prev) / dt;


y_dot(i) = (y_init - y_prev) / dt;
z_dot(i) = (z_init - z_prev) / dt;

% Calculate PID Control Outputs


x_u = Kp(1) * x_err(i) + Ki(1) * x_int - Kd(1) * x_dot(i);
y_u = Kp(2) * y_err(i) + Ki(2) * y_int - Kd(2) * y_dot(i);
z_u = Kp(3) * z_err(i) + Ki(3) * z_int - Kd(3) * z_dot(i);

% Update Robot Position


x_init = x_init + x_u * dt;
y_init = y_init + y_u * dt;
z_init = z_init + z_u * dt;

% Store Trajectory Points


x_traj(i) = x_init;
y_traj(i) = y_init;
z_traj(i) = z_init;

% Update Previous Values


x_prev = x_init;
y_prev = y_init;
z_prev = z_init;
end

% Create a 3D animation
figure;
axis equal;
grid on;
xlabel('X (mm)');
ylabel('Y (mm)');
zlabel('Z (mm)');
title('Cartesian Gantry Robot Trajectory Animation');

% Draw the robot at each time step


for i = 1:steps
% Clear the previous robot position
clf;

% Draw the robot at the current position


plot3([x_init, x_init+L1], [y_init, y_init], [z_init, z_init], 'r-');
hold on;
plot3([x_init+L1, x_init+L1], [y_init, y_init+L2], [z_init, z_init], 'g-');
plot3([x_init+L1, x_init+L1], [y_init+L2, y_init+L2], [z_init, z_init+L3], 'b-');
plot3(x_traj(1:i), y_traj(1:i), z_traj(1:i), 'k--');

% Set the view and limits


view(3);
xlim([0 2000]);
ylim([0 2000]);
zlim([0 800]);

% Pause to create the animation


pause(0.01);
end

You might also like