19 lines
448 B
Matlab
19 lines
448 B
Matlab
clear -all
|
|
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; ]; % xy xy xy
|
|
d = 3;
|
|
figure(1)
|
|
[R,K] = rigidityMatrixFnc(conRobot);
|
|
vref = 5;
|
|
B = [1;1; 0;0; 0;0; 0; 0;];
|
|
kp = 2;
|
|
ki = 10;
|
|
|
|
sInit = [corRobot; 0;0;0;0;0;];
|
|
tspan = linspace(1,10);
|
|
dydt = @(t, y) systm_robot(t, y,d, R,K, kp, ki, B, vref);
|
|
|
|
[t,y] = ode45(dydt, tspan, sInit);
|
|
figure(2)
|
|
plot(y'(1,:),y'(2,:),y'(3,:),y'(4,:),y'(5,:),y'(6,:),y'(7,:),y'(8,:))
|