86 lines
2.0 KiB
Matlab
86 lines
2.0 KiB
Matlab
clear -all
|
|
close all
|
|
%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1; 5 3 1; 5 1 1;6 4 1; 6 2 1; 5 6 1];
|
|
conRobot = [1 2 1;
|
|
1 4 1;
|
|
1 5 1;
|
|
1 6 1;
|
|
2 3 1;
|
|
2 4 1;
|
|
3 4 1;
|
|
4 5 1;
|
|
5 6 1;];
|
|
%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1;];
|
|
|
|
%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1; -1; -1; 0; -1; ]; % xy xy xy
|
|
corRobot = [2; 2.5;
|
|
2.7; 2.6;
|
|
3; 2.3;
|
|
2.5; 1.5;
|
|
1.8; 1.5;
|
|
1.5; 2;
|
|
]; % xy xy xy
|
|
%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1;]; % xy xy xy
|
|
d = 3;
|
|
%% figure(1)
|
|
[R,K] = rigidityMatrixFnc(conRobot);
|
|
vref = [-5; 5];
|
|
B = [
|
|
1 0; 0 1;
|
|
0 0; 0 0;
|
|
0 0; 0 0;
|
|
0 0; 0 0;
|
|
0 0; 0 0;
|
|
0 0; 0 0;
|
|
];
|
|
kp = 2;
|
|
ki = 3;
|
|
|
|
sInit = [corRobot; zeros(size(R(corRobot,K),1),1)];
|
|
%% tspan = linspace(1,0.1,30);
|
|
tspan = 1:0.01:5;
|
|
dydt = @(t, y) systm_robot(t, y,d, R,K, kp, ki, B, vref);
|
|
|
|
[t,y] = ode45(dydt, tspan, sInit);
|
|
%% figure(2)
|
|
str_tmp = "plot(";
|
|
for i = 1:length(corRobot)-1
|
|
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),",i));
|
|
endfor
|
|
str_tmp = strcat(str_tmp,sprintf("y'(%i,:));",i+1));
|
|
eval(str_tmp);
|
|
|
|
hold on
|
|
str_tmp = "@(t) plot( [";
|
|
for i = 1:2:length(corRobot)
|
|
str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
|
|
endfor
|
|
str_tmp = strcat( str_tmp, sprintf("], ["));
|
|
for i = 2:2:length(corRobot)
|
|
str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
|
|
endfor
|
|
str_tmp = strcat( str_tmp, sprintf("], \"^\");"));
|
|
plot_rb = eval(str_tmp);
|
|
|
|
xrb = 1:2:length(corRobot);
|
|
yrb = 2:2:length(corRobot);
|
|
|
|
function plot_con (pltRb, yOut, conIn,xm,ym, time)
|
|
pltRb(time);
|
|
for i = 1:length(conIn)
|
|
plot([yOut(xm(conIn(i, 1)),time), yOut(xm(conIn(i, 2)),time)],
|
|
[yOut(ym(conIn(i,1)),time), yOut(ym(conIn(i,2)),time)], "r" )
|
|
endfor
|
|
endfunction
|
|
|
|
plot_con(plot_rb, y', conRobot, xrb, yrb, 1);
|
|
plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan));
|
|
|
|
%% plot_rb(20);
|
|
%% plot_rb(40);
|
|
%% plot_rb(60);
|
|
%% plot_rb(80);
|
|
%% plot_rb(100);
|
|
|
|
|