diff --git a/kinematics/matlab-old/demonstration_main.m b/kinematics/matlab-old/demonstration_main.m deleted file mode 100644 index 49f9391..0000000 --- a/kinematics/matlab-old/demonstration_main.m +++ /dev/null @@ -1,14 +0,0 @@ -%% -clear; close all; clc; - -%% Demonstration of stroke of each stage -% Initalize data for demonstration -run displacement_init.m - -% Run the simulation -run displacement_sim.m - -%% Test the measurement of sample position -run sample_pos_init.m - -run sample_pos_sim.m \ No newline at end of file diff --git a/kinematics/matlab-old/displacement_init.m b/kinematics/matlab-old/displacement_init.m deleted file mode 100644 index 84c4c1a..0000000 --- a/kinematics/matlab-old/displacement_init.m +++ /dev/null @@ -1,98 +0,0 @@ -%% -clear; close all; clc; - -%% Initialize simulation configuration -opts_sim = struct(... - 'Tsim', 30 ... -); - -initializeSimConf(opts_sim); - -%% Initialize Inputs -load('./mat/sim_conf.mat', 'sim_conf') - -time_vector = 0:sim_conf.Ts:sim_conf.Tsim; - -% Translation Stage -T_ty = 4; -ty = zeros(length(time_vector), 1); -ty(1:T_ty/sim_conf.Ts) = 10e-3*sin(2*pi*(1/2)*time_vector(1:T_ty/sim_conf.Ts)); - -% Tilt Stage -T_ry = 4; -ry = zeros(length(time_vector), 1); -ry((T_ty)/sim_conf.Ts:(T_ty+T_ry)/sim_conf.Ts) = 2*pi*(3/360)*sin(2*pi*(1/2)*time_vector(T_ty/sim_conf.Ts:(T_ty+T_ry)/sim_conf.Ts)); - -% Spindle -T_rz = 4; - -rz = zeros(length(time_vector), 1); -rz((T_ty+T_ry)/sim_conf.Ts:(T_ty+T_ry+T_rz)/sim_conf.Ts) = 2*pi*0.5*(time_vector((T_ty+T_ry)/sim_conf.Ts:(T_ty+T_ry+T_rz)/sim_conf.Ts)-time_vector((T_ty+T_ry)/sim_conf.Ts)); -rz((T_ty+T_ry+T_rz)/sim_conf.Ts:end) = rz((T_ty+T_ry+T_rz)/sim_conf.Ts); - -% Micro Hexapod -T_u_hexa = 10; -u_hexa = zeros(length(time_vector), 6); -% Tz -u_hexa((T_ty+T_ry+T_rz)/sim_conf.Ts:(T_ty+T_ry+T_rz+2)/sim_conf.Ts, 3) = 10e-3*sin(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz)/sim_conf.Ts:(T_ty+T_ry+T_rz+2)/sim_conf.Ts))); -% Tx-Ty -u_hexa((T_ty+T_ry+T_rz+2)/sim_conf.Ts:(T_ty+T_ry+T_rz+3)/sim_conf.Ts, 1) = 10e-3*(time_vector((T_ty+T_ry+T_rz+2)/sim_conf.Ts:(T_ty+T_ry+T_rz+3)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+2)/sim_conf.Ts)); -u_hexa((T_ty+T_ry+T_rz+3)/sim_conf.Ts:(T_ty+T_ry+T_rz+5)/sim_conf.Ts, 1) = 10e-3*cos(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz+3)/sim_conf.Ts:(T_ty+T_ry+T_rz+5)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+3)/sim_conf.Ts))); -u_hexa((T_ty+T_ry+T_rz+3)/sim_conf.Ts:(T_ty+T_ry+T_rz+5)/sim_conf.Ts, 2) = 10e-3*sin(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz+3)/sim_conf.Ts:(T_ty+T_ry+T_rz+5)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+3)/sim_conf.Ts))); -u_hexa((T_ty+T_ry+T_rz+5)/sim_conf.Ts:(T_ty+T_ry+T_rz+6)/sim_conf.Ts, 1) = 10e-3 - 10e-3*(time_vector((T_ty+T_ry+T_rz+5)/sim_conf.Ts:(T_ty+T_ry+T_rz+6)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+5)/sim_conf.Ts)); -% Theta x Theta y -u_hexa((T_ty+T_ry+T_rz+6)/sim_conf.Ts:(T_ty+T_ry+T_rz+7)/sim_conf.Ts, 1) = 2*pi*(3/360)*(time_vector((T_ty+T_ry+T_rz+6)/sim_conf.Ts:(T_ty+T_ry+T_rz+7)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+6)/sim_conf.Ts)); -u_hexa((T_ty+T_ry+T_rz+7)/sim_conf.Ts:(T_ty+T_ry+T_rz+9)/sim_conf.Ts, 1) = 2*pi*(3/360)*cos(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz+7)/sim_conf.Ts:(T_ty+T_ry+T_rz+9)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+7)/sim_conf.Ts))); -u_hexa((T_ty+T_ry+T_rz+7)/sim_conf.Ts:(T_ty+T_ry+T_rz+9)/sim_conf.Ts, 2) = 2*pi*(3/360)*sin(2*pi*(1/2)*(time_vector((T_ty+T_ry+T_rz+7)/sim_conf.Ts:(T_ty+T_ry+T_rz+9)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+7)/sim_conf.Ts))); -u_hexa((T_ty+T_ry+T_rz+9)/sim_conf.Ts:(T_ty+T_ry+T_rz+10)/sim_conf.Ts, 1) = 2*pi*(3/360) - 2*pi*(3/360)*(time_vector((T_ty+T_ry+T_rz+9)/sim_conf.Ts:(T_ty+T_ry+T_rz+10)/sim_conf.Ts)-time_vector((T_ty+T_ry+T_rz+9)/sim_conf.Ts)); - -% Gravity Compensator system -T_mass_start = T_ty+T_ry+T_rz+T_u_hexa; -mass = zeros(length(time_vector), 2); - -mass((T_mass_start)/sim_conf.Ts:(T_mass_start+2)/sim_conf.Ts, 1) = 2*pi*( 20/360)*(time_vector((T_mass_start)/sim_conf.Ts:(T_mass_start+2)/sim_conf.Ts)-time_vector(T_mass_start/sim_conf.Ts)); -mass((T_mass_start)/sim_conf.Ts:(T_mass_start+2)/sim_conf.Ts, 2) = 2*pi*(-10/360)*(time_vector((T_mass_start)/sim_conf.Ts:(T_mass_start+2)/sim_conf.Ts)-time_vector(T_mass_start/sim_conf.Ts)); -mass((T_mass_start+2)/sim_conf.Ts:(T_mass_start+3)/sim_conf.Ts, 1) = mass((T_mass_start+2)/sim_conf.Ts, 1); -mass((T_mass_start+2)/sim_conf.Ts:(T_mass_start+3)/sim_conf.Ts, 2) = mass((T_mass_start+2)/sim_conf.Ts, 2); -mass((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts, 1) = mass((T_mass_start+2)/sim_conf.Ts, 1)-2*pi*( 20/360)*(time_vector((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts)-time_vector((T_mass_start+3)/sim_conf.Ts)); -mass((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts, 2) = mass((T_mass_start+2)/sim_conf.Ts, 2)-2*pi*(-10/360)*(time_vector((T_mass_start+3)/sim_conf.Ts:(T_mass_start+5)/sim_conf.Ts)-time_vector((T_mass_start+3)/sim_conf.Ts)); - -opts_inputs = struct(... - 'Dy', ty, ... - 'Ry', ry, ... - 'Rz', rz, ... - 'Dh', u_hexa, ... - 'Rm', mass ... -); - -initializeInputs(opts_inputs); - -%% Initialize SolidWorks Data -initializeSolids(); - -%% Initialize Ground -initializeGround(); - -%% Initialize Granite -initializeGranite(); - -%% Initialize Translation stage -initializeTy(); - -%% Initialize Tilt Stage -initializeRy(); - -%% Initialize Spindle -initializeRz(); - -%% Initialize Hexapod Symétrie -initializeMicroHexapod(); - -%% Initialize Center of Gravity compensation -initializeAxisc(); - -%% Initialize NASS -initializeNanoHexapod(struct('actuator', 'lorentz')); - -%% Initialize Sample -initializeSample(struct('mass', 20)); diff --git a/kinematics/matlab-old/displacement_sim.m b/kinematics/matlab-old/displacement_sim.m deleted file mode 100644 index 9ab982f..0000000 --- a/kinematics/matlab-old/displacement_sim.m +++ /dev/null @@ -1,5 +0,0 @@ -%% -clear; close all; clc; - -%% -sim('sim_nano_station_disp.slx'); diff --git a/kinematics/matlab-old/error_NASS.m b/kinematics/matlab-old/error_NASS.m deleted file mode 100644 index 6ef8d1c..0000000 --- a/kinematics/matlab-old/error_NASS.m +++ /dev/null @@ -1,50 +0,0 @@ -%% Plot all 6 errors expressed in the NASS base -figure; - -%% Tx -subplot(2, 3, 1); -hold on; -plot(error_nass.Time, error_nass.Data(:, 1), 'k-', 'DisplayName', '$\epsilon_x$'); -legend(); -hold off; -xlabel('Time (s)'); ylabel('Position (m)'); - -%% Ty -subplot(2, 3, 2); -hold on; -plot(error_nass.Time, error_nass.Data(:, 2), 'k-', 'DisplayName', '$\epsilon_y$'); -legend(); -hold off; -xlabel('Time (s)'); ylabel('Position (m)'); - -%% Tz -subplot(2, 3, 3); -hold on; -plot(error_nass.Time, error_nass.Data(:, 3), 'k-', 'DisplayName', '$\epsilon_z$'); -legend(); -hold off; -xlabel('Time (s)'); ylabel('Position (m)'); - -%% Rx -subplot(2, 3, 4); -hold on; -plot(error_nass.Time, error_nass.Data(:, 4), 'k-', 'DisplayName', '$\epsilon_{\theta_x}$'); -legend(); -hold off; -xlabel('Time (s)'); ylabel('Rotation (rad)'); - -%% Ry -subplot(2, 3, 5); -hold on; -plot(error_nass.Time, error_nass.Data(:, 5), 'k-', 'DisplayName', '$\epsilon_{\theta_y}$'); -legend(); -hold off; -xlabel('Time (s)'); ylabel('Rotation (rad)'); - -%% Rz -subplot(2, 3, 6); -hold on; -plot(error_nass.Time, error_nass.Data(:, 6), 'k-', 'DisplayName', '$\epsilon_{\theta_z}$'); -legend(); -hold off; -xlabel('Time (s)'); ylabel('Rotation (rad)'); diff --git a/kinematics/matlab-old/sample_pos_init.m b/kinematics/matlab-old/sample_pos_init.m deleted file mode 100644 index a4d1bab..0000000 --- a/kinematics/matlab-old/sample_pos_init.m +++ /dev/null @@ -1,69 +0,0 @@ -%% -clear; close all; clc; - -%% Initialize simulation configuration -opts_sim = struct(... - 'Tsim', 0.001 ... -); - -initializeSimConf(opts_sim); - -%% Initialize Inputs -load('./mat/sim_conf.mat', 'sim_conf') - -time_vector = 0:sim_conf.Ts:sim_conf.Tsim; - -% Translation Stage -Ty = 0*ones(length(time_vector), 1); - -% Tilt Stage -Ry = 2*pi*(0/360)*ones(length(time_vector), 1); -% Ry = 2*pi*(3/360)*sin(2*pi*time_vector); - -% Spindle -Rz = 2*pi*0*(time_vector); -% Rz = 2*pi*(190/360)*ones(length(time_vector), 1); - -% Micro Hexapod -Dh = zeros(length(time_vector), 6); - -% Gravity Compensator system -Dm = zeros(length(time_vector), 2); -Dm(:, 2) = pi; - -opts_inputs = struct(... - 'Ty', Ty, ... - 'Ry', Ry, ... - 'Rz', Rz, ... - 'Dh', Dh, ... - 'Dm', Dm ... -); - -initializeInputs(opts_inputs); - -%% Initialize Ground -initializeGround(); - -%% Initialize Granite -initializeGranite(); - -%% Initialize Translation stage -initializeTy(); - -%% Initialize Tilt Stage -initializeRy(); - -%% Initialize Spindle -initializeRz(); - -%% Initialize Hexapod Symétrie -initializeMicroHexapod(); - -%% Initialize Center of Gravity compensation -initializeAxisc(); - -%% Initialize NASS -initializeNanoHexapod(struct('actuator', 'piezo')); - -%% Initialize Sample -initializeSample(struct('mass', 20)); diff --git a/kinematics/matlab-old/sample_pos_sim.m b/kinematics/matlab-old/sample_pos_sim.m deleted file mode 100644 index 9ab982f..0000000 --- a/kinematics/matlab-old/sample_pos_sim.m +++ /dev/null @@ -1,5 +0,0 @@ -%% -clear; close all; clc; - -%% -sim('sim_nano_station_disp.slx'); diff --git a/kinematics/matlab-old/setpoint_vs_position.m b/kinematics/matlab-old/setpoint_vs_position.m deleted file mode 100644 index b139170..0000000 --- a/kinematics/matlab-old/setpoint_vs_position.m +++ /dev/null @@ -1,56 +0,0 @@ -%% -figure; - -%% Tx -subplot(2, 3, 1); -hold on; -plot(pos.Time, pos.Data(:, 1), 'k-'); -plot(setpoint.Time, setpoint.Data(:, 1), 'k--'); -legend({'x - pos', 'x - setpoint'}); -hold off; -xlabel('Time (s)'); ylabel('Position (m)'); - -%% Ty -subplot(2, 3, 2); -hold on; -plot(pos.Time, pos.Data(:, 2), 'k-'); -plot(setpoint.Time, setpoint.Data(:, 2), 'k--'); -legend({'y - pos', 'y - setpoint'}); -hold off; -xlabel('Time (s)'); ylabel('Position (m)'); - -%% Tz -subplot(2, 3, 3); -hold on; -plot(pos.Time, pos.Data(:, 3), 'k-'); -plot(setpoint.Time, setpoint.Data(:, 3), 'k--'); -legend({'z - pos', 'z - setpoint'}); -hold off; -xlabel('Time (s)'); ylabel('Position (m)'); - -%% Rx -subplot(2, 3, 4); -hold on; -plot(pos.Time, pos.Data(:, 4), 'k-'); -plot(setpoint.Time, setpoint.Data(:, 4), 'k--'); -legend({'$\theta_x$ - pos', '$\theta_x$ - setpoint'}); -hold off; -xlabel('Time (s)'); ylabel('Rotation (rad)'); - -%% Ry -subplot(2, 3, 5); -hold on; -plot(pos.Time, pos.Data(:, 5), 'k-'); -plot(setpoint.Time, setpoint.Data(:, 5), 'k--'); -legend({'$\theta_y$ - pos', '$\theta_y$ - setpoint'}); -hold off; -xlabel('Time (s)'); ylabel('Rotation (rad)'); - -%% Rz -subplot(2, 3, 6); -hold on; -plot(pos.Time, pos.Data(:, 6), 'k-'); -plot(setpoint.Time, setpoint.Data(:, 6), 'k--'); -legend({'$\theta_z$ - pos', '$\theta_z$ - setpoint'}); -hold off; -xlabel('Time (s)'); ylabel('Rotation (rad)'); diff --git a/kinematics/matlab-old/test2.m b/kinematics/matlab-old/test2.m deleted file mode 100644 index 6dad2b9..0000000 --- a/kinematics/matlab-old/test2.m +++ /dev/null @@ -1,71 +0,0 @@ -%% Compute position angle from R and Q -thetas_R = zeros(length(pos.Time), 3); -thetas_Q = zeros(length(pos.Time), 3); - -for i = 1:length(pos.Time) - [thetax, thetay, thetaz] = RM2angle(R.Data(:, :, i)); - thetas_R(i, 1) = thetax; thetas_R(i, 2) = thetay; thetas_R(i, 3) = thetaz; - - [thetax, thetay, thetaz] = quaternionToEulerAngles(Q.Data(i, :)); - thetas_Q(i, 1) = thetax; thetas_Q(i, 2) = thetay; thetas_Q(i, 3) = thetaz; -end - -%% Compute setpoint -setpoint_c = zeros(length(pos.Time), 6); - -for i = 1:length(pos.Time) - setpoint_c(i, :) = computeSetpoint(ty.Data(i), ry.Data(i), rz.Data(i)); -end - -%% -figure; -hold on; -plot(pos.Time, pos.Data(:, 1), 'k-', 'DisplayName', 'position'); -plot(pos.Time, setpoint_c(:, 1), '--', 'DisplayName', 'Computed setpoint'); -hold off; -legend(); -xlabel('Time (s)'); ylabel('Translation (m)'); - -%% -figure; -hold on; -plot(pos.Time, pos.Data(:, 2), 'k-', 'DisplayName', 'position'); -plot(pos.Time, setpoint_c(:, 2), '--', 'DisplayName', 'Computed setpoint'); -hold off; -legend(); -xlabel('Time (s)'); ylabel('Translation (m)'); -%% -figure; -hold on; -plot(pos.Time, pos.Data(:, 3), 'k-', 'DisplayName', 'position'); -plot(pos.Time, setpoint_c(:, 3), '--', 'DisplayName', 'Computed setpoint'); -hold off; -legend(); -xlabel('Time (s)'); ylabel('Translation (m)'); - -%% -figure; -hold on; -plot(pos.Time, pos.Data(:, 4), 'k-', 'DisplayName', 'position'); -plot(pos.Time, setpoint_c(:, 4), '--', 'DisplayName', 'Computed setpoint'); -hold off; -legend(); -xlabel('Time (s)'); ylabel('Rotation (rad)'); - -%% -figure; -hold on; -plot(pos.Time, pos.Data(:, 5), 'k-', 'DisplayName', 'position'); -plot(pos.Time, setpoint_c(:, 5), '--', 'DisplayName', 'Computed setpoint'); -hold off; -legend(); -xlabel('Time (s)'); ylabel('Rotation (rad)'); - -%% -figure; -hold on; -plot(pos.Time, pos.Data(:, 6), 'k-', 'DisplayName', 'position'); -plot(pos.Time, setpoint_c(:, 6), '--', 'DisplayName', 'Computed setpoint'); -hold off; -legend(); -xlabel('Time (s)'); ylabel('Rotation (rad)'); \ No newline at end of file