35 lines
1.6 KiB
Matlab
35 lines
1.6 KiB
Matlab
%% This script creates a CATIA Law file with the current points.
|
|
|
|
% Here you can specify the points to be reached:
|
|
% points = [time, X, Y, Z, OMEGA, CHI, PHI; ...]
|
|
points = [ 2, 188e-3, 0, 0, 0, 0, 0;...
|
|
4, 188e-3, 10e-3, 0, 0, 0, 0;...
|
|
6, 188e-3, 10e-3, 10e-3, 0, 0, 0;...
|
|
8, 188e-3,-10e-3, 10e-3, 0, 0, 0;...
|
|
10, 188e-3,-10e-3,-10e-3, 0, 0, 0;...
|
|
12, 188e-3, 10e-3, 0, 10e-3, 0, 0;...
|
|
14, 188e-3, 0, 0, 0, 20, 0;...
|
|
15, 188e-3, 0, 0, 0, 40, 0;...
|
|
16, 188e-3, 0, 0, 0, 50, 0;...
|
|
17, 188e-3, 0, 0, 0, 60, 0;...
|
|
18, 188e-3, 0, 0, 0, 70, 0;...
|
|
19, 188e-3, 0, 0, 0, 80, 0;...
|
|
10, 188e-3, 0, 0, 0, 90, 0];
|
|
|
|
number_of_points = size(points,1)
|
|
|
|
%% Write CATIA Law file
|
|
fid = fopen('smargon_laws.txt','wt');
|
|
fprintf(fid, '// Law for Befehl.1 bis Befehl.6\n');
|
|
fprintf(fid, '---------------------------------\n');
|
|
fprintf(fid, '*COLUMNS = *TIME, Befehl.1, Befehl.2, Befehl.3, Befehl.4, Befehl.5, Befehl.6\n');
|
|
fprintf(fid, '*INTERPOLATION = polyline,spline,polyline,polyline,polyline,polyline\n');
|
|
fprintf(fid, '*UNIT = Deg,m,m,m,m,Deg\n');
|
|
fprintf(fid, '*YSCALE = 1,1,1,1,1,1\n');
|
|
for i = 1:number_of_points
|
|
t=points(i,1);
|
|
[b1,b2,b3,b4,b5,b6] = calcIK_CATIA(points(i,2),points(i,3),points(i,4),points(i,5),points(i,6),points(i,7));
|
|
fprintf(fid, '%d\t%f\t%f\t%f\t%f\t%f\t%f\n', t,b1,b2,b3,b4,b5,b6);
|
|
end;
|
|
|
|
fclose(fid); |