Skip to content
Snippets Groups Projects
Commit 19e9c483 authored by manxilin's avatar manxilin
Browse files

add spawn objects

parent ccb891f0
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 0 additions and 809 deletions
formerSrc/ex13-2 deleted 100644 → 0
+ 0
− 57
View file @ ccb891f0
%%%%%%%%%%%%%
%% Kalman localization
laser "localize"
laser "push t='1.0' cmd='localize'"
wait 1
%% set odometry
%set "$odox" 3.3
%set "$odoy" 0.35
%set "$odoth" 0
%% make a turn
turn 180
wait 1
%% make an array to store points
array "x" 10
array "y" 10
array "th" 10
x[0]=1.7
x[1]=0.2
x[2]=0.2
x[3]=0.95
x[4]=0.95
x[5]=0.2
x[6]=0.2
x[7]=1.7
x[8]=1.7
x[9]=0.2
y[0]=0.3
y[1]=0.3
y[2]=1.05
y[3]=1.05
y[4]=1.8
y[5]=1.8
y[6]=2.55
y[7]=2.55
y[8]=3.3
y[9]=3.3
th[0]=180
th[1]=90
th[2]=0
th[3]=90
th[4]=180
th[5]=90
th[6]=0
th[7]=90
th[8]=180
th[9]=180
%% run route
c = 0
label "loop"
xx = x[c]
yy = y[c]
t = th[c]
drivew xx yy t:($targetdist<0.25)
wait 3
c=c+1
eval c
if(c<10) "loop"
\ No newline at end of file
formerSrc/ex13-3-1 deleted 100644 → 0
+ 0
− 27
View file @ ccb891f0
% find route
laser "findroute startx=3.3 starty=0.3 endx=0.38 endy=3.38"
wait 1
%%%%%%%%%%%%%
%% Kalman localization
laser "localize"
laser "push t='1.0' cmd='localize'"
wait 5
%% set odometry
%set "$odox" 3.3
%set "$odoy" 0.35
%set "$odoth" 0
%% run route
c = $l4
eval c
label "loop"
stringcat "getpoint p="c
laser "$string"
wait 1
eval $l5
eval $l6
eval $l7
th = $l7/3.14159*180
drivew $l5 $l6 th:($targetdist<0.25)
wait 3
c=c-1
if (c>-1) "loop"
\ No newline at end of file
formerSrc/ex13-3-2 deleted 100644 → 0
+ 0
− 53
View file @ ccb891f0
%%%%%%%%%%%%%
%% Kalman localization
laser "localize"
laser "push t='1.0' cmd='localize'"
wait 3
%% set odometry
%set "$odox" 1.7
%set "$odoy" 0.3
%set "$odoth" 0
%%%%%%%%%%%
% find route
array "sx" 5
array "sy" 5
id = 0
sx[0]=1.7
sx[1]=0.2
sx[2]=3.2
sx[3]=0.2
sx[4]=1.7
sy[0]=0.3
sy[1]=0.3
sy[2]=0.3
sy[3]=1.05
sy[4]=0.3
label "bigloop"
startx=sx[id]
starty=sy[id]
endx=sx[id+1]
endy=sy[id+1]
eval startx
eval starty
eval endx
eval endy
stringcat "findroute startx="startx"starty="starty"endx="endx"endy="endy
laser "$string"
wait 1
%% run route
c = $l4
eval c
label "loop"
stringcat "getpoint p="c
laser "$string"
wait 1
eval $l5
eval $l6
eval $l7
th = $l7/3.14159*180
drivew $l5 $l6 th:($targetdist<0.25)
wait 5
c=c-1
if (c>-1) "loop"
id = id+1
if (id<4) "bigloop"
\ No newline at end of file
formerSrc/feedBackControl deleted 100644 → 0
+ 0
− 49
View file @ ccb891f0
% feed back control
%%%%%%%%%%%
xTw = 0.1
yTw = 0.1
thTw = 90/180*3.1415926535 %radian
kRho = 0.3
kAlpha = 0.8
kBeta = -0.35
epoch = 0
thT = $odoth-thTw
rho = 0
wl = 0
wr = 0
v = 0
w = 0
%log "thT" "rho" "wl" "wr" "v" "w"
%invtrans(xTw,yTw,thTw,$odox,$odoy,$odoth)
%%%%%%%%%%%%
log "$odox" "$odoy" "$odoth"
label "loop"
epoch = epoch+1
%%%%%%%%%%%%
xT = cos(thTw)*($odox-xTw)+sin(thTw)*($odoy-yTw)
yT = -sin(thTw)*($odox-xTw)+cos(thTw)*($odoy-yTw)
thT = $odoth-thTw
thT = normalizeanglerad(thT)
dx = -xT
dy = -yT
rho = sqrt(dx*dx+dy*dy)
e = atan2(dy, dx)
alpha = -thT+atan2(dy, dx)
beta = -thT-alpha
alpha = normalizeanglerad(alpha)
alpha = alpha
beta = normalizeanglerad(beta)
beta = beta
%%%%%%%%%%%%%%%%
v = kRho*rho
w = kAlpha*alpha+kBeta*beta
l = 0.26 % measured
wl = (2*v-w*l)/2
wr = (2*v+w*l)/2
motorcmds wl wr
if (v<0.005) "stop"
if (epoch>9999) "stop"
if(rho>0.01|abs($odoth-thTw)>0.02) "loop"
label "stop"
stop
formerSrc/feedBackControl-cl deleted 100644 → 0
+ 0
− 42
View file @ ccb891f0
%% feedback control
xTw = -0.5
yTw = -0.5
thTW = -180/180*3.1415926535897932384626
kRho = 0.3
kAlpha = 0.8
kBeta = -0.15
epoch = 0
invtrans(xTw,yTw,thTw,$odox,$odoy,$odoth)
log "$odox,$odoy,$odoth"
label "loop"
epoch = epoch+1
xT = cos(thTw)*($odox-xTw)+sin(thTw)*($odoy-yTw)
yT = -sin(thTw)*($odox-xTw)+cos(thTw)*($odoy-yTw)
thT = $odoth-thTw
dx = -xT
dy = -yT
rho = sqrt(dx*dx+dy*dy)
alpha = -thT+atan2(dy,dx)
beta = -thT-alpha
alpha = normalizeanglerad(alpha)
beta = normalizeanglerad(beta)
v = kRho*rho
if(v<0.05) "velocity"
label "continu"
w = kAlpha*alpha+kBeta*beta
l = 0.26 %measured
wr = (2*v-w*l)/2
wl = (2*v+w*l)/2
motorcmds wl wr
if(rho>0.01) "loop"
if(abs(thT)>0.02) "loop"
if(epoch==10000) "stop"
label "stop"
stop
label "velocity"
v = v
goto "continu"
formerSrc/feedBackControl-r deleted 100644 → 0
+ 0
− 63
View file @ ccb891f0
% feedback control
%% x,y and theta value for goal pose % given in question 8
xTw = 0
yTw = 0
thTw = 90*3.1415926535897932384626/180
% define in moveToTaget function in MATLAB
% feedback parameters given in q7
k_rho = 0.3
k_alpha = 0.8
k_beta = -0.35
epoch = 0
% coordinate transformation given in q10
invtrans(xTw,yTw,thTw,$odox,$odoy,$odoth)
% storing log values
log "$odox" "$odoy" "$odoth"
label "loop"
epoch = epoch+1
%
xT = cos(thTw)*($odox-xTw)+sin(thTw)*($odoy-yTw)
yT = -sin(thTw)*($odox-xTw)+cos(thTw)*($odoy-yTw)
thT = $odoth-thTw
thT = normalizeanglerad(thT)
dx = -xT
dy = -yT
%define in moveTOTarget function in MATLAB
rho = sqrt(dx*dx+dy*dy)
alpha = -thT+atan2(dy, dx)
beta = -thT-alpha
alpha = normalizeanglerad(alpha)
alpha = alpha%/3.1415926535897932384626*180
beta = normalizeanglerad(beta)
beta = beta%/3.1415926535897932384626*180
v = k_rho*rho
if(v<0.005) "velocity"
label "continue"
%define in moveToTaget function in MATLAB
w = k_alpha*alpha+k_beta*beta
% defined in robot2wheel function in MATLAB
l = 0.26 % measured
wl = (2*v-w*l)/2 % vl is speed of left wheel
wr = (2*v+w*l)/2 % vr is speed of right wheel
motorcmds wl wr % given in the q10
if(rho>0.01) "loop"
if(abs(thT)>0.02) "loop"
if (epoch==10000) "stop"
label "stop"
stop
label "velocity"
v = v
goto "continue"
formerSrc/guidemarks.cl deleted 100644 → 0
+ 0
− 49
View file @ ccb891f0
array "gmx" 15
array "gmy" 15
array "gmth" 15
pi=3.1416
gmx[1]=0
gmx[2]=0
gmx[3]=4
gmx[4]=4
gmx[5]=0
gmx[6]=0
gmx[7]=4
gmx[8]=4
gmx[9]=0
gmx[10]=4
gmx[11]=0.4
gmx[12]=3.6
gmx[13]=2
gmx[14]=2
gmy[1]=1.5
gmy[2]=1.5
gmy[3]=1.5
gmy[4]=1.5
gmy[5]=3.5
gmy[6]=3.5
gmy[7]=3.5
gmy[8]=3.5
gmy[9]=4.7
gmy[10]=4.7
gmy[11]=5
gmy[12]=5
gmy[13]=4.0
gmy[14]=4.0
gmth[1]=pi
gmth[2]=0
gmth[3]=pi
gmth[4]=0
gmth[5]=pi
gmth[6]=0
gmth[7]=pi
gmth[8]=0
gmth[9]=0
gmth[10]=pi
gmth[11]=-pi/2
gmth[12]=-pi/2
gmth[13]=pi
gmth[14]=0
formerSrc/laserl deleted 100644 → 0
+ 0
− 4
View file @ ccb891f0
drive @v 0.3 :($irdistfrontleft<0.45)
turn -90
stop
formerSrc/log deleted 100644 → 0
+ 0
− 279
View file @ ccb891f0
0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
0.000000 0.000000 0.387987 -0.387987 0.000000 -2.984513
0.000000 0.000000 0.387987 -0.387987 0.000000 -2.984513
0.000000 0.000000 0.387987 -0.387987 0.000000 -2.984513
0.000000 0.000000 0.387987 -0.387987 0.000000 -2.984513
-0.029553 0.000051 -0.195227 0.195257 0.000015 1.501861
-0.029553 0.000051 -0.195227 0.195257 0.000015 1.501861
-0.089447 0.000051 -0.201456 0.201486 0.000015 1.549776
-0.089447 0.000051 -0.201456 0.201486 0.000015 1.549776
-0.105208 0.000051 -0.203095 0.203125 0.000015 1.562386
-0.105208 0.000051 -0.203095 0.203125 0.000015 1.562386
-0.074473 0.000051 -0.199898 0.199929 0.000015 1.537798
-0.074473 0.000051 -0.199898 0.199929 0.000015 1.537798
-0.043738 0.000051 -0.196702 0.196733 0.000015 1.513210
-0.043738 0.000051 -0.196702 0.196733 0.000015 1.513210
-0.012609 0.000102 -0.193108 0.193170 0.000031 1.485686
-0.012609 0.000102 -0.193108 0.193170 0.000031 1.485686
0.018126 0.000102 -0.190861 0.190922 0.000031 1.468397
0.018126 0.000102 -0.190861 0.190922 0.000031 1.468397
0.048467 0.000051 -0.183321 0.183352 0.000015 1.410280
0.048467 0.000051 -0.183321 0.183352 0.000015 1.410280
0.077626 0.000051 -0.180289 0.180319 0.000015 1.386953
0.077626 0.000051 -0.180289 0.180319 0.000015 1.386953
0.105996 0.000051 -0.177338 0.177369 0.000015 1.364257
0.105996 0.000051 -0.177338 0.177369 0.000015 1.364257
0.133579 0.000051 -0.174469 0.174500 0.000015 1.342191
0.133579 0.000051 -0.174469 0.174500 0.000015 1.342191
0.161162 0.000051 -0.169935 0.169966 0.000015 1.307313
0.161162 0.000051 -0.169935 0.169966 0.000015 1.307313
0.187956 0.000051 -0.167149 0.167179 0.000015 1.285877
0.187956 0.000051 -0.167149 0.167179 0.000015 1.285877
0.213963 0.000051 -0.164444 0.164475 0.000015 1.265072
0.213963 0.000051 -0.164444 0.164475 0.000015 1.265072
0.240757 0.000051 -0.161657 0.161688 0.000015 1.243637
0.240757 0.000051 -0.161657 0.161688 0.000015 1.243637
0.265976 0.000051 -0.159035 0.159065 0.000015 1.223462
0.265976 0.000051 -0.159035 0.159065 0.000015 1.223462
0.290406 0.000051 -0.156494 0.156525 0.000015 1.203917
0.290406 0.000051 -0.156494 0.156525 0.000015 1.203917
0.314837 0.000051 -0.153953 0.153984 0.000015 1.184373
0.314837 0.000051 -0.153953 0.153984 0.000015 1.184373
0.339661 0.000101 -0.175549 0.175609 0.000030 1.350606
0.339661 0.000101 -0.175549 0.175609 0.000030 1.350606
0.363697 0.000052 -0.147493 0.147524 0.000015 1.134680
0.363697 0.000052 -0.147493 0.147524 0.000015 1.134680
0.388916 0.000052 -0.144870 0.144901 0.000015 1.114506
0.388916 0.000052 -0.144870 0.144901 0.000015 1.114506
0.414134 0.000052 -0.142248 0.142279 0.000015 1.094331
0.414134 0.000052 -0.142248 0.142279 0.000015 1.094331
0.436595 0.000100 -0.170724 0.170784 0.000030 1.313492
0.436595 0.000100 -0.170724 0.170784 0.000030 1.313492
0.458267 0.000052 -0.136524 0.136556 0.000016 1.050308
0.458267 0.000052 -0.136524 0.136556 0.000016 1.050308
0.481909 0.000052 -0.132844 0.132876 0.000016 1.022000
0.481909 0.000052 -0.132844 0.132876 0.000016 1.022000
0.505551 0.000052 -0.130386 0.130417 0.000016 1.003087
0.505551 0.000052 -0.130386 0.130417 0.000016 1.003087
0.526829 0.000052 -0.128173 0.128204 0.000016 0.986064
0.526829 0.000052 -0.128173 0.128204 0.000016 0.986064
0.546531 0.000052 -0.126124 0.126155 0.000016 0.970303
0.546531 0.000052 -0.126124 0.126155 0.000016 0.970303
0.566627 0.000098 -0.163800 0.163859 0.000029 1.260229
0.566627 0.000098 -0.163800 0.163859 0.000029 1.260229
0.586723 0.000053 -0.121004 0.121035 0.000016 0.930920
0.586723 0.000053 -0.121004 0.121035 0.000016 0.930920
0.608789 0.000053 -0.118709 0.118740 0.000016 0.913267
0.608789 0.000053 -0.118709 0.118740 0.000016 0.913267
0.630462 0.000097 -0.160471 0.160529 0.000029 1.234616
0.630462 0.000097 -0.160471 0.160529 0.000029 1.234616
0.648981 0.000053 -0.113719 0.113751 0.000016 0.874882
0.648981 0.000053 -0.113719 0.113751 0.000016 0.874882
0.670259 0.000053 -0.111506 0.111538 0.000016 0.857859
0.670259 0.000053 -0.111506 0.111538 0.000016 0.857859
0.691537 0.000053 -0.109293 0.109325 0.000016 0.840837
0.691537 0.000053 -0.109293 0.109325 0.000016 0.840837
0.708087 0.000053 -0.107572 0.107604 0.000016 0.827597
0.708087 0.000053 -0.107572 0.107604 0.000016 0.827597
0.725425 0.000053 -0.105769 0.105800 0.000016 0.813727
0.725425 0.000053 -0.105769 0.105800 0.000016 0.813727
0.741974 0.000053 -0.104048 0.104079 0.000016 0.800487
0.741974 0.000053 -0.104048 0.104079 0.000016 0.800487
0.758918 0.000095 -0.154372 0.154428 0.000028 1.187692
0.758918 0.000095 -0.154372 0.154428 0.000028 1.187692
0.775074 0.000053 -0.099965 0.099997 0.000016 0.769082
0.775074 0.000053 -0.099965 0.099997 0.000016 0.769082
0.793987 0.000053 -0.097998 0.098030 0.000016 0.753951
0.793987 0.000053 -0.097998 0.098030 0.000016 0.753951
0.813689 0.000053 -0.095949 0.095981 0.000016 0.738189
0.813689 0.000053 -0.095949 0.095981 0.000016 0.738189
0.828663 0.000053 -0.094391 0.094423 0.000016 0.726210
0.828663 0.000053 -0.094391 0.094423 0.000016 0.726210
0.844424 0.000053 -0.092752 0.092784 0.000016 0.713601
0.844424 0.000053 -0.092752 0.092784 0.000016 0.713601
0.858610 0.000053 -0.091277 0.091309 0.000016 0.702253
0.858610 0.000053 -0.091277 0.091309 0.000016 0.702253
0.872795 0.000053 -0.089802 0.089834 0.000016 0.690904
0.872795 0.000053 -0.089802 0.089834 0.000016 0.690904
0.886587 0.000092 -0.148341 0.148396 0.000028 1.141297
0.886587 0.000092 -0.148341 0.148396 0.000028 1.141297
0.900378 0.000053 -0.086502 0.086534 0.000016 0.665520
0.900378 0.000053 -0.086502 0.086534 0.000016 0.665520
0.918898 0.000091 -0.146376 0.146431 0.000027 1.126181
0.918898 0.000091 -0.146376 0.146431 0.000027 1.126181
0.937417 0.000054 -0.081961 0.081993 0.000016 0.630593
0.937417 0.000054 -0.081961 0.081993 0.000016 0.630593
0.955543 0.000054 -0.080076 0.080108 0.000016 0.616092
0.955543 0.000054 -0.080076 0.080108 0.000016 0.616092
0.973669 0.000054 -0.078191 0.078223 0.000016 0.601592
0.973669 0.000054 -0.078191 0.078223 0.000016 0.601592
0.985490 0.000054 -0.076961 0.076994 0.000016 0.592135
0.985490 0.000054 -0.076961 0.076994 0.000016 0.592135
0.998099 0.000054 -0.075650 0.075682 0.000016 0.582047
0.998099 0.000054 -0.075650 0.075682 0.000016 0.582047
1.010709 0.000054 -0.074339 0.074371 0.000016 0.571960
1.010709 0.000054 -0.074339 0.074371 0.000016 0.571960
1.022530 0.000054 -0.072818 0.072850 0.000016 0.560262
1.022530 0.000054 -0.072818 0.072850 0.000016 0.560262
1.034351 0.000054 -0.071588 0.071621 0.000016 0.550805
1.034351 0.000054 -0.071588 0.071621 0.000016 0.550805
1.045384 0.000054 -0.070441 0.070473 0.000016 0.541978
1.045384 0.000054 -0.070441 0.070473 0.000016 0.541978
1.055629 0.000054 -0.069375 0.069408 0.000016 0.533782
1.055629 0.000054 -0.069375 0.069408 0.000016 0.533782
1.066662 0.000054 -0.068228 0.068261 0.000016 0.524956
1.066662 0.000054 -0.068228 0.068261 0.000016 0.524956
1.077695 0.000054 -0.067081 0.067113 0.000016 0.516129
1.077695 0.000054 -0.067081 0.067113 0.000016 0.516129
1.087940 0.000054 -0.066015 0.066048 0.000016 0.507933
1.087940 0.000054 -0.066015 0.066048 0.000016 0.507933
1.098973 0.000054 -0.064868 0.064900 0.000016 0.499107
1.098973 0.000054 -0.064868 0.064900 0.000016 0.499107
1.110006 0.000054 -0.063720 0.063753 0.000016 0.490280
1.110006 0.000054 -0.063720 0.063753 0.000016 0.490280
1.119463 0.000054 -0.062737 0.062769 0.000016 0.482715
1.119463 0.000054 -0.062737 0.062769 0.000016 0.482715
1.128920 0.000054 -0.061753 0.061786 0.000016 0.475149
1.128920 0.000054 -0.061753 0.061786 0.000016 0.475149
1.138377 0.000054 -0.060770 0.060802 0.000016 0.467584
1.138377 0.000054 -0.060770 0.060802 0.000016 0.467584
1.147046 0.000054 -0.059868 0.059901 0.000016 0.460649
1.147046 0.000054 -0.059868 0.059901 0.000016 0.460649
1.156503 0.000054 -0.058885 0.058917 0.000016 0.453083
1.156503 0.000054 -0.058885 0.058917 0.000016 0.453083
1.165960 0.000054 -0.057901 0.057934 0.000016 0.445518
1.165960 0.000054 -0.057901 0.057934 0.000016 0.445518
1.175417 0.000054 -0.056917 0.056950 0.000016 0.437952
1.175417 0.000054 -0.056917 0.056950 0.000016 0.437952
1.184086 0.000054 -0.056016 0.056048 0.000016 0.431017
1.184086 0.000054 -0.056016 0.056048 0.000016 0.431017
1.193542 0.000054 -0.055032 0.055065 0.000016 0.423451
1.193542 0.000054 -0.055032 0.055065 0.000016 0.423451
1.202999 0.000054 -0.054049 0.054081 0.000016 0.415886
1.202999 0.000054 -0.054049 0.054081 0.000016 0.415886
1.211668 0.000054 -0.053147 0.053180 0.000016 0.408951
1.211668 0.000054 -0.053147 0.053180 0.000016 0.408951
1.220337 0.000054 -0.052246 0.052278 0.000016 0.402016
1.220337 0.000054 -0.052246 0.052278 0.000016 0.402016
1.228218 0.000054 -0.051426 0.051459 0.000016 0.395711
1.228218 0.000054 -0.051426 0.051459 0.000016 0.395711
1.236099 0.000054 -0.050607 0.050639 0.000016 0.389406
1.236099 0.000054 -0.050607 0.050639 0.000016 0.389406
1.243191 0.000054 -0.049869 0.049901 0.000016 0.383732
1.243191 0.000054 -0.049869 0.049901 0.000016 0.383732
1.251072 0.000054 -0.049049 0.049082 0.000016 0.377428
1.251072 0.000054 -0.049049 0.049082 0.000016 0.377428
1.258953 0.000054 -0.048230 0.048262 0.000016 0.371123
1.258953 0.000054 -0.048230 0.048262 0.000016 0.371123
1.266834 0.000054 -0.047410 0.047443 0.000016 0.364818
1.266834 0.000054 -0.047410 0.047443 0.000016 0.364818
1.274320 0.000081 -0.129646 0.129695 0.000024 0.997465
1.274320 0.000081 -0.129646 0.129695 0.000024 0.997465
1.281807 0.000054 -0.045780 0.045813 0.000016 0.352283
1.281807 0.000054 -0.045780 0.045813 0.000016 0.352283
1.295993 0.000054 -0.044305 0.044338 0.000016 0.340935
1.295993 0.000054 -0.044305 0.044338 0.000016 0.340935
1.309390 0.000055 -0.042799 0.042832 0.000016 0.329351
1.309390 0.000055 -0.042799 0.042832 0.000016 0.329351
1.316483 0.000055 -0.042062 0.042094 0.000016 0.323677
1.316483 0.000055 -0.042062 0.042094 0.000016 0.323677
1.322787 0.000055 -0.041406 0.041439 0.000016 0.318633
1.322787 0.000055 -0.041406 0.041439 0.000016 0.318633
1.329092 0.000055 -0.040750 0.040783 0.000016 0.313590
1.329092 0.000055 -0.040750 0.040783 0.000016 0.313590
1.335396 0.000055 -0.040095 0.040127 0.000016 0.308546
1.335396 0.000055 -0.040095 0.040127 0.000016 0.308546
1.340913 0.000055 -0.039486 0.039518 0.000016 0.303861
1.340913 0.000055 -0.039486 0.039518 0.000016 0.303861
1.347218 0.000055 -0.038830 0.038863 0.000016 0.298818
1.347218 0.000055 -0.038830 0.038863 0.000016 0.298818
1.353522 0.000055 -0.038174 0.038207 0.000016 0.293774
1.353522 0.000055 -0.038174 0.038207 0.000016 0.293774
1.359827 0.000055 -0.037518 0.037551 0.000016 0.288730
1.359827 0.000055 -0.037518 0.037551 0.000016 0.288730
1.366131 0.000055 -0.036863 0.036896 0.000016 0.283687
1.366131 0.000055 -0.036863 0.036896 0.000016 0.283687
1.371648 0.000055 -0.036264 0.036297 0.000017 0.279078
1.371648 0.000055 -0.036264 0.036297 0.000017 0.279078
1.377953 0.000055 -0.035608 0.035641 0.000017 0.274035
1.377953 0.000055 -0.035608 0.035641 0.000017 0.274035
1.384257 0.000055 -0.034952 0.034985 0.000017 0.268991
1.384257 0.000055 -0.034952 0.034985 0.000017 0.268991
1.390562 0.000055 -0.034297 0.034330 0.000017 0.263947
1.390562 0.000055 -0.034297 0.034330 0.000017 0.263947
1.396078 0.000055 -0.033723 0.033756 0.000017 0.259534
1.396078 0.000055 -0.033723 0.033756 0.000017 0.259534
1.400807 0.000055 -0.033231 0.033264 0.000017 0.255751
1.400807 0.000055 -0.033231 0.033264 0.000017 0.255751
1.404747 0.000055 -0.032811 0.032844 0.000017 0.252519
1.404747 0.000055 -0.032811 0.032844 0.000017 0.252519
1.409476 0.000055 -0.032319 0.032352 0.000017 0.248736
1.409476 0.000055 -0.032319 0.032352 0.000017 0.248736
1.414204 0.000055 -0.031827 0.031860 0.000017 0.244953
1.414204 0.000055 -0.031827 0.031860 0.000017 0.244953
1.418933 0.000055 -0.031336 0.031369 0.000017 0.241171
1.418933 0.000055 -0.031336 0.031369 0.000017 0.241171
1.423661 0.000055 -0.030844 0.030877 0.000017 0.237388
1.423661 0.000055 -0.030844 0.030877 0.000017 0.237388
1.428389 0.000055 -0.030352 0.030385 0.000017 0.233605
1.428389 0.000055 -0.030352 0.030385 0.000017 0.233605
1.433118 0.000055 -0.029860 0.029893 0.000017 0.229822
1.433118 0.000055 -0.029860 0.029893 0.000017 0.229822
1.437058 0.000055 -0.029447 0.029481 0.000017 0.226646
1.437058 0.000055 -0.029447 0.029481 0.000017 0.226646
1.441787 0.000055 -0.028956 0.028989 0.000017 0.222863
1.441787 0.000055 -0.028956 0.028989 0.000017 0.222863
1.446515 0.000055 -0.028464 0.028497 0.000017 0.219081
1.446515 0.000055 -0.028464 0.028497 0.000017 0.219081
1.451244 0.000055 -0.027972 0.028005 0.000017 0.215298
1.451244 0.000055 -0.027972 0.028005 0.000017 0.215298
1.455972 0.000055 -0.027480 0.027514 0.000017 0.211515
1.455972 0.000055 -0.027480 0.027514 0.000017 0.211515
1.460701 0.000055 -0.026989 0.027022 0.000017 0.207732
1.460701 0.000055 -0.026989 0.027022 0.000017 0.207732
1.465429 0.000055 -0.026497 0.026530 0.000017 0.203950
1.465429 0.000055 -0.026497 0.026530 0.000017 0.203950
1.469369 0.000055 -0.026091 0.026124 0.000017 0.200830
1.469369 0.000055 -0.026091 0.026124 0.000017 0.200830
1.474098 0.000055 -0.025599 0.025633 0.000017 0.197047
1.474098 0.000055 -0.025599 0.025633 0.000017 0.197047
1.478826 0.000055 -0.025108 0.025141 0.000017 0.193264
1.478826 0.000055 -0.025108 0.025141 0.000017 0.193264
1.483555 0.000055 -0.024616 0.024649 0.000017 0.189481
1.483555 0.000055 -0.024616 0.024649 0.000017 0.189481
1.488283 0.000055 -0.024124 0.024157 0.000017 0.185699
1.488283 0.000055 -0.024124 0.024157 0.000017 0.185699
1.492224 0.000055 -0.023714 0.023748 0.000017 0.182546
1.492224 0.000055 -0.023714 0.023748 0.000017 0.182546
1.495376 0.000055 -0.023387 0.023420 0.000017 0.180024
1.495376 0.000055 -0.023387 0.023420 0.000017 0.180024
1.498134 0.000074 -0.118078 0.118122 0.000022 0.908462
1.498134 0.000074 -0.118078 0.118122 0.000022 0.908462
1.500893 0.000055 -0.022819 0.022853 0.000017 0.175662
1.500893 0.000055 -0.022819 0.022853 0.000017 0.175662
1.511926 0.000055 -0.021672 0.021705 0.000017 0.166836
1.511926 0.000055 -0.021672 0.021705 0.000017 0.166836
1.522959 0.000055 -0.020525 0.020558 0.000017 0.158009
1.522959 0.000055 -0.020525 0.020558 0.000017 0.158009
1.526111 0.000055 -0.020197 0.020230 0.000017 0.155488
1.526111 0.000055 -0.020197 0.020230 0.000017 0.155488
1.529263 0.000055 -0.019869 0.019902 0.000017 0.152966
1.529263 0.000055 -0.019869 0.019902 0.000017 0.152966
1.531628 0.000055 -0.019634 0.019667 0.000017 0.151159
1.531628 0.000055 -0.019634 0.019667 0.000017 0.151159
1.534780 0.000055 -0.019306 0.019339 0.000017 0.148637
1.534780 0.000055 -0.019306 0.019339 0.000017 0.148637
1.537932 0.000055 -0.018978 0.019012 0.000017 0.146115
1.537932 0.000055 -0.018978 0.019012 0.000017 0.146115
1.541084 0.000055 -0.018650 0.018684 0.000017 0.143593
1.541084 0.000055 -0.018650 0.018684 0.000017 0.143593
1.544237 0.000055 -0.018323 0.018356 0.000017 0.141072
1.544237 0.000055 -0.018323 0.018356 0.000017 0.141072
1.547389 0.000055 -0.017995 0.018028 0.000017 0.138550
1.547389 0.000055 -0.017995 0.018028 0.000017 0.138550
1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
1.550541 0.000055 -0.017667 0.017700 0.000017 0.136028
formerSrc/point2point deleted 100644 → 0
+ 0
− 16
View file @ ccb891f0
%we need input (dx,dy,dt)
dx=-0.5
dy=-0.5
dt=-180
log "$odox" "$odoy" "$odoth"
arg=atan2(dy, dx)
%arg = normalizeangledeg(arg)
wait 0.5
dis=sqrt(dx*dx+dy*dy)
arg = arg/3.1415926535932384626*180
turn arg
fwd dis
stop
arg = -arg
turn arg
turn dt
formerSrc/point2point-cl deleted 100644 → 0
+ 0
− 14
View file @ ccb891f0
dx=0.5
dy=0
dtheta=0
log "$odox" "$odoy" "$odoth"
angle = atan2(dy,dx)
wait 0.5
dis = sqrt(dx*dx+dy*dy)
angle = agngle/3.1415926535932384626*180
turn angle
fwd dis
stop
angle = -angle
turn angle
turn dtheta
formerSrc/point2point-r deleted 100644 → 0
+ 0
− 20
View file @ ccb891f0
%q11 same as q8
dx=0.5
dy=-0.5
dtheta=-90 % orientation
% logging the odometry
log "$odox" "$odoy" "$odoth"
ang = atan2(dy, dx) % calculating the angle
ang = ang/3.1415926535932384626*180 % converting angle to radian
wait 3
dist=sqrt(dx*dx+dy*dy) % distance
turn ang
fwd dist
stop
ang = -ang
turn ang
turn dtheta
formerSrc/point2point.save deleted 100644 → 0
+ 0
− 16
View file @ ccb891f0
x%we need input (dx,dy,dt)
dx=0.5
dy=0.5
dt=0
log "$odox" "$odoy" "$odoth"
arg=atan2(dy, dx)
%arg = normalizeangledeg(arg)
wait 0.5
dis=sqrt(dx*dx+dy*dy)
arg = arg/3.1415926535932384626*180
turn arg
fwd dis
stop
arg = -arg
turn arg
turn dt
formerSrc/qrcode deleted 100644 → 0
+ 0
− 8
View file @ ccb891f0
get "guidemark" :($guidemarkok>1)
wait 5
switch($guidemarkok)
turn 10
case 2
fwd 0.5
stop
endswitch
formerSrc/ruchir deleted 100644 → 0
+ 0
− 9
View file @ ccb891f0
laser “scanpush cmd='zoneobst' “
turn 30
log "$I0" "I1" "I2" "I3" "I4" "I5" "I6" "I7" "I8"
wait 0.5
turn 30
wait 0.5
turn -10
wait 0.5
formerSrc/stop1 deleted 100644 → 0
+ 0
− 1
View file @ ccb891f0
stop
formerSrc/test0 deleted 100644 → 0
+ 0
− 13
View file @ ccb891f0
%followline "br" @v 0.2 :($drivendist>2.0)
%stop
%followline "bl" @v 0.2 :($drivendist>2.0)
%stop
%followline "bl" @v 0.2 :($crossingblackline)
%stop
drive @v 0.3:($irdistfrontleft<0.4)|($irdistfrontmiddle<0.4)|($irdistfrontright<0.4)
stop
formerSrc/test1 deleted 100644 → 0
+ 0
− 4
View file @ ccb891f0
fwd 1.5 @v 0.3
turn 180
fwd 1.5 @v 0.3
stop
formerSrc/test1-cl deleted 100644 → 0
+ 0
− 4
View file @ ccb891f0
fwd 1.5 @v 0.3
turn 180
fwd 1.5 @v 0.3
turn 180
formerSrc/test10 deleted 100644 → 0
+ 0
− 81
View file @ ccb891f0
drive @v 0.2 :($irdistfrontleft<0.45)|($irdistfrontmiddle<0.45)|($irdistfrontright<0.45)
stop
wait 3
switch ($fiducialid)
eval $fiducialid
stop
case 1 %%%%%
turn 180 @v 0.2
stop
fwd 1
turn -180 @v 0.2
followline "bl" @v 0.2 :($crossingblackline==1)
stop
fwd 0.25 @v 0.2
stop
turn -90 @v 0.2
%followline "bm" @v 0.1 :($drivendist>0.1)
drive @v 0.2 :($drivendist>0.1)
stop
case 2 %%%%%
turn 180 @v 0.2
stop
fwd 1
turn -180 @v 0.2
followline "bl" @v 0.2 :($crossingblackline==1)
stop
fwd 0.3 @v 0.2
followline "bm" @v 0.2 :($crossingblackline==1)
stop
%%%%%%%%%%
fwd 0.25 @v 0.2
stop
turn -90 @v 0.2
%followline "bm" @v 0.1 :($drivendist>0.1)
drive @v 0.2 :($drivendist>0.1)
stop
case 3 %%%%%
turn 180 @v 0.2
stop
fwd 1
turn -180 @v 0.2
followline "bl" @v 0.2 :($crossingblackline==1)
stop
fwd 0.3 @v 0.2
followline "bm" @v 0.2 :($crossingblackline==1)
stop
fwd 0.3 @v 0.2
followline "bm" @v 0.2 :($crossingblackline==1)
stop
%%%%%%%%%%
fwd 0.25 @v 0.2
stop
turn -90 @v 0.2
%followline "bm" @v 0.1 :($drivendist>0.1)
drive @v 0.2 :($drivendist>0.1)
stop
case 4 %%%%%
turn 180 @v 0.2
stop
fwd 1
turn -180 @v 0.2
followline "bl" @v 0.2 :($crossingblackline==1)
stop
fwd 0.3 @v 0.2
followline "bm" @v 0.2 :($crossingblackline==1)
stop
fwd 0.3 @v 0.2
followline "bm" @v 0.2 :($crossingblackline==1)
stop
fwd 0.3 @v 0.2
followline "bm" @v 0.2 :($crossingblackline==1)
stop
%%%%%%%%%%
fwd 0.25 @v 0.2
stop
turn -90 @v 0.2
%followline "bm" @v 0.1 :($drivendist>0.1)
drive @v 0.2 :($drivendist>0.1)
stop
endswitch
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment