120 lines
2.1 KiB
Matlab
120 lines
2.1 KiB
Matlab
clear -all
|
|
|
|
graphics_toolkit('gnuplot')
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
|
|
|
|
bParam = struct();
|
|
bParam.model = omnidirectional(0.94, 0.96, 0.01, 2.2, 1.5,0.099,0.1,1.5,0.025,30,0.035,19,0.00011,1.69,0.0059,0.0059,0.9,9.1,6,'./');
|
|
bParam.edgeL = [1 2 1;3 2 1;3 1 1;];
|
|
bParam.dScale = 3;
|
|
bParam.nNodeVref = 1;
|
|
bParam.ki1 = 0;
|
|
bParam.ki2 = 0;
|
|
bparam.hStep = .01;
|
|
bParam.tspan = 1:bparam.hStep:20;
|
|
|
|
#### KOOR 1
|
|
bParam.corRobot = [0; 0;0;1; 2;0;-2; 3;0;]; % xy xy xy
|
|
## CASE 1
|
|
bParam.kp1 = 50;
|
|
bParam.kp2 = 3;
|
|
|
|
bP(1,1) = bParam;
|
|
|
|
## CASE 2
|
|
bParam.kp1 = 80;
|
|
bParam.kp2 = 7;
|
|
|
|
bP(1,2) = bParam;
|
|
|
|
# CASE 3
|
|
bParam.kp1 = 100;
|
|
bParam.kp2 = 15;
|
|
|
|
bP(1,3) = bParam;
|
|
|
|
## KOOR 2
|
|
bParam.corRobot = [0; 0;0;-2; -4;0;3; -2;0;]; % xy xy xy
|
|
## CASE 1
|
|
bParam.kp1 = 50;
|
|
bParam.kp2 = 3;
|
|
|
|
bP(2,1) = bParam;
|
|
|
|
## CASE 2
|
|
bParam.kp1 = 80;
|
|
bParam.kp2 = 7;
|
|
|
|
bP(2,2) = bParam;
|
|
|
|
# CASE 3
|
|
bParam.kp1 = 100;
|
|
bParam.kp2 = 15;
|
|
|
|
bP(2,3) = bParam;
|
|
|
|
## KOOR 3
|
|
bParam.corRobot = [0; 0;0;1; 2;0;3; -3;0;]; % xy xy xy
|
|
## CASE 1
|
|
bParam.kp1 = 50;
|
|
bParam.kp2 = 3;
|
|
|
|
bP(3,1) = bParam;
|
|
|
|
## CASE 2
|
|
bParam.kp1 = 80;
|
|
bParam.kp2 = 7;
|
|
|
|
bP(3,2) = bParam;
|
|
|
|
# CASE 3
|
|
bParam.kp1 = 100;
|
|
bParam.kp2 = 15;
|
|
|
|
bP(3,3) = bParam;
|
|
|
|
## KOOR 4
|
|
bParam.corRobot = [0; 0;0;-2; -3;0;3; 2;0;]; % xy xy xy
|
|
## CASE 1
|
|
bParam.kp1 = 50;
|
|
bParam.kp2 = 3;
|
|
|
|
bP(4,1) = bParam;
|
|
|
|
## CASE 2
|
|
bParam.kp1 = 80;
|
|
bParam.kp2 = 7;
|
|
|
|
bP(4,2) = bParam;
|
|
|
|
# CASE 3
|
|
bParam.kp1 = 100;
|
|
bParam.kp2 = 15;
|
|
|
|
bP(4,3) = bParam;
|
|
|
|
close all
|
|
|
|
for i = 1:size(bP,1)
|
|
[t,y,Dd,DdNorm] = run_my_simulation(bP(i,1));
|
|
figure
|
|
plot_con( y', bParam.edgeL, [1, length(bP(1,1).tspan)] ,bP(i,1));
|
|
title(sprintf("Motion dari Robot Case %i ", i ));
|
|
|
|
figure
|
|
resdd = struct();
|
|
resdd.DdNorm = DdNorm;
|
|
allresdd(1) = resdd;
|
|
for j = 2:size(bP,2)
|
|
[t,y,Dd,resdd.DdNorm] = run_my_simulation(bP(i,j));
|
|
allresdd(j) = resdd;
|
|
endfor
|
|
plot(t,allresdd(1).DdNorm,t,allresdd(2).DdNorm,t,allresdd(3).DdNorm);
|
|
title(sprintf("Norm Distance error Case %i ", i));
|
|
endfor
|
|
|
|
|
|
|
|
|
|
|