You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
116 lines
3.9 KiB
116 lines
3.9 KiB
#
|
|
# Copyright 2022 Philipp Schönberger - mail@phschoen.de
|
|
#
|
|
# This program is distributed in the hope that it will be useful,
|
|
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
# GNU General Public License for more details.
|
|
#
|
|
# See <http://www.gnu.org/licenses/>.
|
|
clearvars
|
|
clear
|
|
close all
|
|
|
|
|
|
finger_h = [ 10, 15, 18, 28 ];
|
|
finger_translate = [ 20, 25, 35 ];
|
|
finger_min_angles = [ -5, -5, -5 ];
|
|
finger_max_angles = [ 85, 90, 70 ];
|
|
finger_min_translate = [ 20];
|
|
finger_max_translate = [ 0];
|
|
|
|
|
|
switch_offset =20;
|
|
home_row = 1/3;
|
|
|
|
home_row_angles = finger_min_angles + home_row * (finger_max_angles - finger_min_angles);
|
|
home_row_trans = finger_min_translate + home_row * (finger_max_translate - finger_min_translate);
|
|
home_pos = calc_finger_pos(home_row_angles, home_row_trans, finger_translate);
|
|
|
|
X=[];
|
|
Y=[];
|
|
Z=[];
|
|
for i = [0:100]/100
|
|
angle = finger_min_angles + i * (finger_max_angles - finger_min_angles);
|
|
trans = finger_min_translate + i * (finger_max_translate - finger_min_translate);
|
|
pos = calc_finger_pos(angle,trans, finger_translate);
|
|
|
|
X = [X, pos(1)];
|
|
Y = [Y, pos(2)];
|
|
Z = [Z, pos(3)];
|
|
end
|
|
|
|
plot (Y,Z,"p");
|
|
i = [0:100]/100;
|
|
i_f = [0:3600]/3600;
|
|
lin_x = interp1 (i, X, i_f, "spline");
|
|
lin_y = interp1 (i, Y, i_f, "spline");
|
|
lin_z = interp1 (i, Z, i_f, "spline");
|
|
|
|
figure(1);
|
|
plot (Y, Z, "s", lin_y, lin_z, "r");
|
|
hold on;
|
|
plot (home_pos(2), home_pos(3), "markersize", 75);
|
|
|
|
axis equal
|
|
|
|
|
|
[row_p1,row_m1 ] = find_next_switch_pos(home_pos, X,Y,Z,switch_offset);
|
|
[row_p2,foo ] = find_next_switch_pos([X(row_p1),Y(row_p1),Z(row_p1)], X,Y,Z,switch_offset);
|
|
[row_p3,row_0 ] = find_next_switch_pos([X(row_p2),Y(row_p2),Z(row_p2)], X,Y,Z,switch_offset);
|
|
[foo, row_m2 ] = find_next_switch_pos([X(row_m1),Y(row_m1),Z(row_m1)], X,Y,Z,switch_offset);
|
|
[foo, row_m3 ] = find_next_switch_pos([X(row_m2),Y(row_m2),Z(row_m2)], X,Y,Z,switch_offset);
|
|
|
|
|
|
plot (Y(row_0), Z(row_0), "markersize", 50);
|
|
plot (Y(row_p1), Z(row_p1), "markersize", 50);
|
|
plot (Y(row_p2), Z(row_p2), "markersize", 50);
|
|
#plot (Y(row_p3), Z(row_p3), "markersize", 50);
|
|
|
|
plot (Y(row_m1), Z(row_m1), "markersize", 50);
|
|
plot (Y(row_m2), Z(row_m2), "markersize", 50);
|
|
#plot (Y(row_m3), Z(row_m3), "markersize", 50);
|
|
|
|
home_angle = calc_angle_switch(row_0,round(abs(row_0-row_m1)/3),X,Y,Z);
|
|
p1_angle = calc_angle_switch(row_p1,round(abs(row_p1-row_0)/3),X,Y,Z);
|
|
p2_angle = calc_angle_switch(row_p2,round(abs(row_p2-row_p1)/3),X,Y,Z);
|
|
p3_angle = calc_angle_switch(row_p3,round(abs(row_p3-row_p2)/3),X,Y,Z);
|
|
|
|
m1_angle = calc_angle_switch(row_m1,round(abs(row_m1-row_0)/3),X,Y,Z);
|
|
m2_angle = calc_angle_switch(row_m2,round(abs(row_m2-row_m1)/3),X,Y,Z);
|
|
m3_angle = calc_angle_switch(row_m3,round(abs(row_m3-row_m2)/3),X,Y,Z);
|
|
|
|
#[ foo, row_p1] = min(abs(switch_offset - D));
|
|
|
|
|
|
angle_switch_x = [p2_angle, p2_angle,home_angle,m1_angle,m2_angle];
|
|
pos_switch_x = [X(row_p2),X(row_p1),X(row_0),X(row_m1),X(row_m2)];
|
|
pos_switch_y = [Y(row_p2),Y(row_p1),Y(row_0),Y(row_m1),Y(row_m2)];
|
|
pos_switch_z = [Z(row_p2),Z(row_p1),Z(row_0),Z(row_m1),Z(row_m2)];
|
|
plot (Y(row_m1), Z(row_m1), "markersize", 50);
|
|
#plot (Y(row_p1), Z(row_p1), "markersize", 50);
|
|
% Noisy data
|
|
## x = linspace (0, 2*pi, 100);
|
|
## y = sin (x) + 0.1 * randn (size (x));
|
|
## % Breaks
|
|
## breaks = [0:5, 2*pi];
|
|
## % Constraints: y(0) = 0, y'(0) = 1 and y(3) + y"(3) = 0
|
|
## xc = [0 0 3];
|
|
## yc = [0 1 0];
|
|
## cc = [1 0 1; 0 1 0; 0 0 1];
|
|
## con = struct ("xc", xc, "yc", yc, "cc", cc);
|
|
## % Fit a cubic spline with 8 pieces and constraints
|
|
## pp = splinefit (x, y, 8, "constraints", con);
|
|
## clf;
|
|
## plot (x, y, "s", x, ppval (pp, x), "r", breaks, ppval (pp, breaks), "+r");
|
|
## xlabel ("Independent Variable");
|
|
## ylabel ("Dependent Variable");
|
|
## title ("Fit a cubic spline with constraints");
|
|
## legend ({"data", "fit", "breaks"});
|
|
## axis tight
|
|
## ylim auto
|
|
|
|
|
|
|
|
|
|
|