From d0148e72a5ae61b5af2f3563fd27b9a7b62f7dd0 Mon Sep 17 00:00:00 2001 From: Jonas Natzer Date: Wed, 6 Dec 2017 19:32:41 +0100 Subject: [PATCH] improved values of pid controllers --- setup_linux.sh | 2 +- src/drivers/Makefile | 6 +- src/drivers/pid_driver/driver.cpp | 18 +- src/drivers/pid_driver/pidSteer.cpp | 2 +- src/drivers/test_bot/155-DTM.rgb | Bin 0 -> 117412 bytes src/drivers/test_bot/Makefile | 29 + src/drivers/test_bot/TrackData.cpp | 103 +++ src/drivers/test_bot/TrackData.h | 203 +++++ src/drivers/test_bot/Trajectory.cpp | 317 +++++++ src/drivers/test_bot/Trajectory.h | 47 ++ src/drivers/test_bot/build_bot | 9 + src/drivers/test_bot/logo.rgb | Bin 0 -> 18550 bytes src/drivers/test_bot/make_clean | 7 + src/drivers/test_bot/optimal_line.cpp | 1091 +++++++++++++++++++++++++ src/drivers/test_bot/optimal_line.h | 142 ++++ src/drivers/test_bot/test_bot.cpp | 577 +++++++++++++ src/drivers/test_bot/test_bot.def | 19 + src/drivers/test_bot/test_bot.dsp | 238 ++++++ src/drivers/test_bot/test_bot.h | 91 +++ src/drivers/test_bot/test_bot.xml | 35 + src/libs/txml/gennmtab/gennmtab | Bin 0 -> 21808 bytes 21 files changed, 2922 insertions(+), 14 deletions(-) create mode 100644 src/drivers/test_bot/155-DTM.rgb create mode 100644 src/drivers/test_bot/Makefile create mode 100644 src/drivers/test_bot/TrackData.cpp create mode 100644 src/drivers/test_bot/TrackData.h create mode 100644 src/drivers/test_bot/Trajectory.cpp create mode 100644 src/drivers/test_bot/Trajectory.h create mode 100755 src/drivers/test_bot/build_bot create mode 100644 src/drivers/test_bot/logo.rgb create mode 100755 src/drivers/test_bot/make_clean create mode 100644 src/drivers/test_bot/optimal_line.cpp create mode 100644 src/drivers/test_bot/optimal_line.h create mode 100644 src/drivers/test_bot/test_bot.cpp create mode 100644 src/drivers/test_bot/test_bot.def create mode 100644 src/drivers/test_bot/test_bot.dsp create mode 100644 src/drivers/test_bot/test_bot.h create mode 100644 src/drivers/test_bot/test_bot.xml create mode 100755 src/libs/txml/gennmtab/gennmtab diff --git a/setup_linux.sh b/setup_linux.sh index baab8da..824b37e 100755 --- a/setup_linux.sh +++ b/setup_linux.sh @@ -1,7 +1,7 @@ #! /bin/bash [ -z "$1" ] && exit 1 -[ ! -d "$1" ] && exit 1 + mkdir -p $1/drivers/human 2>/dev/null if [ ! -e $1/drivers/human/car.xml ] || [ drivers/human/car.xml -nt $1/drivers/human/car.xml ] then diff --git a/src/drivers/Makefile b/src/drivers/Makefile index a17cc23..834239b 100644 --- a/src/drivers/Makefile +++ b/src/drivers/Makefile @@ -2,9 +2,9 @@ # # file : Makefile # created : Sat Mar 18 23:08:21 CET 2000 -# copyright : (C) 2000 by Eric Espie -# email : torcs@free.fr -# version : $Id: Makefile,v 1.4 2002/06/30 14:11:14 torcs Exp $ +# copyright : (C) 2000 by Eric Espie +# email : torcs@free.fr +# version : $Id: Makefile,v 1.4 2002/06/30 14:11:14 torcs Exp $ # ############################################################################## # diff --git a/src/drivers/pid_driver/driver.cpp b/src/drivers/pid_driver/driver.cpp index a1f91bc..af25807 100644 --- a/src/drivers/pid_driver/driver.cpp +++ b/src/drivers/pid_driver/driver.cpp @@ -17,20 +17,20 @@ const float Driver::SHIFT_MARGIN = 4.0; /* [m/s] */ const float Driver::ABS_SLIP = 0.9; /* [-] range [0.95..0.3] */ const float Driver::ABS_MINSPEED = 3.0; /* [m/s] */ const float Y_DIST_TO_MIDDLE = 5.0; -const float GOAL_POS_Y = 20; +const float GOAL_POS_Y = -20; const float GOAL_POS_X = 0; Driver::Driver(int index) { float dt = 0.02; - float Kp = -0.5; - float Ki = -0.2; - float Kd = -0.0005; + float Kp = -0.3; + float Ki = -0.3; + float Kd = -0.001; _pidAcc = PidAcc(dt, Kp, Ki, Kd); - Kp = -0.3; + Kp = -0.5; Ki = -0.1; Kd = -0.005; - float G1 = 0.2; - float G2 = 0.8; + float G1 = 0.3; + float G2 = 0.7; _pidSteer = PidSteer(dt, Kp, Ki, Kd, G1, G2, 12); INDEX = index; } @@ -77,7 +77,7 @@ void Driver::drive(tCarElt *car, tSituation *s) { std::cout << "To middle: " << car->_trkPos.toMiddle << std::endl; std::cout << "To left: " << car->_trkPos.toLeft << std::endl; std::cout << "To right: " << car->_trkPos.toRight << std::endl; - std::cout << angle << std::endl; + std::cout << "Angle:" << angle << std::endl; memset(&car->ctrl, 0, sizeof(tCarCtrl)); @@ -112,7 +112,7 @@ void Driver::handleSpeed() { std::cout << "Acceleration: " << car->ctrl.accelCmd << std::endl; // Check if car is upside down - if (angle > M_PI * 0.5) { + if (std::abs(angle) > M_PI * 0.5) { car->ctrl.accelCmd *= -1; } diff --git a/src/drivers/pid_driver/pidSteer.cpp b/src/drivers/pid_driver/pidSteer.cpp index 13ceb4f..3a36100 100644 --- a/src/drivers/pid_driver/pidSteer.cpp +++ b/src/drivers/pid_driver/pidSteer.cpp @@ -29,7 +29,7 @@ float PidSteer::step(float setpoint1, float setpoint2, float currentVal1, float // Calculate total output float output = Pout + Iout + Dout; - // std::cout << "P: " << Pout << ", I: " << Iout << " D: " << Dout << std::endl; + std::cout << "P: " << Pout << ", I: " << Iout << " D: " << Dout << std::endl; // Restrict to max/min if (output > _max) diff --git a/src/drivers/test_bot/155-DTM.rgb b/src/drivers/test_bot/155-DTM.rgb new file mode 100644 index 0000000000000000000000000000000000000000..a6700205a124aa98b65983b23b62ca3f69192b10 GIT binary patch literal 117412 zcmeEv2S60p_Wm6g5di@advA!nVM7E&MY=TUA_5{JA|N6lAY0f)x~Pb~_W~M3MMNwy zY7#Yx1+l~!lbB+XXA)mx)cJpRcW0S})u@Ta_xt<8_jY&Yo^!r?&b{}$Gdmzt%P>UH zh%_QZizxobDqr8d{Qr;v=1d`BPDc>t)Of<2=t!7qA)rN=V_85G!c^q~tq60p0tf;c z6Q*)2Fp)4vJ^)4%rs5O;9~Jt9Ia~^O5$4bj0BjGL5$51oAQEU!nDT{$Ik1Z`WnTlv zgn18s_Adsy5N4lN#0DNVlFS7fYSn3(?(CK@%3T1lA5Q-q1QN0=F?(ezP-2@NL9v;~Bjx`HrM z_7Nrkee)kp7+>_kyOJvFaeBFTO%*R7$9f*=$~#8VRZHqMjKqT?h>XEOBkjdA?!#( zo=hd=PbndP>>=d8mk9aoPeOieM#zKCg#0|1kRKfg`C%F%-_0fDUIrmwFDK;gRzmI^ zA>{TALOwSnG{@#eAJJSb7KiP)0mL= zFA#FFnvml-U)_a}s(3;wzb52}h>(h*gdBQE$iZ4d%0D3F0Q{7F2Y}OiR)p+d4>TtP zbA^=t0?Y;oAtmL21n>nAs|3gPqCb1L11W$T(1VaYzXKlu@UtfZ=ugP*AAnti?7B_J zPV{YuHzC{6*KLTsts5a*6A9V!A<&+X&EUG}W1u%78^K}2b-j){> zOi2C%AQ!*@$U6mu5R&@@C^;xhIKs}OS zpX^9T5^9w+3(zMdaSz~6$o!{30boQ(0$vmFIH~}pP@wpCc0%ie20frFo3&0ux$Gk@YI5zn#uo&jqP9CKR%AU|$6HtD~>F#!3Q zGzn-;i0d_A6)+PR0U$pvp8*?yIRNtGf@97f1IvIZKodfoz|$!J&?Ch02rwNmB*X#! z9bo5xeA%Oa_Ok)BnTUK$oJfe>e*pNgLk=dO9}{u`#2xah|4T359nfX*Gg3?u0Bq?cW7}>mK;reGU2#ZQ2F?JBL93=R^CUTicyLtrHH$<3-ljt3oim@(Eo+dfAA>4`+Vq6-lsr!!sO0`{>Km|+X{OC*epWqMVtxUKO@5UOd`zWT>0*ig8u(Tn6c1%YwR7ygb-%*BIti9^dDny80uiIN0=elvkijn0Dr>tiz7@Q*qb2- zrU#(^r=kBB2-5}aI{_Vlc9)_5SE2ua31IvM^dJ2;M7^4IB8=WJ!ZdXuOp_UeX)Gd) zU=8&D5cK~t^#2b+9(N(+5$4eE(S$t29Qt)H^uHSV5B>Y`9wGM$A>W%2@~tT$-;5;W zD|Kof|8;~M4#XZ{81?{|>*Zar2N(+d_k{j0g!X>|VC~ok zj{9)F6navMHYFbdi-Ax8^S(q6djC1_9snMD!FTUK0BhQws{kg?p7{WL?S@Y6J_^9k zF4*mW{$M_l?Ye|)^CM*I9)N-VL;E&w2k?FqIBrD0Hezks5CQ!^2DF6!$3p+l1AUbjPlxvZ2bcowM~vb@(ElvxKh`nKC$bcum%z{BN6`OY0r<%W*L*u@ zKYZmO2YKzG{fM25yyRp<|B>6Qp8)9gBIG1u-k;X}Y0+K+ssA!n(Nfi-{?v>(1wVu9w+{sRDVll%)158X%nB=k4Q z4BC(NIWZ1s3+;!``6HnHh@F7fc}JlC`Gmx6AS5P@kZ3PLqDDji!7K6xFax>|ZJN0N zKs_Rm%ZOwE`<59Pk27F11MkDn02o8z*h@?=1ze%~KLgkog~3-S`VqPnK+QtXhmf6s zKcEfWNBe1bKdlWk|2D84hyg4CUFdxc0NbhP*Hl};2%3KdfbXCnz!ZA_BTx>2&lEer z73rGZP03B%lMF771g8|gR?-79h_!R+Rzz~2y^Unc=0J!>g zgWi7!><5wo@bWQ$-Xmw;I|2Cj1`ls=om>N~2O@w$(EHzkY9I%IFE2CbJ@Vsu3V?r4 z@bK&dy?+2y03yHxBL~ehMJJj^OKX1&{*6q5F4$ zOaR9xo&Z7sU1)zP;04`(41nJRH|YK&0RG3jLi^FbaWVjLZPx-lq5nTX|DjLs?kG% zIUpGNk7IkfVh=C^`aco+9|Qd_h4w!Ira|}r1LOeE!>#bW6>+xU*cP_!0NRhZ#g@?j<%7ziLQGUQ(d4VEF_GU&9d7c?I+rO-g>Y5=;HaUB2$$@>8GS&UqZeg!&X4}gB8 zyAx(1Y}4>Q6*XJ%8DWw$36lhko{wD3`;sv6-3b$i9K}E@qqU*`4+#_5i7*l42@^h> zFkwrf|ENnaY=e-eDex2E2mOaG``jUnH)45BCX7cCVcb?h|0|*YH=+M5VH|o8W}*{e zCZJa17DNB{L;o*B{~zNX0ODFW5@uvHVTOZ?`3~s+Dd_*_(EmSh51={j0rbW_02{*e zg8!Zogy|knn6B8nbp|>D?UM=9HjOZ?ftElEF=3j6uOWOjtAhStg8u)2djP19Hu|hJ z8utLa2}9xtdAbDpk2(M73)}(e;xE6>;1PMK>x2n z|G$I&|A~75m`As5sf1DH(6$C%$A3JE!j zb^U{}xCemqC)?v5fIT6&*G7)sBcw6{djRO~Va(}61oKfEsLQ2Sg$q@gZ}43`{94%IOzXsX#cN(BlI6S zzK((ZPl5g)23kV@6QKRzw`wBve=pDh`j54^_%~oG^uHSV{}B3*J}q4e{hx(<0KV7* zVBIVTf%cyT!l3&sfLM8>p#7Hs@Xa|1=wc6mV_9y{f8;O|JY}B&{?L8!k`@D&(Ee)x z&P%=mCPDwt0ezwWLg+u%>h$}-An1Q0wEqTx{HLK#spv;4e5L#jARj6Hp#2wtc%TLJ zA9+u14ej3mfO8`JCuRZW(0;7p^Mj!Mw*b7(LmuX#26H`e4`49v0f2kdH_-p-(Ei5& z@)BtT?Oy?)PZ97Hu>`<4n(-~L9I%G&e+kF{@SpxMfSgU&gZ5*;5r*7{HG}pa1#mnR zH3&Hl!~&h6`^e|Cxd6sz@K3-&U?JcDpdV8abLx8l@-h{9n%WVX4~{`QfY|`*KjjJV zK9CDc2H-dFK5ztB1o!|wq4&Q4#{h5*fUf}f_x~K&2FwA_NB<_!`^x~vjvqMsSp(qj zdksKNeP;s5mCqjl`r!lrKJEbKkN39#_$%yd4h*01I<4REC&35uF!k=Ib z1Gqr<;oE)_Faf&%1&|8Bm)(Z|a%u-Y6OI8hffmqy@EDJNkH@$hw*wdf?LPrv-95L0 zFlV4Sr=eA+M&TX+^sXA(bPQ|nv4zlntkG2>0M}(lO91FdkmWU4m&{izW`u+s3Wu=>+r#;0JNnX>-PcZMj3RgtO@iVe)dCu_d`?m&4>11 z1F+7Q{1?~&On~-(3kZQ0(Epjxf3)9)Yu256q5n9yV<_}r0vJI1p{v_qv-K{J4z!2< zqwQww6E9Qa9xwu0kC>UrQ6_9M!vXlpL|YZm>B3}G-?`^ z5B)z3{l7_=8K}|p0fY&ijPC%T;ZyUV|68H|=$ro_!uX;O-us~cKjR*N3GM-)Ka+g% z9Y7LcoDkCi{hIg{VeHWF@fa(%u7t5cF0G27|3{(!sM9EnuMxd*4*+|Rq3FZlRD1_e z4E-;K{-1&V--iDGhI;_IxChV{_W(K*rVG#s=m50qiF*J*E5HOWMs1B8@ErhhrjP#V zW<&osLI1%;>l*a`SKI?I#65uixCh{c?*K6W{@4ipKMVc;8TSAh;U0hq?g5zM9)K0T z1MtRo0ML$m$@mTcYxLb!(En2CKh}`XwQvu>0^b45h5ln7aCJZQ{|NN|1L!|=9ACod7yP|G{CyMW8qI ze<8FV_UoW~YYU+LKLGa7|IGk$y$WqtqQ5IALjRGU6cs7EsFldYlss8v!hz(D&q0+XQqzW^CPW9UC#6Yx4O8}|V48haP|kDSg%oLLf} z1GFEpBS%2{4+0+0{l~y=0OK!$K>IP~X81$*!95%?!tJ2@-vOBb#%kC(ARg!e-M<9D zZ|DH%ehq*&AtRytcL0pxX&8&a@DqF*Kplb+BN%p5k=v=@g?Ub<1_FJc`KU`!1&{)a z2QZ$e!2guhKp4;udjAMOE&_`H919!*fOEik0LKD?0C4pG5hw@J0Py!Wf!>3YANuBp zV}8y+J81qr0LOe20UJOEntu@}27&=I=>0?B6i^7@nD+qaJ&sMT0;GT|&^d84NcLT|QJ%D2#HNbix0vH6nN59>xflR;~=mEV)e%x@(Ee}8k-EeHu4?r1! z{7f1T=tJ|*0|h`3&<}c#{J5L}ihwWx`EkZE=Mz8{-~ru#41lMT8$h7_rGP)61?`7_ z2iQ57K>N`@`w#$aCL$jbEus5g1Mp*q985qzCWrvU9e)K#2T;dxm>c8Z&lYWM=fHRm zZ~%A@fPaF15>ucjV5WTSf!Fp*w8Lu$C3@i%{t4!>>O9x3v$9PmJ796 znk!+1SG|{nG1}9-&^pmRa-ey%1?&Jvz!mTT0stC&CJ+q>0U1yLECaR!2Z3^61+Yr_ zx)QGxYk@VuI$$x74^UgIU!)MA?N`?WJ;vGd&vD1NSL&C7#^6BXa9EG`6dQppz&0N0 zf2M8L^T7Gw{Bo}}9v7egx&g;IaJJlM`pWsI{V4)y?v^UwZ^G+lfWEE=ID7h@;s|gU z*bUH_)Giwk0&{^VARGt)=y;{IgEq=*ITo+9wp^X*D>RFA0q9t22E5D|GDLei?&vt8 zumPq6l!Glm=M){c!vH#->3eRhb8~_FOxsWl0q8kC=p3SRY!E=-^XC^mPJMd1v&%m#vhF~C42=-Ncv z^4BMNjILYUx<=Owy0+0*y57-O4%}z@O8L-N?)?b7w*+YH8Gtv?65z%OU58(0?WS$% zn%xC>QT-1@TN?Xy>i;VEU(Eyk%xNqCICqSDU4wQMYk63Q_Z-&aJ;g>~3$Trc`k!f= z^*nGs_`d0T8jp+5f8BuN95`F&Zr0cTjyOiqNs0b=eW(BL^q-D<4)rya z|Cwu}>6PD3(|b)e^q8i$^cc6UKg&*&EnlzyE&Z2)`~Rx`(|rxMKJeEHx<8)?cqu_O zI0~R^%ot#x691RO`cLP^zpejttWy1_`({q>=_@zC>HGe`|DyhU{AF!4wdam;KA-iV%!rbAkIJdo*AXj*L9xFU;nA^6!Fn1&?XfO0?lAFdhX30O-D2-M8r^Av! zR^)}y!rZ0^M1#A^gO8L4H*Vh~ciunOh8H$~c2*wOxV=_xFL_KKdCV^0(Kt7PCI3tl zUQB;ha=N`DXgdU@IZlv=?1PY6xl>v4Pqa`E=%oyp*4|VeYN$N0w>;D z-K*ELyuRcKp4Z7NmWKvhg{M7P@@l7ezP*qKorb=1;rrE2srklwQ*Y!76`o)A1kbA{ zvhfV_S3AY?ZBIuKgo+QfYr+FtJ(jnol^5Xu%Tx! zjF1$xF!xzrU-E>S*ZPHhUJ)7&19|H= z>`$2Mp=m^pM#`Ue;xAY}-&j-6a+jvboYMuiPkCp4AZPogFg$kLKmwjvJ%=HC=1q z`cT7#4>!t{XEjvw-E}2KrCMJ8-qkwqPQhvUc>k*dYO%SC`*(M7FjE@dn)*j_^?&ys zV8gr6Z}0usH^ues`i|uFeEaaSUx?s-rsnTf)aF;i3cX#qeBJyka9yvl$A5J~EsITZ z&ESVVT~haag3|A+oZ|Vmlz&S~ACb}E{nbzMA~?w-=xUDe>L+;-@Rc-u>PAx!UhNdm zH@=|vqE~Vnvh!-E)P29U=K}E1LN2}bsK;CRJi{xT;_>!IPUwzL-8Y_>dHEAOuRg3s zpD%Y<&7bh}vz}>r$rC)UczEX}AG3L;1<$J=tMQqhmpiQHPk8!SPc*&c37%Ix)m8sd ztHHQnpbxwGYJ3puh0pSW;Au7Gqh4*4zI9kbJT5W^18yXXWe`u8O_M*=_765wkAGV6 zY?~1tF&iY0?Iw@i5wY>$%ITT%=>Je7US!o1ggOZ9BM*!OqSJ){ z0g30+@IWL@uCY9h;sHtAl>U3Cd2#x(!t}ueU=+?*YIX*V^s8EfY(aLIR_9j`v>}Ydw6(y zd3yW#k7?bsS$iuNcP~%(N$y^gy?uSWTn4q#9!G@7kOY>z#H#akL z#g^6H6RkU%Psu9Cl87@TNpWeJIigINYY!c5oA{vlt5@fzMMTbxPfnc~5z@O&H}9Yz z>C&QtMG}!TyFeHlXWw>k^tP>Ac5Ys~RFttgFC%aL=7f=Ldf2Z#e(q@Lj+Lv{uPCdz z^#0zsQOz(g($xmWJJzp1*BSi|jEYH|pIn$RUpPN_)aWrChfmAR6E8?kO;3u87iG;(7CZN7 zqBA~yK}t$+_}qv57m?cS?pPrR7KP1Yo-C(z1TZcdor-@^% zR*$u|w2cfL+NQf*{-F=5k5nGmvtGL4!o`z&<18AEly{V0XO=aLkuHh_#e`H_IM~^Z zA8$8dobC7tc6N^LgWHduFma;YM0X#5UpHqj-+-W@O^rrP@%4aEIQ#hf`?yZDAKlBi z({N7@$BA|mCN#DypWy7=$3#neNN`A?kGq?vx0k!S=VT9O>vl%%Y(0Ggd^{%m_h+OuK^PCFuL^-;-s?d^@TG8bjaG8biKXY10pQfVgM>SbkR%4C^1oRyKG zlOhD@3v5er13q~yjT;n-2Q z8_e5NsKU2AyKA-fW`Q&_1Fc13Nrs+OM&F4f8T!)f42cNGBpFg+dK2lQ3<*vMg<^@m z6ha5zVzk#qdxe34G!y61_@%<{mWLO^hd?ZpG^3;uycrieLn;-?W7Fe8kr=G%V(Wpn zyt!1S|JT7aRG}p+hVFMIqgm078$qn-`t`=xAlA#(-*2QLvXQ+0rZn=k>xf)itB6eZ zeg<`WzkjBghWNVjp7*2iYrf&f#}W-RH) zzS=o;_xR(_;9SY$T<89k&hgy$XXVZh1hL{D&dH~fg?z^h@~?JE&9~5|7ZJ6I&`uZH zjG}!g4`eect3Ijujm}$MpntZVI)!v2BI**{Bwd`j{dNoaLZu)gEN@zm#6RAWM`Qpi zO0Vh9g+CC8T&RWryXSeKaNUp9n2z!WQC{toCf}Ez?fa!pY4Uyb*}h-uR0H1@tmr8` zNXlw!ztrXzUy+wrJ*_FucQnqIwY6oMVF-4ou@s@19yUeY=)Jfc-DJc!rxS~bep@`?v` zEf=1PWC>;PW(_s4zSofzJ^|}HTD6U9a3A})@EBMh0PAy&YqV;GUkdICziWk&s{^+3 zZ8k^yOb=d@dmhF71RhXD~5~oaP^_e>QCfdxQ^2gCdmb8I5KZU2h6`?+mQY- zJy!DD_1aoNjo_-}{o}_gw9CcS)gNNqUqgq!LSepps>_N_KAOsAl(SENe=K0d53dB6 z%L`H0leff8z5Wfug00VHqJP)Fx_9q)GbnU0n_bTSe)`taJEuRm!;it`YVKkCxGe*uSvz&c=uDtBZLjeZH5FD1%dQ&!PG=*o(N`(D} z5-V1CB$$5$oELh>956QyUm;XQ$wV3LuX?I;{gKs~Mfx&@bHnaT7!>6~) zYMyE#HG|}-F{UKmj17?3`?N6&QpgNQ{Sz(J10EZI4%;XL zYGE_=lx{od!8EpoJmBAM!3!9|3Xk=J@3E}#5ba?z7Lm|3M0&MTJl~=4-Iv(PM?3W_ zOsA33SQD1K+9{szF!=5brkr;J7QVq7zF+MW&-Zjxy_ej#=m{1NVYwl$E9ga5Is&w0 z$=^NC3l&a1cPAp7^l_}{yUSBq>HV^=vC}bNMaR;+(Bo-rTbBH@&3N%=(wi+d?FsHG z9J8a_Tx(YJkjeWKjG9#ZWC??ThRebOJbE$>Lnr>7`icb2@3o_HOd2e#>r z!8RS2+H|M;B^{T78tK)mS56$S5R}W_D?fhx@>SGQcvY)bbp30L-TO|g=+>1kG)G#B zdiG+;+vthc(fO?SMoBaVt8!ndRU^83LQpNNlJDoMg(u|u`CI7S107rx-N1dS(?9mW zBACYZVadN~t=@_2CD-*?@!eWnMTst-z;xp-qqyzm7bxPB9<1ohBiwiVvm%YfFFk<% z8@1z69>L0{PY(+Zi+Rw$TzFnjNknqDRz8w`fV>={w*+hCJkhvYx9&Z9JHKFK6O)#$ zJKMcTV>L$7_fJ;tFT#BRnw7~4T@W4rRY^f=Za#Ei-wDq+E7TL{i;PBUJgXk$dhMt0 zZ<44QUins84?ILaf|XiXjUF+?{#j=Yn(6B|uM5JrRS!~Id+E#;RS;o?GM8M8F&5^k z81+szZ*JVYr3FWTe^Ncdg$w7ZH)Sf?d0~M&j1V+e@(_aIV)H@D2n|k3b(-p`dQtDJ zdW@>7!~0f9RWa^dQKkyNHGotN9Xe!Ce`P)3rrz0R%?u1$C{tDMta^|gJGQKrEl>sd zjMk&}S{kbe(yw1%GgI7&qC*01>YdfrZlYr_T;o~wAo$uWEhd(0C@dSo7XSI5|NL<4 z*3Fx@Zd~~2!_pnwm*(ZAM9m2Kz9lO<@w+m-s7H4}H=*@&>~`)@*oCUu3ADvJh9jPB zRW>+a|-50M$QUw^KiBu zKW0GZcE&B5`f zFQ41FNaecbwl2H-@q_Pg+`N7L>bX5d(J|9Jooz>S>C~y6adTsRV}tfxyY=qfXV8cd z7PjNYPjC&?4#|j!jEr8Oog%jE@Fk+$`-;l~zPn5go1*5rNTcia0%K{j76u)m|#?9Jw?Pg}y zd*JZlqik$#$GZkjYaEg>W9Cc+^Y(Y@e5>c@(xvmKHY=y)IQHk2 zg@;cMkD3!Nk6|R`Vz{_C+K*Mvrx)l)d)kkVCJp;xU}oVzH8gB`==539af*IOG^B0w zri~jmZB=y1kiGE7y&E@gTs*%oKPPEMbTod#qr#j9b!lzXs%3NekC90WLnA{2lTO`D z`}FB+Zecmz&dzOG@YHF+A>rfo_TTva!Ig6let!6reY#O+T+$ecI9R8e>)Ptot5&Yr zz{T3EjJ01a*5EE}j9Rs7F8?tyG0`&G*0OUCv%dZM4YwR)XK(Kjff=E;;c;pOxH8F>NU@+G1dA(6Os&pML%D)1gy8bK8Jv z;WHybH$q$#-4)q*zJCHl4qgd*``KmAf=-VF-$S`uE3A z$IktSjSCE!5gCbPNcj^L8{5!jzf|h7OFfsnDNC&D2F(pRbm?i&3G_I-!fDXDVY-&FV)EuSy=SR6!mn$by@|w z`E-$9lvSoUqbDZvEV})bD6DcGjA>>W8aZoL%-lLFQ&*Lheze>QOO?8nrT*xUnON!L z=GMh>=BzSx6|o%TRy?watmNnSN^ammij}$%&9g*f#`Mk-FEkLM7*9^oV)QUyWR)dO z(8c^mJbv?;b3O();VH-!;j^Lyw;|w)-k##>RN};)<);+x90nvtmKJHf2mczByZ5jKjoIZ<`#9$zpQIM zP|oJrE#Ii#-?|nX!uCfz8sml*4o{= zP%P3Hrm;~h;Wf45nvNKyYU)XwHWSrvI+|Jp zY;Db!iVND$L$4ox2s&%qCR;BR2RHjr8KbWCkh<0hs@CmNXlq&uIr+NQgH)~C#j9Gk zQTRSrpHvE)@#JL5iARlDQEWHkB1)=AEYaN$!`kbp+#~w|IaWoI!rm6hEdue{$931( zevqR5=mzci{`b}S@0Tt2PfHE|ynLmy84oK;K7OKw)ELQ{7mLP^H!BuL>Aerz4<6y@ zwN@V1#p1XoS!wKSmOQKZ>AL1#w7E`pgXV@(m3Q4b?=9Wr-ZSORdA!nC$uDIkmq6gA zfnW2Lq6rhsR*LP0VEq#wL>U*iOS4)jDH_=JlG3oQwZE$MP*v;p3u)^??HacBRkg13 z-8NP3yUk|}NikTj%1UqH9{dx}Hd3Rtl-F9(05^31N05~~aU4`QW^KkApC${DY9uU; z<(ssvGx4Z18L~ufq9fE`;?X8U6=hPL$pA$Z?KI9rkvjY`NE+LSm3&oNiis;9BlriooEz}6+JUv@d`Cl;Hr02O+Z->a)*>s0^ zd$%x@?UOG-v7l|is2$Sn3)`8bKT2cc6ddb}^u6kgx=-hf7PnihFls5%WTfX&XT*!t zCSPvUwk{HnsWzMQq`IW?4=~+m;JICQH)Xn8yr%hX1U>i^!|(AJ%N-+kXRLGA?RO!K zji*fcMuuJ*jjXJ8DvSnP)L>-bQDTfvWT9)vuy1((9CbMet4;L!}z8{ z4q_QgpfuHNJol<>OgWo^ww%o&jW+J8I5T>1Hp)2Tnhq)FYmf zpOPl~MUy6(?iX2Fpcv;L;IV^0E$oKvmzE}XHOY9G#^N{o(pa8NhPJOpn~4gWmNE@C z5^Zml&FE38IHSfZ<48Cg9!*_5FLAKs9BBTTq~pC$yA0XvStfq3M;;{c0G;YBgyv-# z1w*whu(wI3EY(bW_iHi{!DO*{lhN4VG%#txi_%>c#W-CNWfXQfauXg$Jk#;hp_0#N zDifP{mo+{l^6=<+NVdgjAIpAw0UQ@~*f!*lbengxzFRSE7f_OFHa?ncI&N3k^wVgg z?ZvYxRmRC|uZW}HcMBJXM-!Ka_sdGI&{PVw{mZo~D9s94hVBO}TXP*1e`Gy&aD{Yd zuu03fU((oAN|R?J(hgABNOjMsY-|-at>$X55o-IXY%+9Db2e5s_2ckpHluqD1O}tfR zQ+n4$%TPvZ&Wpz5Yrsl=C@;AoU>7}>G_l-VRpss7qe@oM#|e5_f#Mgm+1;;7c67Oi zv68NU(&by}jH$D7=B(Q8QChW7ux((~#A1^wmLq4Sh*jjS7K=yLkd^#cR&rUuWiqSKWZiO1B4&HM2HcIa3}tJ}AddU1<>4~3bvk?xo+bV2Y<>Kk<4^KZ+XUC&#%x2&&#MvhTR`%y2z4j4Xt2>{` zEE{8ZjAj2@jjIitF6FJyEID1dch}zCyS7QfolFgNyEscflsnU>{=s+AsW z)jVW`JM}Wu?dp>Gk!BBz^xAP8462^Pp|hh@kq9{lUWZz+89yE>IfX@H(bJCu+vs+( z^oZ1oT_52w+0Q*FCN5|F#@!|R_9}jMu8inp6!Is_Uax7)?%1c_J$B>X(vniu&)$v2 zNz=SsEQfSzZeW6MHH-(?cyB+6J%ap+Li&^r7i$lVrPmp;;$matB0RhiEVf~==3!j0 z{mptb43?}2W^6#e@-QA?>$CHereKSneiT4E=03BoW3df87R<#e)$1P9uw%i>j?Jz_}TRz}SXj7;cH za}(U%Z{=9{HAd~(9~-lq|17s}qi@*U$f)kez_=5BE!H(=02JN>U2D|`Cr{zvK!3UqZBNsrex_~V;1iUx{99Bt|>dtLF> z>sQVk-MsPceZ|?vY}V7q55M}be9fnqSJp+%E;O~U9&K*kH{j68x`u!Ld}*stq~5Tg zsjjhsu7URWUFCHRAANs1(>q$xupx`Qb<3~RUO97Yv*^tAi>dP6dm5Yb^!KkmKD=x5 z=FGyZd38kcltlXVjFF{zsM`K|X=mo_Aips6w#bBWGjP%Ij-D}I)%NGpdBLMiJ31)Z zYNE69YVDPC$F~SUC#_b|Am`SJT^rY}TeB`H)MXN%PA}wTn0db*G17$|T-&qR5dl8l zo*o|RZ7CfCJxXVG_yn%);ou=%JG5(MGEC7{1D*BjYHF{1bYiPKJDl0x?d#XBUcGW{ z@`5SOJUWZbEUYbuoA>J(%cGNkzXdR9lB=7CdfNg$(9t(&qT@3=d_3pZv86(u%@u7m z(b;hQ#?|wuc8Jb$*}<*+|9&^0%~`)={W^T7x;8}|=BmcY(#CRx`GB5tGnFag3_`p; z+$Xu=ujQy46zJ-;05P4We$h%|oWWo`pFsBy!xaWRb{XvbD>XHZYsJ^E-N!@Pba3Zw zSby{OwTtiX7M;7HtRn;2y76iGN8f<$nyu^Bu3A;RdVws0vdv(BxO@eVElRFkyZQs? zq0r27EYdl!XF^uGr#wY`4^w?SJ>2PE?faJF8`qR0sHXcM+t1s@(nACmMqT6k5@R%Wk1R*EL^f=>9Xa^3(s+03QaA?jxrxQ zpx6AIG&fCNf`g~{`}ufzPv*Q_SXx}Xa^olDbA3jc#mtlSS z^fogy?P=4Xb7J=*3S(GD6Pl}13ohh$bE_*gJYZ3moK~6z_&IhXg^%`R{WWb<4y%Mt)xIW8M z`24uY8NvPmfeKGzWJ*vVL$ZpCmZ&%t=oqxn*4AyTJ0&*M_F0}>e7)V>ob7Dg6rR+I zct)0mzsZo3Rk$?&3`cMM+Uu8&S5_2Z_C_7vG+v6K+lLd|$PR`Db z#prC}qSS<#sa{iOah*LalK<6&jGUaT)10@(rdC#i2Mp1 z(>ZTvGjp;QW#{BBS-SWv=dD27z*tLLPrIqltf{oa&-6AsOOlp2D>N)bL#HWCi9{+z zQB<8?SNzfGiX-J4WT!r>O0LDXwp293vB-aU`Ot})AKANQ5)XmnA}1$juOM&dai7Pt zIWeN7v}hmKkU5K#wLX=SmYSB9o{m3Z!F69!&|~;;!5~?m-cbu?PkJ%GNh=DLNTR~# z&Qkalv!_J@iA~6w)JSv&Yq6Jf5fplX^WzK zy`0=5BNbsqY?WYEW@K~>?cWJ5g@v7ZW1RKq-ZMOMrYeOmA1i0$!iecpBSor29*d5i z6CIb3yihEyD;FbZ#586EI!=>3REd1)SmMPyM1n}^{A^_`wXs+gH75!W7|l%CW`<638EYSn=yV&I%T~^aj0lgMH76lCty-DY zLS(gn?_OQH^@yAs?e@1r=4_Z36yzNuoxz1XIx9LVGCFo%ib#Bt3t7NuX*bd$jdbwW z?8g1=kcHcmr_TtRlaC=wd#RS=BY5Hr&tt|WE==Qw(UOfzj+{EUcm48=4?cT8717TX z;L9B(c@UfbqrZ!rlcQtWkzcQWaHQrx^e#x=uh-9&e{%B?^m3lPqocrS@g(oSKp#gd z3yaaCEk@he+gsCd?Cs?HAQlQ6nN~>NCxE{69=!$)3bH zv%+Jt!ZcLla0ni{oiQsmF)ih|G8x62iw+$-uzS;T>6y$DR`mgN$vMhH8jC|)wUVeT)9uvl(AY&XuVkFs1 zSFXrSjq@9W&y!v47NIT+WZ?m>u3ll`GUZG-6f`wBI5Z+Uetu#VM`dxxKK;ykbm?X` zFFBUZgx5nQXJc|yNW`3!5EYeRnw8melhabUNm-o7XlXHxh?aifG(S2+UJsQeyT!?I ziK((s1r@cKQyw^FYH&zI3~HrfwQ*@#<-Q#oizKHno?KA-;B3XpFF&HS8psyycl2?L z2$JQI`rye z+O12sUh#?1F0Y?c)=FvgOl(x9ahwhW1O`nDiHJ*1UBFH7;v6CnFoY54`cL&4`}#Q* zZYfMl%E`)9jwrPOBB-5VU~2&#`Wvg))g}^C)3B$(%XGv9Ev$FZg$v|89~9S8MxoW+s8j3I0}!u z;-Mb-Ua~l^Q?I_J-MaQPi;j(Qd{fM%d6MJ={OL>8gnVzZJZEzflIqrmEJaDR^}Kz( zEZ-Efyp;uuq=nfTn&#&|H!p8Y(WtozbCJGnwU>VT_1C(ewK+RWcWqs@R8(>7=z`im zcJ0kxzTqc%iPwr8eB75TEh=8QepB8oS63fjPiIHZ+_Mk=TYKu*wcqBmWsk0ZTz01R zH};oPi~a5C0_*59IVjl2-jXVw?UdBwU0XMAu2*S6j%>zg8-M4q)pOXKxn92h{^3)? zBe^+J>gwU;=N~vTIwtyXL#~TDnVR+N*3HyxM&wMpH_cTPHD_+TFfl!X($RPIa3Arexn?d%5=shkvXxxb=6R`$yEhi1$hnDely*&7RYhgp zPeN##NF+>{8=kXtX~ZS=^!mk1w||S_(4XCS!NJ2-T1dyk>b3ZGa@j09`+zAP&W>JL zXa9I|^TR_mKj5bH4?XX-s4c!+UWH{G8=-hQEby15t;Y8r;BGrwV6oN8E2d!Q-km$P zHyj$cfspAl+SzSFz#q|U&itU6b7NDb_mntK^6(7^jEIh#eW)Sv$X58hs}3Ez zbawXe82PUfmnDjFvV~EJ7}WF*UM^ee;x;)TC^#}MA%+g>dKr%pG}dk;&~B=0VPn(h zUnQO+l@_lo5J{B8)kaN;vzwQHQ0T0<E6A&pE+)1F4j|A>~>?^q$NhjLS+i{ZW$FdXGP8sU^Q5>vprDQ+@8uUX4g27E8mQ;! z=rnDjqi>+=;N>~sU#7lvQ$bO_R9YCNVKZOqjBQ+S*sQq36xC*I)xr(CcW&RbN_gb# zIT^U0D%^TNIby2p-6t(l8!*b0;Q}$C5D~oer4AJ0rj?D4LgLcn%E4!jgy;QNF(r7xw8V7*IH5P|wiV zITml0U(wpE^tm$=>P7)p?8*K?A(1f&$s9jXesh8D5`AN%rjEYerf-(tirtH|g-Oyv z-0f~~6-weOfrZd6MyXTwbw_=M-loBr~h}BT){T@DGIwLYJB~^7fvnGG@maQ8%tQ3`g zbcUAt=Rvoz*|Onu{Lje4W>ii^^cW{1Z~=c(wq z`veAuhR=*iO6DetIIoS7pn0LOK@+P$_wH|$-l`2ENyFGd(}P8OG|g|M4ct&pPXGphvbqk@Jkr-DV^rz8I|*_I#*SnD9ej*^LIp@s~ktsI(xoQ zouz)3-fmW?vm||9w6BM6bUb%&yUg34{+jvBxcNz{IxjXKIeeJ8`9Raa#E9W$}% zP)Nx1@R@TGQ$*Y?w2rmC zPWW}u$Dt0bbjj`^FIMRJqn+KYQRs}+n3$j`LGf{1Cd&hH8!j|_*4#u@CYLN8YH2xY z`KPnP7iYT3Fqeo0%$b*5{lH_LC` zwxvajGGs+cerlyY6cRpTM#Sv6dC7HCdUa9NiK?>w+hiYqeVLZ}gEL>#U(TrQ!|vMd z5D=2}{8FFU?u+NTfB$_RE8gp9iE0~dcoCtNNUR;*$D!Kd)TH?EK(AOOTQPemSP&)) z$M-r3$*O5xXl^qW-=>Y|ldxnSt@fLxShTVzFMZxj8MKgI)`4P(0Jo@S$Hgb5sit;b zt2XW0wrkf)&o9B77W~apT(fKQ+QN*~#hPkf88JH=-;yOIrK*N!@w(Ng&Q_Q0-z>fS z!__RXJ-N5)qaPJDk9P8i|ErpBqcyLsoySW4@Eg~(DcQ}Wy-?9Jyqw)9py=~Mr-V** zb4yO(zS%v3Z=CS2&BV`3TA(WW(qUt5@ePW3?}cj;hrM}@*$d~VCd~}Zi&t@sh?)(G ziHWHTRaMV#+pc}CE@@lW!0Uh z1bX?3bF!4TM@}TAq@)QYGT!(3#jA&nu^u^m$Pg<%QtkrDYaj^fE= znnh_=cHUy{L5bqx4sF}gKNiz8IKh(^{moKbQIwb|O-v}1Dk$=5u6$s#_(a}@J)5`f zT$6tGv$M3y$JS&Xxu~e}DF;74{DUjc*`=L2eMULE6IjUu_i;K+$u1`6g~~irQRcZJ zp$Uo7trseCqDW7vy9X?EdtSI;+FI+qBX%G}hPg z4s)eDvNz1Cs4!<$VNof^N<%R}jSo%9TeEGg@btM4XfY42UA9_T%vFw_p)XoY`~|)B zjxBY`POld!Wmb%0osJ93+FQ}l(O=`c@3P* zO$_mu)SBu{@|SO|-zcZToGnK$+*akOei`-I6;F@4Sz37{EL$l$dFB+|WgW}fx#gZ> zQf9cirT%3dw<;tTb^P9bB7Ek3{VZ+w|xpFvxdw8&7w|ZWOjrB}=j97d8m}1%ehO6(7j=rcM$`6c52tUOL`TmtIbNkuDx>g+BY`J5-a%WfS;~$r!J$M+|1>& z@OJX+)UDqTTIXX~Zv(&jrPgILp1wb^leSi)TrHC!%a2yjQlHAd%a!_bNACsn9~yk2 zeJ@JA*}elxJ?Mo>?cz>L{f&pAj<%MTe*96Cx-pyiaCMe6D{seBYh|(d3|p8Evb3ee zetQ}AY}KwCE%)2PP(#5Vq>SvS-MCSX!I0I*D&&_rS@aiTYiqG|f9mLy{QBw*9hQ9U zsx1&SlIp}AQ`Gy4^}qoG2Ydao%)fHScUY#jD z#hmUn)Y68o;BP5CuFh`-Jq>kUYhi1lT)o#T>iqTY?^Shnc%54_x)s0cNC-nnqiE%f zmRGvl=e$aTV7*GewFJ4<`z;~(oNCJq%r_jXUQ~-hUr^mc*=qlCTCLcbz37D(W;Lxg zBS5vI_|@)uQ$w|72EEMbync%b_QcMt)7wK(bG7AD2i4wKt*G|zzjJeY+Y43uZ>-^} zxgDk4h|A}8m+KmONq<$Nj(fW3L3z$8>u$!xoOZg;U+qIQ5y-_CmQeN07`t=33 z3+r_8#R~h@Fl$iQe~H;&7FNP9>y zpck*U6g#`y&SLW(e(b2HO)rXOV4JL}ZHIc3maDBff6`)S_coF1*|IGzi0aDwFOqw1 zg@Tt2C|4yb`Mk?pwtUHO9A3FIVH& zmAk(x@`<`4Z-1@pvn`5zYj1Y6w{E~+zY6NfSkF!ZWo1pxt-CQU?Cq<3i|A|M?^ z>d?k%%+RE15Ro7%5~D^C2`VB2mLM8qtccOgx+d|{B%8j`MDKU*opuKX1UG@Tw= z-1nUGyytn#c{@t-N98$bsqvu>b4gAoxnm-11w!|NXoSTl+1z23Fv5D*&@B`pd3w=} zP4fPp3&Ifk=(>qbQci4l2ugBcs+ZrETvoE%{G|;g*uZxJB)?*@8~#7LgR zaM~#zsqD*Jr#s)$Cq84V@5jJJ zoEVQ(b#GOXk_M&?{WCMd*Lk`B0XIa9IY_o}z|DTVd-xWr<3q6N#y1hF#}$MLsh-#( z;u@6dWF;vXIC7vm%Q`+U%HH`0KGoxEyWcr{Zxe`3p8v+A+TuX(MO?f+u3+cw=0>pA zyQ(`;5vZ1zB@&fwva@`h9st$Y*m?dn@e%j1=6gJ$;#A@rM)frNZxpK0-5}`x0FjP* zg0*IXQFlC2ogc3xO;k`;m6egzvP((}*!3eU26rpKX1*Cq?~jMfvI0ydGtJW%#yKD4$1P9YXs)Y5l9G}p>Tb*ptNso_t{JPTXc=eYH*E7afmM+W zuqGZsek$Kq_vzHZG*5_JF@FLux%>mnT?r+pUrw1fjXg|eX^jd~Jpb5iRTUX##i9Pg zT^Nko8y(LlfLK`n#~aHPrbMTBaruwUILugUuB8C{OUoMNC3)ROkVoOx=8nA`ZIwLp6`nB(HVmsWoLi$Ol( zNtsCCWSFnO_%F_nDd=g9SvOZnO1eTjJ!3^Lf_%3#J~FeW|Hm=e@7%-Iy|vVfE2w_6x*=d=vA%OsM(2A+aBx;^Up9(0QA=r6afI z$7gh(%?+Hb3^iX$dM+i^3Et2CZckcLe%;kyM&W)1OsB&!x9IHV&d+&noqU)n&E=l% zGsgiF+gDLV=Cgm%)g@(b@07ueC{DF>&>0QpoA~cxfn%S?hvl6955K%l%Ccion$aq_ zN($}y>p%Qhbv#px!k&NJk$>Vm z57_nq{O84KImbEa!u5Jm((+5TWrVc<1}}FQsi|lhWqdnI{YTeKF_<@}5T>SMsM+iZ zz+8y!fA;YRK~5LuX~3O@>cnW zP4Y}T!m$ZI(038HEHgTXe%&o}5;DeCXat@dG>iPq)GapT*f8y162QV|>F7( zw%9=OPgm=s?e!ETXUy0DSEL9?9@lAu@vZ7l9Xip|2Gfh~4J7wYm}Fs19%p=0l+@D4p(n$l`6t81JW;X&cTRVFUYs&;!$PkK4x9g0&yC5j)xloe*}qBJby<|} z%jPm%`1+9xyzLGLcH{XBqX)xAoc-et=;4YCkzv$$2BboU$Jvog92|ED3Jl}U{!Odr z#%5ULJT~B*l+ZeBT{+4ydV>P@Jobc-?Y>LM^x(A=;hDebLpCO{mLlh|C+nn3j`ikr zSib^dAG^Nspgnef+VF$+?*=knr3GSzcb-fivayMUJ5TVT)uc3goOBEGXfCm$cb;Hp z2z2|!&tSq0Rl-A@N5?gTIpGHRt&oY{pfG*N-bk$QGMn*%6h@`opy16{3U0Ak777C+ z;Vnhuxy%Nx#t`08^e!QL8)7HJXJZp9a{bidacv@GYW75mLOKy^{0AL4MEbzucD2(12=%s<x<45P+(cgMu5#d!So271wp zA%!j+&y0|;iQx`2-rOulEt}&fq!+ybG+E?M$Va30xWa~cvTiVZJ8Z-0oj$*br%{1ta49VGGo2I44X3yl`KI76@|`dKWC!;m-**ui_g>fOM<$+F-qwi`#ExTR zDXRr?EY6?@(iy{X-IdpFx#L3^d!^oDvzheD7dY}Fb{jYJ%3ZcOr>$pG}y0mfl zZ^C`W+d<>ryZsJ$O$@hpJy-H8E%ssp4&{WF%*Q#W$62?46oyq=-qRR@#}fJ!QF0CN zC#~*!tmMsGa4*n**RXMCE{30m9_pf>f?D-Q8*}S3qT#2B4IW(^NA#9V! zNiS=;zG{f4FZLFO2Lax~kmkLGQFK*HMusSB_6%2dQEXfn>YJet-SFvsC$R11V-ux! zI7oLWC7G}krkT36!wgf58N(BV3RVq!Bo-Xn81L* zV9LMw^kUd7dF;Y7MwUW$-wA9REV+K;zF;IMdDDqa485<9+M#cg6%mxpYP!w{`E7EW zSR*sexP=q5*enujoOe78(Nut~(Ms9eXURB!eRi?C_u_T_6LjdSWJhdu(bM_L`b+F^ z9q+pV4cvx8gG)Z=3WvGV32ivJa!BQmDT`!g%#b9hFRyN$x{7z^wDh0SyO}O@P)5_7K>-j9Cdk;f~>2*%bcmibFBX{9*-F6PP+%E zm3(lxk#Xs2KQHV7SJis>roHhoR$f#K@13`}Ilz18N)2yhF2i(1ygT;ZIreV-zkYvx zV~4Qu1m-%-^!1IGnlI(PThC{1^5MXf^&Qv$_1iEw%ukH92;?GpBGD@Zo~)lbIPhA1 z_l+k4?}*`Te`Z7Lq2?p?+xx%3@pjQQDC~{A@y42225)-c6n68C)1@NJ?J=;jF*mbVvM`O7D6Df+0jQ)PudJx@*zFEcK)c~q91>YYo){SE zDdcV{0M%5N?X9k9y~P8BV{Zr2my=K3&SaOA?5Mf|XQhUQpwKt&Ioo&Qcxxl`=HnZ} zI{3S5u+JO0JRIzXz;R>X@|=q^3FWfj&0Mm*>|yY+7TVA3D36U{0l`dGahw-~FaVE| zmzz&xvf0HuF7x5NZ*K2kZMA&aqWqm{LT0A|UfK5Otc--XitPe;Ir#-t2D`XqM`b@B zUe!!3Ek#vzvh1b|KVjoK74Y`dP>R@;tZFJ3o(Ruf@yRL4sTnyr`LrHB(tY)p26{ee zJIKEC{XHJfM#xc`kK6V)ynB7+5T9mdikmS>;;2&Nn^|Ug+68*}I%6zvjN0Vmlbcb* zWBF`sVq!v4YDNYnuZs_Em!;Ec2U}ZnLs|oc7xPRBxYF&}1v&9i z{O#R*aFyCKXK83?DU%|pAv}(!1l+FbqVkel%5EMw5r)r1#l*zMC#PlRZWl0IUvs|i ze0y6n=lat-yg&$Wwz-8Qy@BEWa+Kk3zR5&b*In1i0mT8Q-b^ujlbg3YR*{(nITbpT>nw9J%X zzu**ToY7f?9Y}lNhVaO!==ikk+;jXQS7mN(ZDnb(%pj>ChDYq=Bdgm}%HEckT9__C z78Dj9y(K0oGf!A+cW9}pYpAIz5`7~1Lz~G**4SLJlR;rr@{ozt*fzhQjhmxm64LSu z&hy>(LFJ+54-XvN%k2B&GOyzWkY`;3!$u|cd{19795X*X9$u+- z4q={tz$-mJJ~GfNG&z&+y*3{pc5_rraux;GB3oKoSeP%>PsoiH@;bS|YHO%D$uVhc zL4j-Y4crhBy(K<1M_Ay>wN=#ARaD4Cp9mLT@I2`r+q<8^%Fkp~@;oNOYO7a3*ruqM z__W*tL8WK_R+|qVs6$y5R@WDP-afSQ%IaM;wRQCkjR%^V4<9*pytS>pqqDQ?KxuMF zkiXaZ9WdYU+|_*{N#f)8Xyf6vD0hW-z}jOduhhhdK##B_=*zhEwbjcnBrGB_COIRg zlV9L=nORzzFI#2`yyEKXWJ9Z|Wl)l%(^)uZ{-NQKTVhkQ^Um_2Rca`ys3<7|vyuEE zEigOT(DoeSF!D24l{{#o+<@R zvXY{r?P~-4oI6vHGAbn{H7xVqmh=#MsC{KPEj= zK>pODu6)0!gr1ctEDtRn-T@&SBNJ08Sz=Ig$Rx4?+@~(*9memXr^DB~4p!Dwu-Vl~ zVhRfs=)kb>h=g>?HY@|}-Cdo+-?nHfEiW%KA+mCJb;50I7S04YSmYWIAuKD+%^%h7 zh>r*i_VsWtx%%Rdp}tdhzqLl_f9O4>KT;X0x<7F8>d+tm*uG}13zQa{1xf^HAePwK z*}84n)i_pZDK7R~8{!uJ7nGLl@c8s?vHsx+9A4Ak;tp;9hOpR_w4~E$X%U4TH*@a7 zd2@AV>-q$Hm`)>X4uw%xN>8E)V7Efm+yLHA%i4NoBxE1v6Zk;dhOPe4%E#!ZRoS!rS#m-m@XN1Gb=lx^?2+*QDxwKYK+AG&(^(YRs3 zzWs-G=5F!|+~DW##_sz4`}>`}gTH1Y=YAaQJ#}aBT6O3eXE%2@8#A><;ejqI?QQK= z1m^8(9;H1gEsk5^=ec@dD^ivo5F8pBvoS{KX)nKkkdUyrl++Z=)1r-F@mvEFqeXgp zixM)TY^M?@BWY`T2Ax8SN_j9vO2EnW!J*;dQOTLPIfCJDeev32bygr~}S5000 z-qPsMfQ>=kDCfUDx%tVb4_{J|(~s|*?jF2*g68S$;^yJ*;TIOU!DICbJ6k)~$g-Lb zNA@2(N)qf{L)=qRk+RIRa)~p0IIyMvY1_6S{Nm! zE_ZC$xG7?D?AB~*zF;1$q4Z>L$MJnT+5Mkir1J6AKKFDFPN}J;Zwc`aTJP=Qmf!XB z!_V4pKc*rwNZB6`ueT4}xmL4z)jBtVN3*Bj#>gPImB8bQfXyW}jr;cDf`H1xXnThZ z9!{SDi4^MAP~Qj`|M30Q9vr%1V|a9OdUm#8s8Kn~V5y2youuA+g@FY+4SCCIMB?tXp&0Y1UOKwDnhlly&V2Y;X< zWTfop!3(EvetwGS=j`fE@YwGY8Wry8WRKC7Rl29XZV$w5H9MGdc5$h2W9^m2bp5ON1?%BSf->HZiQug}Eoz9-S1A9_eqvX_h z`h`Y?y1^oWo!z>Ca4Nl!Rge?pOt33-+Th{%B`{M^3Pa?Q zj-c#T>zbg<%r{JqWk^v^`o{y8JHB{IMHV16(Cl>FywX&##>L&;!`(ArV`PZi3R_vb z!c_qtcHe+Msl`db0q(vDFy`j2jCLm^B__a%9wm<^s7uwl28IR(3l}VmNr?A;OG9M` zw#RMRl)wPuxQ5QBY~7lIt|~Fuf?`tn-i#UVNxerPdieM`y`3Ru)kh3M0z12uXGo+W z@5{`}%E-=zrPU(AvhTj~)17T6j_zfhztG9^q8aScIXn0>6)}Xi;a_)8oxgFqa>Ht8 z=x>Yt0z<-rJREIpp*gS*@^3$Xr|~uWnOA*W{$vveHodV8I)XD#x4=$reX$c??pZ6?(N0%-~`R=_^>)Z zBbCK~Qiu|T^B)7bB$>}C*iPq^l?j%m8!EfH+S@*E;Pei3Qn-{+ktn48<%8>=T<-gU>P;l-$I~P0=a*;?S2($!#ILRWoua6c~YXi`vD!{DS-f8l$MRT!6cLhK%$KsJA4KK;Kp4*UdRp zyPLx(V(@h1yu(B;(+Nxti(Ob&DKxWh_ti^hIzMUTT)f$xi<>9;ja^hMpt*8^mF1G9rbdQI zxyfGRQk%1@ zWNF!f^s`|Os<^DAdRK+O?fV;sZufR~H5FbNyqq&cMKY0{&j-)-T{+73TIY(o!q?kl z-Ou0#N^wC}>ZX8Ag3^AOQOIPV-YpZB_A31q4rWWuj1AIQnOX2ZS|UU?jCCzGLf>Pnlc70CpCjt zvY5yf4xyM+Qc_k~QT8cc6J&U$ndwp^14>CAnqje;VzSc`Vk1H~gmU3Ak!w2$m7J>T z>fJTf0!)=evMStjD@EFv8i{6BtR~OkKwlpZXD6=_8QpT!6(%GvYmjya{{0^Diy}u5k+_H)c4#qFcfJYQeEsh&;b_`;O#Y*5KgX=fb=_{A%#VrDM#22REpokBZDd z*k9j0bM?mUETrs%=6wx&_tw_u@gY6CdGnU!ZPwkpx1aGfR4=k$Wp8C+s-IOt^BbXI zQ;@%(udlan0AIs*x9{AQyeER8k|?J}mM0Ts!&0$4iC|!vYN)GAs+R$1<1Z>uNs?1V0TM$}Vo;}35x&4O6=h{5WhEsmt^m&40(6aoLwB!TXa;mx z;X#v>9Y5UIP+woykee1L3e0Y$ot1^LUK*PR4AN!=`TBT!d-?c^rUgu}pO`E|3{2)Z z4br-)0~YvMaMOx|vv=^py@B3CwEhQQY{#+!VvqWc9@w|xJv;DbgN5U!DS9|Zlz5a9EJx{nFrIC`?fBpXT)sDme{`IgBN=dao z24!>gX<@oQf4tpHFBeUR!hrevHLRn;bT6M@E(*-$rW*;Na9`c??_vCg{-X#-PJVre zAV-9j3#%8^@4L?ZOh!1bUw%I*`n{6GD!M5A{^DDBoDG%d{)vhZv7-)X2Z;WokI;{D zmA~;XI0gUvzu-D>pqAhK2b{D9Quy_WmVps6)K{NdPiV}!!@H!P^wp!Q$x)tDrp)lv9q@0{j3b- zHL{-I%qSb%*)F#i{H%@jRTR~=beGzVWu_T5Gl#51R#a3XsVJ(bkjP{;eG_X-Yu?Yo zKu#k=BsWGj)|R${-*TgcN@U25sf`#jIRB}@snLO!lTRg)-G8*066dc@s@e?;?g#2? zYif7z-hbf8W%+@UD~E%Lt}kKN=?y7_wNpCegwnw7%gvCajJDp&+SbzALQP4-WMf6= zGpsz3QI81~S7M6SCRf)s9z1a1gW4KGE$hI+qnP54LS)^9ibLB_#UEmd1DD&{Ew{B) zSJE`ySaDXY;u26SGtViZa`!&MerEmd_X)dMIK(b|#nsZmt*GKddw0KIx_!OnavRI# z=Bg;-ay(+3SCo;$SD&|Zd3VM&+1nnl#_8jSXh^aD8<& zYS#sC7YAPnNWIo_@&^KR>hF|3rv zkXlwd%$AECW{Yk?*1+4sfh7aj>n&Zs!b{YTJrwLe>5n`16mLz5#~YiA(L*-QK})yz8gb2gNsxsq#E4J7jhH`HKRh?jYuob zix*N3XyOUk3M?t0mX1&jL@eQu%0qqotfdotT6oVyHMNt_#Q>~boCMvU5#+}I5fJ7h zCEAiM&KdSng*BgGbq$;X(#3TdVZgsO2uj;IjR znCLE6NIgX$R3&?kZ^_z$Ye`0E35Qe;VSV1xj(YWxky>(x)M zjh$6Hd#@vLO+PT7jRM8XXr>5c)>#N;>`|FjqsXWy3S{thY`Jz;3+&)v5)syu))UWA znBH0mEyOm4mzRDUCsXTt1bK1`(?gWAPR1{79ED0Y@kN``zoM(s2FB|-HpxXbDKvvC7d11p6mCLC*a2`}d6+fXHy4%}D78V^ z+=_z4%gApL$YhfRGM<>s4ny%Wa*+ZVyq%@P?ciX}K+0ZRXz2kj{Fz7!JwR?^xVS88 z0-7OWWPJWJ%D8;diY8XQl|nZUC0;}>3@7rjKt!QrbP-aBP()$46-$M4JS!p?Syx)l zy#()kAZuTvcmRcXfZ3>92=*RAe8+sOvccg1tKMMNKd49w>c|<0P-3M}Vh&DXBPKEX zD=`wb?+b0Pgf>FCHXiXLZ~(D*?gilc*}!y%QX}9qXk=L0faoh&S7p8Sn!n4Tk;#@% z;fdnXFD)0Ow-u&e@IID4b!2*zGJbjsYhn7iqWW>rSod1me*hO21X&rQNIaNE5wLPosXqf8If8(N|1WCK(}13sBQtM0)+Kd zOY3v6U!qa2T+dj$)}WqAmG6e^-2Fd*`n9I(%6ewGVj2btms+!epL)Ced0}cdEOo~4 z)HvO8INdI2YBERK)WVd_<7oOvE-|axyf<3@(|XKCtC((5Qe{vN-bmcYQtKF zUbG}aKMVdlLdsfM%gi92!7$;H+VYd?3zKGJNjrxpwdN<)ixSbAPK4=V$SDp>F2ee) zv*im|LG`p(sbM%d8P+gd>|w;x^BQRTaqfzhHOwllI801jTGtiAv<_%m?Gb5h1$ym- zY3*@(ad2{w!avWqTzmz1g3VN_Q9RYmXibQs827&d?__ImR#r3D8oi(*iHHO~wK+fa z7A;|F%_2zcYy?sMu+&1`8vo?$&f@Ae;pk{s$;>6T zf#eUj0M#{8(8@|?ip*Ke4la2nKKV(0^3O;iSn{sn$unk)*B%JTlVyNr$b|?l*6o(l zzd_L3ExVebRL)qr(y*Latd#|%pMoy@rOSdc2l#^qg=a{7a*81NLT>V$5y_o|+BGu; z$+_Cqx!Q5K;B?gYt!+QZA$wkD$tjxH+S|MM`Xz14%7xnjD0x;^1+3ylWeoFGP#Oka zg8N#C(JRXs77mzMys-S}!cKhQR^k>^c;#^6Ow_WSy^D`u;t0!*eBl-u%<>9MSR@C` zFI(;?AgBNAiF9A(1k0u~VcJbeUWqKPM3g3L=o*`?uytHv<6wvV>|MMAU>83nCp(AE zU@#dBmP`rLZ65ki_0R(#;)SDTzC#JqTLbzsm?VZPPk7q-of8(|?Buvs@N+aXHz30bosv9RMNLIR zO+{zHd;?=6OIrg%U|MlQ4Xt6{>As6sKUSILlaQL7m6=OpTNW|55l>-u!B|5B?B}c( z82kFlmENwi=doW$QF!3G)vKHZzg6ZI25_25Ng0f(t7xdJ=q%(L3&I;y6*VTyH%8dY ztgG8Eu=V8A{+_d)-Po_4y}{pk)yg%3-%2y{MGCMIsw5|pF^gdtJEk&IG-DlN9NPTh z!F}cVT)S3C*NdzDeO=vW&$hL-oNQ}O+_EJ)D8SpvVQK7E|MmWM>;0Ua9W0j3Axg^= zp=7G*%rQ1y?%=)|-7raI8nT&mb*Rcf8aOW@F*YjDSD^UiwJV^w3lz7)eFMp{u`#eE zVa>`BiWP`ZD8(q&$mJ^z3SRH$;vrD{V&K!>v**rso;Y#5?Zlz@=;)|Gf3H=;5bI)y z7346*R-%ZZH5pON?hW3$*0zgl!yjSXfq3N5pFjQktHHic&zEiVC)vB{EYh1vP}RHg=tX|m$A8h4wJKjsh?iR zvnWn`oK9Sm1RUPKf9HC~9)bEt_f?UaZ*ShZ)_<<+!}wq?$2E55=4Nx%$YezVxj|7y zRZV;5%sG0-);7zRTQ7$rDS>bqL0oKN>KfID;EHd*pdw;jf$H7)^w#a`{oUsdCx&>h zT5E4^VLn$~Qn8UtR#H_{*O@tME>Eqsv%7l`U#;3B5w%H4@zLx3MBwiH_KTa>KJDuI zFfPz@Q;b>)HHd9SSQAtsOEGOHi4l|wntb2L6yEwXxP%?0VaBg?ksS|Cj zM~)sW3ieyG+G+J_Co2o{S?a0;r9$#>H5L|TOBdS>S2NV#N9fvj7QnO|IdXs#=ImZ| zOq%&&MS0=E1@m;xMALqI`{uQQD;K)EdPUMoN-|{hL{olq^NWEi(9Cq=wTgl{e*DOx zYL-Ck{!lo=_`hp>^QNZ8MtWAGeTB1NGDM+CysW5^o}P23o2mtBe!j8^VVrzTMSQV5 zLdjlgY(L6Zf;>|}!9eUsQ5!dJUGHywpPO(!LO%&6Zo&Bv%ml%jX|>cr{5Q6&EQuty zNZdzJ0}sBud%ca9iS=lB!}An36DwCb*jYI8wG98N0{dnZjl_NxH8OPnX8&=ak)a`; zr+hF^*~7A;Fds0(gZ~Pik`v}bu8>61A0vsV)sW!ekl+}6a+o3?=;-Js^wKX%4N4BtA4xu?|6&e9T|j~s zK@5qcG0SA_#HALp^c^QCg@X%G97H{;Fdy)Y@Ra^oa!6{B{<&nYR0q9NvX_2G63{Xp zd<&nRX`q8(z}o?9Bi>l}H^qV?s9FCEPN6M97_U*--$)Hf4g%O-dY5De{WPJMIdJbD zVURFX`02H$PhW)r@`no$CXIF;-L(yo>-0(EGLeBW&eNnNn2xmbZZH+|?CC!VL!3`9 z4i4VCcMY;~4mjv0z!`5riTT+cMrs%5Ak6bLM=1#gjfU<#l6be&;PRJ>0ad^<=!r0% z4ywXQWGMJh$msWhug@fV=%;|MEn4q+!y|Ae zA~AJBIHWNMou(&2r>#a9zYIixmz6Q-^SEBtLL~nC1UPG35W3?c3A*EIgx-r9BcoN^ z0Cf|(3rXU?`;4;_4R#DA=;xgTc8np9=w2R)xBGp=eK=^yO6{sV&001h|{;QSpaMi`_!%ENX@LIOAQrdz1^%%Bte1E;`m<8q*B|eU+6dZ=y3K|i0nD%`7^egEh=HMVV5IjfdWn8$2 zMuP0Gp7uR@`o~6u)d5S=umkhe5&#cQJr7Y; z5B%={1R(bz5P1w8^Yhne%Rz)n4Hr1R{$-j~h~&vy!VX~!K2=2MzeA?5SON=q z&q4@w27-w%P=?^|Pw77|gev(OsRz?@5BQX#%#?Bp3#% z@-$}aSN|d5Of%cR{sUSrVZoZLw4!hv7fTc>rP64QHqh0fE+~9_?_$Tl(7;wu!~x0Q zm<(qhx}@_7m|d9T8X6KY`nKOh)zdKby<*kB)i)9Ki70jvreV78JQbt+Ej|s?y-b2dJ5Qroq3{QVCTy^%2)aYa0PXKc zGr}yLCXEnaFgC(2jqfgPZv|sPS*V1>;YipcUB$!XF6gxqQ=Ue@7}Y#%DpcwIvwR z8U!7gnWeL4>S!pE$XfFa42ediix(T|&(+bMHD7O$oI$bPY%O(-*$ed-$r_Xz8O+zw z(9oPUUw@H|L7AcPVgr3$4eTS)pw!UFV4<#-4w{r`P-19kpr@-d6Vt(gREi|@jzYTR zuq9#eJR9i<| zMNV3psGy=r)GF1PJxg1IEG;dss);7l(Np+M0U4r_mX54;sm@FtRe4xqBdco3XzzekR5&{4x1^>LNaA6L;btDUV5mye`cP=pjHGS3 zbQYV<%GnxYDJ`>nqq~y>VMUXJqqD#JD#w7J1@q0lL&L)&qx^#-fE?~TIu8=bB|j;c;P%>UQ*>HXzQ@f z+r!1l#MEY$AssCx(2nC8v;RS!OfQQmBgWjOL`LG75&b~tjZaTE!{rzsb@pHUWQQ|J z-mAQ!nq5&@UcRfYv8lOX4{h0ETkrIm=CZT`2D@TsWkyus>V+h&ke&3r+_bFRwBiE| z?BwVGdu`SE8@5p>Sy_~<)VP!!c;KhZPhQ$8F1DiUNNpLR|>XIKu?K@&7iEcuvrIJ*7=8p1^W4Ut}(M(vr=zlDfu7h$@a9El41s}Nw0*~fz3;; zmcyPPOLGe=`0r(g+H&%Tcob0W3a^BVyUU=GIO(KOHEA-XR64N z=b2fUOPWKT%Z$`>0O&6Fc)8yYXOg#A2{W>s+AlfJI1##|FK z3wR^L+`@ujR=#Yh@hlB(JtIRyBZxl;h8zR9scn{u90_6um +/*************************************************************************** + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + ***************************************************************************/ +#include "TrackData.h" +#include + +TrackData::TrackData() +{ + mid.x = mid.y = mid.z = 0.0f; + width_l = width_r = 10.0; + angle = 0.0; + step = 5.0; +} + +void TrackData::setWidth(float width) +{ + width_l = width_r = width / 2.0f; + assert(width_r > -width_l); +} + +void TrackData::setLeftWidth(float width) +{ + width_l = width; + assert(width_r > -width_l); +} + +void TrackData::setRightWidth(float width) +{ + width_r = width; + assert(width_r > -width_l); +} + +void TrackData::setStep(float step) +{ + assert(step > 0.0f); + this->step = step; +} + +void TrackData::AddStraight(SegmentList& segments, float length, float end_width_l, float end_width_r) +{ + int N = 1 + (int) floor(length / step); + float s = length / (float) N; + float d_width_l = (end_width_l - width_l) / (float) N; + float d_width_r = (end_width_r - width_r) / (float) N; + float hpi = PI / 2.0f; + for (int i = 0; i < N; ++i) + { + mid.x += s * sin(angle); + mid.y += s * cos(angle); + Point left(mid.x + width_l * sin(angle - hpi), + mid.y + width_l * cos(angle - hpi), + mid.z); + Point right(mid.x + width_r * sin(angle + hpi), + mid.y + width_r * cos(angle + hpi), + mid.z); + segments.Add(Segment(left, right)); + width_l += d_width_l; + width_r += d_width_r; + } + + width_l = end_width_l; + width_r = end_width_r; +} + +/// arc in radians + +void TrackData::AddCurve(SegmentList& segments, float arc, float radius, float end_width_l, float end_width_r) +{ + arc = arc * PI / 180.0f; + float length = fabs(arc) * radius; + int N = 1 + (int) floor(length / step); + float s = length / (float) N; + float d_width_l = (end_width_l - width_l) / (float) N; + float d_width_r = (end_width_r - width_r) / (float) N; + float d_angle = arc / (float) N; + float start_angle = angle; + float hpi = (float) (PI / 2.0); + for (int i = 0; i < N; ++i) + { + mid.x += s * sin(angle); + mid.y += s * cos(angle); + Point left(mid.x + width_l * sin(angle - hpi), + mid.y + width_l * cos(angle - hpi), + mid.z); + Point right(mid.x + width_r * sin(angle + hpi), + mid.y + width_r * cos(angle + hpi), + mid.z); + segments.Add(Segment(left, right)); + angle += d_angle; + width_l += d_width_l; + width_r += d_width_r; + } + width_l = end_width_l; + width_r = end_width_r; + angle = start_angle + arc; +} diff --git a/src/drivers/test_bot/TrackData.h b/src/drivers/test_bot/TrackData.h new file mode 100644 index 0000000..042bc47 --- /dev/null +++ b/src/drivers/test_bot/TrackData.h @@ -0,0 +1,203 @@ +// -*- Mode: c++ -*- +// copyright (c) 2006 by Christos Dimitrakakis +/*************************************************************************** + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + ***************************************************************************/ + +#ifndef TRACKDATA_H +#define TRACKDATA_H + +#include +#include +#include +#include +#include +#include + +class Point +{ +protected: + float _length; +public: + float x; + float y; + float z; + + Point() + { + _length = -1.0f; + } + + Point(float x, float y, float z = 0.0) + { + this->x = x; + this->y = y; + this->z = z; + _length = -1.0f; + } + + float Length() + { + if (_length < 0) + { + _length = sqrt(x * x + y * y + z * z); + } + return _length; + } + + void Normalise() + { + float s = 1.0f / Length(); + x *= s; + y *= s; + z *= s; + _length = 1.0f; + } + + Point& operator=(const Point& rhs) + { + x = rhs.x; + y = rhs.y; + z = rhs.z; + return *this; + } + + Point& operator-=(const Point& rhs) + { + x -= rhs.x; + y -= rhs.y; + z -= rhs.z; + return *this; + } + + Point& operator+=(const Point& rhs) + { + x += rhs.x; + y += rhs.y; + z += rhs.z; + return *this; + } + + Point operator+(const Point& rhs) + { + Point lhs; + lhs.x = x + rhs.x; + lhs.y = y + rhs.y; + lhs.z = z + rhs.z; + return lhs; + } + + Point operator-(const Point& rhs) + { + Point lhs; + lhs.x = x - rhs.x; + lhs.y = y - rhs.y; + lhs.z = z - rhs.z; + return lhs; + } + + Point& operator*=(const float& rhs) + { + x *= rhs; + y *= rhs; + z *= rhs; + return *this; + } + + Point operator*(const float& rhs) + { + Point lhs; + lhs.x = x*rhs; + lhs.y = y*rhs; + lhs.z = z*rhs; + return lhs; + } + + Point& operator/=(const float& rhs) + { + x /= rhs; + y /= rhs; + z /= rhs; + return *this; + } + + Point operator/(const float& rhs) + { + Point lhs; + lhs.x = x / rhs; + lhs.y = y / rhs; + lhs.z = z / rhs; + return lhs; + } + +}; + +class Segment +{ +public: + Point left; + Point right; + + Segment(Point left, Point right) + { + this->left = left; + this->right = right; + } +}; + +class SegmentList +{ +protected: + std::vector segments; +public: + + void Add(Segment segment) + { + segments.push_back(segment); + } + + Segment& operator[](int i) + { + return segments[i]; + } + + int size() + { + return segments.size(); + } + + void PrintSegments() + { + int N = segments.size(); + for (int i = 0; i < N; ++i) + { + printf("%f %f %f %f\n", + segments[i].left.x, segments[i].left.y, + segments[i].right.x, segments[i].right.y); + } + } +}; + +class TrackData +{ + float width_l; + float width_r; + float angle; + float step; + Point mid; +public: + TrackData(); + void setWidth(float width); + void setLeftWidth(float width); + void setRightWidth(float width); + void setStep(float step); + void AddStraight(SegmentList& segments, float length, float end_width_l, float end_width_r); + void AddCurve(SegmentList& segments, float arc, float radius, float end_width_l, float end_width_r); +}; + +#endif diff --git a/src/drivers/test_bot/Trajectory.cpp b/src/drivers/test_bot/Trajectory.cpp new file mode 100644 index 0000000..7d635c9 --- /dev/null +++ b/src/drivers/test_bot/Trajectory.cpp @@ -0,0 +1,317 @@ +// -*- Mode: c++ -*- +// copyright (c) 2006 by Christos Dimitrakakis +/*************************************************************************** + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + ***************************************************************************/ +#include +#include +#include +#include +#include +#include +#include "Trajectory.h" +#include + + + +/// Return a point + +Point Trajectory::GetPoint(Segment& s, float w) { + float v = 1.0f - w; + return Point(w * s.left.x + v * s.right.x, + w * s.left.y + v * s.right.y, + w * s.left.z + v * s.right.z); +} + +#define EXP_COST +#undef DBG_OPTIMISE +/// Optimise a track trajectory + +void Trajectory::Optimise(SegmentList track, int max_iter, float alpha, const char* fname, bool reset) { + int N = track.size(); + clock_t start_time = clock(); + int min_iter = max_iter / 2; // minimum number of iterations to do + float time_limit = 2.0f; // if more than min_iter have been done, exit when time elapsed is larger than the time limit + float beta = 0.75f; // amount to reduce alpha to when it seems to be too large + w.resize(N); + dw.resize(N); + dw2.resize(N); + indices.resize(N); + accel.resize(N); + + // initialise vectors + int i; + for (i = 0; i < N; ++i) { + if (reset) { + w[i] = 0.5f; + } + dw2[i] = 1.0f; + indices[i] = i; + } + + + // Shuffle thoroughly +#if 1 + srand(12358); + for (i = 0; i < N - 1; ++i) { + int z = rand() % (N - i); + int tmp = indices[i]; + indices[i] = indices[z + i]; + indices[z + i] = tmp; + } +#endif + + //float prevC = 0.0f; + float Z = 10.0f; + float lambda = 0.9f; + float delta_C = 0.0f; + float prev_dCdw2 = 0.0f; + + for (int iter = 0; iter < max_iter; iter++) { + + float C = 0.0f; + float P = 0.0f; + float dCdw2 = 0.0f; + float EdCdw = 0.0f; + + float direction = 0.0; + for (int j = 0; j < N - 1; ++j) { + int i = indices[j]; //rand()%(N-3) + 3; + int i_p3 = i - 3; + if (i_p3 < 0) i_p3 += N; + int i_p2 = i - 2; + if (i_p2 < 0) i_p2 += N; + int i_p1 = i - 1; + if (i_p1 < 0) i_p1 += N; + //int i_n3 = (i + 3)%N; + int i_n2 = (i + 2) % N; + int i_n1 = (i + 1) % N; + //Segment s_prv3 = track[i_p3]; + //Segment s_prv2 = track[i_p2]; + //Segment s_prv = track[i_p1]; + Segment s_cur = track[i]; + //Segment s_nxt = track[i_n1]; + //Segment s_nxt2 = track[i_n2]; + //Point prv3 = GetPoint(track[i_p3], w[i_p3]); + Point prv2 = GetPoint(track[i_p2], w[i_p2]); + Point prv = GetPoint(track[i_p1], w[i_p1]); + Point cur = GetPoint(track[i], w[i]); + Point nxt = GetPoint(track[i_n1], w[i_n1]); + Point nxt2 = GetPoint(track[i_n2], w[i_n2]); + //Point u_prv2 = prv2 - prv3; + Point u_prv = prv - prv2; + Point u_cur = cur - prv; + Point u_nxt = nxt - cur; + Point u_nxt2 = nxt2 - nxt; + u_prv.Normalise(); + u_cur.Normalise(); + u_nxt.Normalise(); + u_nxt2.Normalise(); + //float l_prv2 = (prv2 - prv3).Length(); + float l_prv = (prv - prv2).Length(); + float l_cur = (cur - prv).Length(); + float l_nxt = (nxt - cur).Length(); +#if 1 + Point a_prv = (u_cur - u_prv) / l_prv; + Point a_cur = (u_nxt - u_cur) / l_cur; + Point a_nxt = (u_nxt2 - u_nxt) / l_nxt; +#else + Point a_prv = (u_prv - u_prv2) / l_prv2; + Point a_cur = (u_cur - u_prv) / l_prv; + Point a_nxt = (u_nxt - u_cur) / l_cur; +#endif + + float current_cost = a_prv.Length() * a_prv.Length() + + a_cur.Length() * a_cur.Length() + + a_nxt.Length() * a_nxt.Length(); + + //accel[i] = +a_nxt.Length(); + accel[i] = (a_prv.Length() + a_cur.Length() + a_nxt.Length()) / 3.0f; + C += current_cost; + + float dCdw = 0.0; + + if (1) { + // Done only for a_cur, ignoring other costs. + { + Point lr = s_cur.left - s_cur.right; + Point d = cur - prv; + float dnorm2 = d.x * d.x + d.y * d.y; + float dnorm = sqrt(dnorm2); + float dxdynorm = d.x * d.y / dnorm; +#ifdef EXP_COST + float tmp = exp(a_cur.x * a_cur.x + a_cur.y * a_cur.y); + dCdw += tmp * a_cur.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw += tmp * a_cur.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#else + dCdw += a_cur.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw += a_cur.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#endif + } + { + Point lr = s_cur.left - s_cur.right; + Point d = nxt - cur; + float dnorm2 = d.x * d.x + d.y * d.y; + float dnorm = sqrt(dnorm2); + float dxdynorm = d.x * d.y / dnorm; +#ifdef EXP_COST + float tmp = exp(a_cur.x * a_cur.x + a_cur.y * a_cur.y); + dCdw += tmp * a_cur.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw += tmp * a_cur.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#else + dCdw += a_cur.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw += a_cur.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#endif + } + } + + if (1) { + { + Point lr = s_cur.left - s_cur.right; + Point d = nxt - cur; + float dnorm2 = d.x * d.x + d.y * d.y; + float dnorm = sqrt(dnorm2); + float dxdynorm = d.x * d.y / dnorm; +#ifdef EXP_COST + float tmp = exp(a_nxt.x * a_nxt.x + a_nxt.y * a_nxt.y); + dCdw -= tmp * a_nxt.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw -= tmp * a_nxt.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#else + dCdw -= a_nxt.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw -= a_nxt.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#endif + } + } + + if (1) { + { + Point lr = s_cur.left - s_cur.right; + Point d = cur - prv; + float dnorm2 = d.x * d.x + d.y * d.y; + float dnorm = sqrt(dnorm2); + float dxdynorm = d.x * d.y / dnorm; +#ifdef EXP_COST + float tmp = exp(a_prv.x * a_prv.x + a_prv.y * a_prv.y); + dCdw -= tmp * a_prv.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw -= tmp * a_prv.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#else + dCdw -= a_prv.x * lr.x * (dnorm + d.x / dnorm + dxdynorm); + dCdw -= a_prv.y * lr.y * (dnorm + d.y / dnorm + dxdynorm); +#endif + } + } + float K = 10.0; + float penalty = 0.0; //K*(0.5f - w[i])*(exp(fabs(0.5-w[i]))-1); + if (1) { + float b = 0.1f; + if (w[i] < b) { + penalty += K * (b - w[i]); + } else if (w[i] > 1.0 - b) { + penalty += K * ((1.0 - b) - w[i]); + } + } + P += K * penalty*penalty; + dCdw += K*penalty; + dw2[i] = lambda * dw2[i] + (1.0 - lambda) * dCdw*dCdw; + direction += dCdw * dw[i]; + float delta = dCdw / (dw2[i] + 1.0); + dw[i] = delta; + w[i] += alpha * delta; + + if (1) { + float b = 0.0; + if (w[i] < b) { + w[i] = b; + } else if (w[i] > 1.0 - b) { + w[i] = 1.0 - b; + } + } + + dCdw2 += dCdw*dCdw; + EdCdw += delta / (float) N; + } // indices + + + if (direction < 0) { + alpha *= beta; +#ifdef DBG_OPTIMISE + fprintf(stderr, "# Reducing alpha to %f\n", alpha); +#endif + } + Z = (dCdw2); + if (Z < 0.01) { + Z = 0.01f; + } + + + + bool early_exit = false; + delta_C = 0.9 * delta_C + 0.1 * fabs(EdCdw - prev_dCdw2); + prev_dCdw2 = EdCdw; + + if (delta_C < 0.001f) { + early_exit = true; + } + + if (iter % 100 == 0) { + clock_t current_time = clock(); + float elapsed_time = (float) (current_time - start_time) / (float) CLOCKS_PER_SEC; + if (elapsed_time > time_limit) { + early_exit = true; + } + +#ifdef DBG_OPTIMISE + fprintf(stderr, "%d %f %f %f %f %f %f\n", + iter, + C / (float) N, + P / (float) N, dCdw2, EdCdw, delta_C, elapsed_time); +#endif + } + + if (iter > min_iter && early_exit) { +#ifdef DBG_OPTIMISE + fprintf(stderr, "# Time to break\n"); + fflush(stderr); +#endif + break; + } + //prevC = C; + } + + + +} + + +#if 0 +// example + +int main(int argc, char** argv) { + if (argc != 3) { + fprintf(stderr, "usage: optimise_road iterations learning_rate\n"); + exit(-1); + } + int iter = atoi(argv[1]); + float alpha = atof(argv[2]); + TrackData track_data; + SegmentList track; + + track_data.setStep(10.0f); + + float width_l = 9.0f; + float width_r = 9.0f; + track_data.setWidth(19.0f); + + track_data.AddStraight(track, 50.0, width_l, width_r); //1 + track_data.AddCurve(track, 90.0, 100, width_l, width_r); //2 + + Optimise(track, iter, alpha); + + return 0; +} +#endif diff --git a/src/drivers/test_bot/Trajectory.h b/src/drivers/test_bot/Trajectory.h new file mode 100644 index 0000000..d34f78c --- /dev/null +++ b/src/drivers/test_bot/Trajectory.h @@ -0,0 +1,47 @@ +// -*- Mode: c++ -*- +// copyright (c) 2006 by Christos Dimitrakakis +/*************************************************************************** + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + ***************************************************************************/ + +#ifndef TRAJECTORY_H +#define TRAJECTORY_H + +#include +#include +#include +#include +#include +#include +#include "TrackData.h" + +class Trajectory +{ +public: + std::vector w; ///< parameters + + std::vector dw; ///< parameter steps + + std::vector dw2; ///< parameter gradients + + std::vector accel; ///< maximum acceleration + + std::vector indices; ///< data indices + + static Point GetPoint(Segment& s, float w); + + void Optimise( + SegmentList track, + int max_iter, + float alpha, + const char* fname, + bool reset = true + ); +}; + +#endif diff --git a/src/drivers/test_bot/build_bot b/src/drivers/test_bot/build_bot new file mode 100755 index 0000000..f2e99d4 --- /dev/null +++ b/src/drivers/test_bot/build_bot @@ -0,0 +1,9 @@ +#! /bin/sh + +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib +export TORCS_BASE=/home/michaelheinrich/plib/torcs/torcs-1.3.7 +export MAKE_DEFAULT=$TORCS_BASE/Make-default.mk + +make clean +make +make install diff --git a/src/drivers/test_bot/logo.rgb b/src/drivers/test_bot/logo.rgb new file mode 100644 index 0000000000000000000000000000000000000000..036f1e29da4c7df97542679bc26aba03da84d923 GIT binary patch literal 18550 zcmeI333Qy-Rmb1tec#MTwk6xKlekgZ>}e{;veQyjH;vP#iztOIQ&pvT?e^H$NfAW8#2YT)@rl$wm0lf%f z9=z%IpsyJ7#(#r08}o)vV_yF~#@zYi#=LH?F|VC8=8ivrZZu|B4f-7Plh8Y%*FaL= zZp`if3jHxO5B)myX=7gVFUH(9Wz4I=*}2V_?!PkTyFYKtD_?KSEB?%wmrodTD|7JH zJW1j=9{T%De+f-OV7&2%82@`1|Bo^L z^v&zA`?dc8-Ng9+2;)p&@1l=)eH6lmU5w){{M>aJ+KM>bm4Q?M#lR;A^h6L_->&XS}hw=iSh2 zp_f6IGVcEZG0tz=&G>(f@juS^{~_c5U(kz~1FvEHX}ja|5dFRD7&HPg7Q5)%U9W`j za~JdX_A}7$LetP!p}QG>`t~-){nhl*t5^%V?_&+v%J|d&ufV65)9<(bCiDXk`fjz1 z>t924=m<0h(U&{!hnUklz}kWRJFtJp2FCP1pg)IBL-ft|QRrjPUg$#*&&l>(5d7Qe z$L)r31;6XJA$;k=&aQ_cZ0Y)Gi2mui6WRuCfjq|i?;!lzR)y%_ZRo${7Z~>l0(UF8 zo%G$ym_Ik~hq@U1e}EW|8?pZeWN$$Bh7T|Yu4n9j7^3g4#pY}9?P~h#C1u7Rz8Bxh z_|s=sUB>t`URVAt^!<$g6O8{qG5-G!UBw(&%N$_*Heb#7WACNR|Bd*)VI0D~_4h!G zQTx9#_TPe9aXhsOF)n63bQ|;*2wmpQ(D%l9u(ZrPFjqptE3_Kcxpm(S-;Y5*1^sNC zewcI*^z+b`pxd5JXWnQQ<9@i_lu zaVol*kA^X6$}oq_tD(0;%scZ6h_N)+La%~ug0@3tZ`bFwwF32P?D;^PN?vfqP8t7(t;^H3)VD*TPsUxI{|g!a!5pc9fY zCzcq|dq>wi;MPD|Kh2|#Z#vi{ZmD9 zWKyU0X$4s+N=uGkd&*n9Op7IyhoC?U=#T-?wrxFsUeYfn3Mvz@{|>^X_?q4vI69R7YnZu6+y;FYLk;n;edeQfXg7}sp)Eg zg|p>UL2dR<4N5A?#k2qw_~5i?HW8!G=NA^{=j}pao^-*RAEcz;g#|kwE>IIJtex+t zO+Nm7(KTAC?Yy@T&XWYnk}x~PB|#PP)MS!^wxk?(8e>Yz%a2d1x6lgMS>ps)%CT87+=4SyP;oZda4+d} z*!jeQTE6$hYDv{qE=x+8scHF4+o>E$YN|LrCFSIlM^*o{D|zIorrw_lrdyQN3A9WV zv_LZn5(zsUG+T2jDp=s> zaKI^xE%(qiptg#|qR&IclpjMoGesgPzKZc;vz;nQRzzA+WMj8fG$DF|%-*BVNOGBS zIaMi?%auyGwelrW_pP{$B((62Gs>8*gyphzKS9~61Z5veNreyT@g3b6Yn;cEfPp(07@Vb6p&IUw4VV!gJ>oy0aL?@#7MaY{t&WO&whZvq2F zT(o-P3k6SeBH8Gu0V(J=?gjO5?_;YZwiecFww|xmYb0JRsQa~2y;kR2t9kWwtw^eE zMM>**3yW79QWmYK`Sl!4UTs8V8lPGcPOl!;>d9K7j^ca>K4nS3Ag~VwLz(N%vK$M+4V;)QPW<%N+Z;UIK*oBTkGPP1T3Kx)Cj@B;#w_9 zs6>1)6Gu-8&(fxmR-&~8HHoMqyuK0HofZj#BG%?&oCr$n_KAOzzubX1-BKYz8TJkd zksc*We5Pjclm^K#vzR`Ku9Nx2lP4FwlUUGpvS0Fih^URqlrRu%;T7_QQ8M=AkZ{U} zW=l;gVu_P@2Xs=|Vcp=^Vm>EGb&B?Ns8ay&ie&iOhBbn(hNVv8G#W&Kg0t|532Uhr zP^tG-8SXWCRaC0Qjy1vx{08z?S)2zgV(UQ0#cm1h;lg}!q1Xt=$%SCPg`EH)Hz2MH zQLqa+-3`PAVzUe2zSPgBKp7x@OP$+(NbHXH2?Cgc+7~}FI)Iv61~t=1G%dj1{2H=8?Cf{slXU*6vX)rQ zP{Sql00luNHJ7hGmdm!?Q^{FKzVMcqdG>=I2i=sb| z{JiLEhd?bqoQ;i0Q?YigFn`v7R8#)M+}u+)UglrYX8JGLc-`0LPMq-Oyc0Rnj~FoUIqCvQ^hf?``7 z&kIL#ZUh8HiqT7o4z711FD>@OpeQ-M4E)ZrYEHO)8Lrd*l!G4v{;R5b4#`k{ih?qS z!s3Z#4orx_P~%s#v5tNmxU($&Yv20E!^f|@%mn+_9S>$bN5hwyu>0xt$8)pCb7XrC z`^Q)C9!SFHL#@XH$J>59KYKpr%atkD{q$^mjF{^=j4B}eGu7GI>fyIuZgN|{IqOw( zv(;H!^=Cs#!pGNLZb~1nR;iS-g&}zK3RC!HsYt>3iB_|+kB%v7s0)yKD( z;J#Efm=VH^nd&>JnZbaWBt-h^Eyfosm+>p#OuAZ{nGp;TNh?G+19x7k5;Mz1^7p~F zR`J3axNS8*Q=OSC5~ZRkYQ(yF678S!*D1cjBlfUiaPHY z8+!p0uS8Nd+r^AvAsXDXMkR|XhfEFxP98KT`8_7R3+Gfr-?8xVwG4hL*nBK}|0S23 zV8e!E!JC;OoBY_K=gkpjj@YB&ayE+_IS|BhU&J{mm1E0?6kjSTKonGwE%#T-rPrwC{aAk2 zlhOwKaAvYxo)j88u)pn-x&2>0*iz;o1U!}AWU`!p`ms+v-1f=BXTDAar=xN(=|ib< z{;5az(=hPaCy*A9L~L&vCQc?_c?7`Xr@lO{Ksh)>))QcXeM(_-z|u!FME>4yO zciks*4}bOvUrbS0PIbtD`Kh)~4q>D)i%$9$c{R8=Ir;QsVv%6BG*$V|Pyzhh6RN$O znAAb@^_VE9L7}TT-EjumNH*;bdTYqaxhdsZ^k`(=;V%_8t#K!=wVb*(Q{~Qt!^=;6 zsrUftj&y=8Y^vY76K8xv6G*Y61(MTg!!Cbf(4GI}Hz}4)Dv0Nd6npjDr^my5=Fq`| z{-NB#L;U6slGsB5G(e7)LsEV~@>1l-)d+`Bc<_)t*zanQ51`?gRiG#qRRXTykblsF zBXP*NqVlMvNd)bom%b13$kTI``A2iY{mr*%lp(<T$aj%hg4%O zbMR0KIPYLVT!Cf3rplo|QVTbqmP3P*k-S(m=rU694i%&Thb5mU7aK4ArzR&3s(X7^ zO&)V?D-!SYV6ArgwEKFs{?n8wq-%i^XVKG~m!(#!{aSeXG-v4394=iUNud>Tl`zVu zMthDVaT<+jKU~@=0FZ$6%23sA)FfA7VaB8f~?yF5J~k$wbk8_R=AeTH*P4cT&Ole zx?GparKu;klw}J0v@K?+R2qTOU6z8cH>9v?L{G|9rmp~NRDS@1+{yB_l_3f1^3{@D zjiQNI-bj6iUgXjkCb{&r%}Y}2?rc#Q#v2Hb;~U{kdbJwg*ot=9TolA40Gu<#Xemn{ zRP@F;pCKf95})+L&)F;0)WnEHD!qJ^>q}`#a`o5RXf)3_c7+vdfvllN7r6A(02%U+`q-^E0K z;(^$W2Hyr3lEuymjrnQrWF3#}0OS%`?_eb|%N9bek%Pv)@v66~ZpCCxmi2f&lw$ex zS%-_d{!*0XR#$gr(wtjvRLm=Z2Ad9qy4=zQ8iRw5C zDLbwGaU^rAeT?LcQcxEI0vDJ#iD5<@KL#Wsh$1SIjY_V4$!b&*R!PXasVwU0sUcm4g=0zsf-KRp9J^mx_E~!E)Sz&%6hm>G z$V=Jr#cQwzAc+M1!V9Y}D>fg0N||N3_FOhC_;41KPokaED=dOAEVhb1`IWbFz8v%i zc{|8Q>O`dRW^pj)qe&f00+1JJd38uCuMXpkU2aMIUeTH5UN$DD^qyqZ*LNII-GVD` z({!zm->-S{I!zW-S0=N*vHYqxPmjXrcl= z=t{z>kxy(9i#42SBor@&($w>6QG<_~PtSeH?RZuu-*S~J6LrT0ZOVHh8W9u3S=V=tw^E4&H{N0_Mcc)yk zCCXJtN7qF#_eN1KN+dzaSQMdTCG)1p4|6@8o$Ca9RrV;qG&;JJa>=u^hk5?|st;2n z!IaT#c3qS*{T;pC0`Hr!w`0A@b@%qBO`*4ABwDicLT_(JQcYVXth)HStaagqibwf+ zHe1J3NiJI4hW&f`?iK_5Fznmog=?eSQoWvz3iUJN*Zgsd0w=CxAa{HHFQ_hVP?tE-YspxZqJlP=utYdgTD{IV5PA^o#CE# z&jg)5o%!6AMp4~n9~l|fE)%b zMWbFesCrRpoIc06GhXDIujm}q*+`G><}V-JJY6zNbaJ;&&?p*_?%tg=MUs);Zl1jh z*J*+tFD&tgtUU}pkID1U8AbRpYO^^y*URp&%A{S@&lgVTS3^3pC+u-kmzuR3sU%FD2>O5$(yiJ0olMX;ykvCTwpiD(ECmnNU`Ma7pIug=@p8 zWLRXm+u3(_l0JP-+T7Epvs27x?z(yV4BuV|ZTqz6+z)o!aG&<1^q@@6dMaDc37d*S zSr5WtouB9W^^o==lXEtj)yX>I^%i*kqo~Ccdq-FhM(D>2)5V|VhWgNl?K&@?qb}EL zao1&Sv`@3=`1JzmI=2rW(mrG=9>^Zh;T!kr1)lV5wk0ao$9eR}Jr>d7h^P}sAf{r! z>%`rbMbnz$B^pPfSqV>s-cs+#h><0BwSGLGKK_?LSnS(*74y+!{d~^)&h5*Gv@f6O z-wUNL4Ea0zzBn*RymkFqwXBpj8XhJZjb{(YTAocsrTX}IBJ?Yf3!Q65P-B;TfAHDA)aLarlhuzhFF>4S1?kW@z`=h+z|F*vYY<>mWhA6K zO`&^aY>ZjywtQ@z#=_kl?1e5&jkpu+q{n!mpPQN}8n@XJ^bFZ&YCBi#+nh)My@>P6{|@dAJXAdCWv#V*oFkv)5?96U%LdhHu(>F%^%^sT}-QFAi!Yk@$$&|okqXG8t5hc^B z8zcTRl=!eJ5-a4!@S>1#=!HSr5I%x-9cMx>p+PrNGKLZ(#J^ND+-=zI&C0gK^sb2p zHL@;=cR2NJ0AjbE4Ip|1O*&=Hwcr8)!jZEsoT;*@9;_2NfQ&tOT*g692i?2JI`rVh z5RJ*f%N@fwaCL}_-D||3^P$DoTpU_7a@M&Owmm1bWrM||bD*vPk@$Xwj1Cr_jJeZ~ z=^b0L(L-^G!7@hL#!^wxt8Shh_j90P%Pt&h*#JIYsPN;>p%j-~@Rx|I-aXp%ZY)~b z-55ySyu{u`k>4A1sDG;>dD&d--o;==A8!ugGOL~&Ryf%7@QjxoVL9Z^fUB0q2r|9m zF=N*DGH&ONAcwE9-dU~()XVds#a><D!I)cQ&W9D7b`e<7BVx#Ec3m{q=CECFA`TD`sRQg?x? oWt$C6=cTgEx?Y;k7B`0*+#TR1$L&gcHVNyE+T!y61KX_s23oVEF#rGn literal 0 HcmV?d00001 diff --git a/src/drivers/test_bot/make_clean b/src/drivers/test_bot/make_clean new file mode 100755 index 0000000..8f60c91 --- /dev/null +++ b/src/drivers/test_bot/make_clean @@ -0,0 +1,7 @@ +#! /bin/sh + +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib +export TORCS_BASE=/home/michaelheinrich/plib/torcs/torcs-1.3.7 +export MAKE_DEFAULT=$TORCS_BASE/Make-default.mk + +make clean diff --git a/src/drivers/test_bot/optimal_line.cpp b/src/drivers/test_bot/optimal_line.cpp new file mode 100644 index 0000000..58ae954 --- /dev/null +++ b/src/drivers/test_bot/optimal_line.cpp @@ -0,0 +1,1091 @@ +#ifdef _WIN32 +#include +#endif + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "optimal_line.h" +#include "Trajectory.h" + +/** + * Get the displaced position of the spot on the line + */ +v2t +LineSegment::pos(float offset) +{ + v2t p = (1 - optimalSpot - offset) * left + (optimalSpot + offset) * right; + return p; +} + +/** + * Get the position of the spot on the line + */ +v2t +LineSegment::pos() +{ + v2t p = (1 - optimalSpot) * left + (optimalSpot) * right; + return p; +} + +/** + * Access segments with wrapping indices. Indices smaller zero and greater + * than the segment count are wrapped around to access neighbors in circular + * track. + */ +LineSegment * +TrackModel::getSegment(int index) +{ + int ind = index; + int size = segments.size(); + if (size == 0) + { + return NULL; + } + + while (ind < 0) + { + ind += size; + } + + + while (ind >= size) + { + ind -= size; + } + + return &segments[ind]; +} + +/** + * + */ +void +TrackModel::initialize(tTrack* track, float maxBreakG, float maxSideG, float suctionParam) +{ + // creal old data first + segments.clear(); + segmentIndices.clear(); + + this->suctionParam = suctionParam; + + // check if we have valid new data + if (track == NULL) + { + return; + } + + std::cout << "optimal_line.cpp: Version 0.07\n"; + + tTrackSeg *first = track->seg; + tTrackSeg *current = first; + + for (int i = 0; i < track->nseg; i++) // limit maximum segments + { + if (current == NULL) + { + break; // error + } + + // initialize LineSegment from track data + + LineSegment seg; + + seg.left.x = current->vertex[0].x; + seg.left.y = current->vertex[0].y; + + seg.right.x = current->vertex[1].x; + seg.right.y = current->vertex[1].y; + + seg.optimalSpot = 0.5; + + seg.type = current->type; + seg.friction = current->surface->kFriction; + + segments.push_back(seg); + + segmentIndices[current] = i; + + /* + std::cout << "optimal_line.h: " + << "i=" << i + << ", lx=" << seg.left.x + << ", ly=" << seg.left.y + << ", rx=" << seg.right.x + << ", ry=" << seg.right.y + << "\n"; + */ + + + // iterate linked list of segments + tTrackSeg *next = current->next; + + current = next; + } + + /* + costOptimization(); + filterLine(); + filterLine(); + filterLine(); + clipLineToLimits(); + //optimizeLine(); + */ + + getTorcsTrajectory(track); + + clipLineToLimits(); + filterLine(); + + // fix problems at finish line + filterFinishLine(); + + + fillRadii(); + + + filterRadii(); + fixFinishRadii(); + fillPointLimits(maxSideG); + propagateRationalLimit(maxBreakG); + + dumpDataToFile("OptimalLineData.csv"); + dumpTrackToFile(track, "TrackGeom.csv"); + + //printRadii(); + return; +} + +const float weightsUnnorm[7] = {1, 1.5, 2, 2.1, 2, 1.5, 1}; +const float weightNorm = 1.0 / (weightsUnnorm[0] + + weightsUnnorm[1] + + weightsUnnorm[2] + + weightsUnnorm[3] + + weightsUnnorm[4] + + weightsUnnorm[5] + + weightsUnnorm[6]); + +const float weights[7] = { + weightsUnnorm[0] * weightNorm, + weightsUnnorm[1] * weightNorm, + weightsUnnorm[2] * weightNorm, + weightsUnnorm[3] * weightNorm, + weightsUnnorm[4] * weightNorm, + weightsUnnorm[5] * weightNorm, + weightsUnnorm[6] * weightNorm +}; + +float +TrackModel::filterSegment(int index) +{ + float vn3 = getSegment(index - 3)->optimalSpot; + float vn2 = getSegment(index - 2)->optimalSpot; + float vn1 = getSegment(index - 1)->optimalSpot; + float v = getSegment(index)->optimalSpot; + float vp1 = getSegment(index + 1)->optimalSpot; + float vp2 = getSegment(index + 2)->optimalSpot; + float vp3 = getSegment(index + 3)->optimalSpot; + + float s = weights[0] * vn3 + + weights[1] * vn2 + + weights[2] * vn1 + + weights[3] * v + + weights[4] * vp1 + + weights[5] * vp2 + + weights[6] * vp3; + + return s; +} + +float +TrackModel::filterSegmentSpatial(int index) +{ + LineSegment *seg = getSegment(index); + + v2t vn3 = getSegment(index - 3)->pos(); + v2t vn2 = getSegment(index - 2)->pos(); + v2t vn1 = getSegment(index - 1)->pos(); + v2t v = seg->pos(); + v2t vp1 = getSegment(index + 1)->pos(); + v2t vp2 = getSegment(index + 2)->pos(); + v2t vp3 = getSegment(index + 3)->pos(); + + v2t p = weights[0] * vn3 + + weights[1] * vn2 + + weights[2] * vn1 + + weights[3] * v + + weights[4] * vp1 + + weights[5] * vp2 + + weights[6] * vp3; + + float toL = (seg->left - p).len(); + float toR = (seg->right - p).len(); + + float w = (seg->left - seg->right).len(); + + float r = -(toL * toL - toR * toR - w * w) / (2 * w); + float l = w - r; + + float s = l / w; + + return s; +} + +void TrackModel::filterSegments(int startInd, int stopInd) +{ + int n = stopInd - startInd; + + float tmpOpt[n]; + + for (int i = 0; i < n; i++) + { + tmpOpt[i] = filterSegmentSpatial(i + startInd); + //tmpOpt[i] = filterSegment(i + startInd); + } + + for (int i = 0; i < n; i++) + { + getSegment(i + startInd)->optimalSpot = tmpOpt[i]; + } +} + +/** + * There is a problem with the optimization convergence on the start and finish + * line. We compensate that by overriding all the values on the start and finish + * straight with 0.5 and then filter the coefficients. + */ +void TrackModel::filterFinishLine() +{ + int start = -1; + int end = 1; + + // scan backwards + for (size_t i = 0; i < segments.size(); i++) + { + int index = -i; + LineSegment *seg = getSegment(index); + + if (seg->type != TR_STR) + { + start = i + 1; + break; + } + } + + + // scan forwards + for (size_t i = 0; i < segments.size(); i++) + { + int index = i; + LineSegment *seg = getSegment(index); + + if (seg->type != TR_STR) + { + start = i - 1; + break; + } + } + + for (int i = start + 1; i <= end - 1; i++) + { + getSegment(i)->optimalSpot = .5; + } + + filterSegments(start, end); + filterSegments(start, end); + filterSegments(start, end); + filterSegments(start, end); +} + +void +TrackModel::filterLine() +{ + float tmpOpt[segments.size()]; + + + for (size_t i = 0; i < segments.size(); i++) + { + float vn3 = getSegment(i - 3)->optimalSpot; + float vn2 = getSegment(i - 2)->optimalSpot; + float vn1 = getSegment(i - 1)->optimalSpot; + float v = getSegment(i)->optimalSpot; + float vp1 = getSegment(i + 1)->optimalSpot; + float vp2 = getSegment(i + 2)->optimalSpot; + float vp3 = getSegment(i + 3)->optimalSpot; + + float s = weights[0] * vn3 + + weights[1] * vn2 + + weights[2] * vn1 + + weights[3] * v + + weights[4] * vp1 + + weights[5] * vp2 + + weights[6] * vp3; + + tmpOpt[i] = s; + + } + + for (size_t i = 0; i < segments.size(); i++) + { + getSegment(i)->optimalSpot = tmpOpt[i]; + } +} + +void +TrackModel::filterRadii() +{ + float tmpOpt[segments.size()]; + + float weights[7] = {1, 1.5, 2, 2.1, 2, 1.5, 1}; + + float sum = weights[0] + + weights[1] + + weights[2] + + weights[3] + + weights[4] + + weights[5] + + weights[6]; + + sum = 1.0 / sum; + + weights[0] = weights[0] * sum; + weights[1] = weights[1] * sum; + weights[2] = weights[2] * sum; + weights[3] = weights[3] * sum; + weights[4] = weights[4] * sum; + weights[5] = weights[5] * sum; + weights[6] = weights[6] * sum; + + for (size_t i = 0; i < segments.size(); i++) + { + float vn3 = getSegment(i - 3)->radiusOfCurve; + float vn2 = getSegment(i - 2)->radiusOfCurve; + float vn1 = getSegment(i - 1)->radiusOfCurve; + float v = getSegment(i)->radiusOfCurve; + float vp1 = getSegment(i + 1)->radiusOfCurve; + float vp2 = getSegment(i + 2)->radiusOfCurve; + float vp3 = getSegment(i + 3)->radiusOfCurve; + + float s = weights[0] * vn3 + + weights[1] * vn2 + + weights[2] * vn1 + + weights[3] * v + + weights[4] * vp1 + + weights[5] * vp2 + + weights[6] * vp3; + + tmpOpt[i] = s; + + } + + for (size_t i = 0; i < segments.size(); i++) + { + getSegment(i)->radiusOfCurve = tmpOpt[i]; + } +} + +void +TrackModel::fixFinishRadii() +{ + for (int i = -10; i < 10; i++) + { + float radius = getSegment(i)->radiusOfCurve; + if (radius < 1000) + { + getSegment(i)->radiusOfCurve = 1000; + } + } +} + +void +TrackModel::getTorcsTrajectory(tTrack *track) +{ + + const int N = track->nseg; + + const int iterations = 10 * N; + + float avgSegLen = track->length / N; + + std::cout << "nSegs=" << N << ", its=" << iterations << ", len=" << + track->length << ", avgSegLen=" << avgSegLen << "\n"; + + Trajectory trajectory; + SegmentList segment_list; + + tTrackSeg* seg = track->seg; + seg = track->seg; + + float lengthLimit = 10; + float length = lengthLimit; + + int indices[N]; + int id = 0; + + + for (int i = 0; i < N; i++, seg = seg->next) + { + length += seg->length; + indices[i] = id; + + if (length >= lengthLimit) + { + Point left(seg->vertex[TR_SL].x, seg->vertex[TR_SL].y); + Point right(seg->vertex[TR_SR].x, seg->vertex[TR_SR].y); + segment_list.Add(Segment(left, right)); + id++; + length = 0; + } + + } + + trajectory.Optimise(segment_list, iterations, 0.02f, "/tmp/result"); + + /** + * position in the index array, where the segment started + */ + int segmentStartIndS = 0; + + /** + * segment index of the current position + */ + int lastSegmentIndexT = 0; + + for (int aS = 0; aS < N; aS++) + { + int curIndT = indices[aS]; + int nextSegmentStartS = aS; + + if (curIndT > lastSegmentIndexT) + { + lastSegmentIndexT = curIndT; + segmentStartIndS = aS; + } + for (; nextSegmentStartS <= N; nextSegmentStartS++) + { + int indT = indices[nextSegmentStartS % N]; + if (indT > curIndT) + { + break; + } + } + + float distance = nextSegmentStartS - segmentStartIndS; + + // modulo for negative number + while (distance < 0) + { + distance += N; + } + + float relative = ((float) (aS - segmentStartIndS)) / distance; + + relative = relative > 1 ? 1 : relative; + relative = relative < 0 ? 0 : relative; + + int nextIndT = (curIndT + 1) % N; + + float cW = 1 - trajectory.w[curIndT]; + float nW = 1 - trajectory.w[nextIndT]; + + float weighted = (1.0f - relative) * cW + relative * nW; + + /* + std::cout << "seg#" << std::setw(4) << aS << std::fixed << + ": segmentStartInd=" << segmentStartIndS << + ", nextSegmentIndex=" << nextSegmentStartS << + ", cW=" << cW << + ", nW=" << nW << + ", relative=" << relative << + ", weighted=" << weighted << "\n"; + + */ + + getSegment(aS)->optimalSpot = weighted; + } + + /* + for (int i = 0; i < N; i++, seg = seg->next) + { + getSegment(i)->optimalSpot = 1 - trajectory.w[i]; + } + */ + + //clipLineToLimits(); + //filterLine(); + //filterLine(); +} + +float +TrackModel::getRadius(int i) +{ + // get segment centers at current state. + // out of bounds indexing is handled by getSegment. + + v2t a = getSegment(i - 1)->pos(); + v2t p = getSegment(i)->pos(); + v2t b = getSegment(i + 1)->pos(); + // find perpendicular bisectors to calculate radius + v2t apC = 0.5f * a + 0.5f * p; + v2t pbC = 0.5f * b + 0.5f * p; + v2t apD = p - a; + v2t pbD = b - p; + + /* + -90 degree rotation matrix + 0 -1 + 1 0 + */ + + v2t acD; + acD.x = -apD.y; + acD.y = apD.x; + + v2t bcD; + bcD.x = -pbD.y; + bcD.y = pbD.x; + + straight2t la(apC, acD); + straight2t lb(pbC, bcD); + + v2t center = la.intersect(lb); + + float dx = p.x - center.x; + float dy = p.y - center.y; + + float r = sqrt(dx * dx + dy * dy); + + // check for extremely huge values or bad values + + if (r > 1000000 || isnan(r) || isinf(r)) + { + r = 1000000; + } + + return r; +} + +/** + * Update the radii for all segments + */ +void +TrackModel::fillRadii() +{ + // 0 - look for first low radius + // 1 - look for second low radius + // 2 - look for high radius + int state = 0; + float lastRadius = getRadius(-1); + float suppressRadius = lastRadius; + + for (size_t i = 0; i < segments.size(); i++) + { + float r = getRadius(i); + bool suppress = false; + + switch (state) + { + case 0: + if (r < 0.5 * lastRadius) + { + state = 1; + } + else + { + suppressRadius = r; + } + break; + + case 1: + if (r < 2 * lastRadius) + { + state = 2; + } + else if (r > 2 * lastRadius) + { + getSegment(i - 1)->radiusOfCurve = suppressRadius; + state = 0; + + /* + std::cout << "segment " << (i - 1) << " suppressed from " + << r << " to " << suppressRadius << "\n"; + + */ + } + else + { + state = 0; + suppressRadius = r; + } + break; + + case 2: + if (r > 2 * lastRadius) + { + suppress = true; + } + state = 0; + break; + } + + LineSegment *seg = getSegment(i); + + seg->radiusOfCurve = r; + seg->state = state; + + if (suppress) + { + + std::cout << "segment " << (i - 1) << " suppressed from " + << getSegment(i - 1)->radiusOfCurve << " to " + << suppressRadius << "\n"; + + std::cout << "segment " << (i - 2) << " suppressed from " + << getSegment(i - 2)->radiusOfCurve << " to " + << suppressRadius << "\n"; + + + getSegment(i - 1)->radiusOfCurve = suppressRadius; + getSegment(i - 2)->radiusOfCurve = suppressRadius; + + + suppressRadius = r; + } + + lastRadius = r; + } +} + +void +TrackModel::printRadii() +{ + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + float r = seg->radiusOfCurve; + float max = seg->maximumSpeedAtPoint; + float rat = seg->rationalSpeedLimit; + int state = seg->state; + + std::cout << "segment " << std::setw(4) << i << std::fixed << + ": optimalSpot=" << getSegment(i)->optimalSpot << + ", r=" << r << ", maxSpd=" << max << ", ratSpd=" << rat + << ", state=" << state + << "\n"; + } +} + +float +TrackModel::calculateLengthCost() +{ + float lengthCost = 0; + + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *start = getSegment(i); + LineSegment *end = getSegment(i + 1); + + float len = (end->pos() - start->pos()).len(); + + lengthCost += len; + } + + return lengthCost; +} + +float +TrackModel::calculateCost() +{ + /* + float cost = 0; + + for(size_t i = 0; i < segments.size(); i++) + { + float r = getRadius(i); + + cost += 1.0 / (r * r); + } + */ + + return /*cost + */calculateLengthCost(); +} + +float +TrackModel::directedDev(int index) +{ + float oldCost = calculateCost(); + + LineSegment *seg = getSegment(index); + + float old = seg->optimalSpot; + + seg->optimalSpot += 0.001f; + + seg->optimalSpot = seg->optimalSpot > 0.95f ? 0.95f : seg->optimalSpot; + seg->optimalSpot = seg->optimalSpot < 0.05f ? 0.05f : seg->optimalSpot; + + float newCost = calculateCost(); + + seg->optimalSpot = old; + + float dev = (newCost - oldCost) / 0.001f; + + return dev; +} + +void +TrackModel::costOptimizationStep(float amount) +{ + + float tmpOpt[segments.size()]; + + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + float grad_i = directedDev(i); + tmpOpt[i] = seg->optimalSpot - amount * grad_i; + } + + for (size_t i = 0; i < segments.size(); i++) + { + segments[i].optimalSpot = tmpOpt[i]; + } + + clipLineToLimits(); +} + +void +TrackModel::costOptimization() +{ + float cost = calculateCost(); + for (int pass = 0; pass < 10; pass++) + { + + costOptimizationStep(0.1); + + float costBefore = cost; + cost = calculateCost(); + float diff = -(cost - costBefore); + + filterLine(); + + std::cout << "optimization pass " << pass << + " cost=" << cost << + ", improvement=" << diff << "\n"; + } +} + +void +TrackModel::clipLineToLimits() +{ + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + + float sp = seg->optimalSpot; + sp = sp > .9 ? .9 : sp; + sp = sp < .1 ? .1 : sp; + + seg->optimalSpot = sp; + } +} + +void +TrackModel::fillPointLimits(float maxG) +{ + float maxA = 9.81 * maxG; + for (size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + float radius = seg->radiusOfCurve; + float friction = seg->friction; + + float maxV = sqrt(friction * maxA * radius); + + + float suction = suctionParam * maxV * maxV; + + maxV = sqrt(friction * (maxA + suction) * radius); + + if (maxV < 5) + { + maxV = 5; + } + + seg->maximumSpeedAtPoint = maxV; + seg->rationalSpeedLimit = maxV; + } +} + +void +TrackModel::propagateRationalLimit(float maxG) +{ + float maxA = 9.81 * maxG; + for (size_t i = 0; i < 2 * segments.size(); i++) + { + LineSegment *seg = getSegment(-i); + float plim = seg->rationalSpeedLimit; + + LineSegment *nseg = getSegment(-i + 1); + float neighbourLim = nseg->rationalSpeedLimit; + + + // calculate limit assuming maximimum deceleration + // over straight segment + + float len = (seg->pos() - nseg->pos()).len(); + + float v0 = neighbourLim; + + float friction = seg->friction; + float r = seg->radiusOfCurve; + float suction = suctionParam * v0 * v0; + float aLim = friction * (maxA + suction); + + // maximum acceleration is the sum of lateral + // acceleration and brake acceleration + float aBrake = sqrt(aLim * aLim - ((v0 * v0) / r)); + + float x = -len; + float a0 = -aBrake; + //float a0 = -maxA * seg->friction; + + float v_x = sqrt(v0 * v0 + 2 * a0 * x); + + if (v_x < plim) + { + plim = v_x; + } + + seg->rationalSpeedLimit = plim; + } +} + +float +TrackModel::getOffsetFromCenter(tTrkLocPos *p) +{ + float relPos = 0; + + switch (p->seg->type) + { + case TR_STR: + relPos = p->toStart / p->seg->length; + break; + case TR_LFT: + case TR_RGT: + relPos = p->toStart / p->seg->arc; + break; + } + + int ind = segmentIndices[p->seg]; + LineSegment *startSeg = getSegment(ind); + LineSegment *endSeg = getSegment(ind + 1); + + if (startSeg == NULL || endSeg == NULL) + { + return 0; + } + + + + float startWidth = p->seg->startWidth; + float endWidth = p->seg->endWidth; + + float startOpt = startSeg->optimalSpot; + float endOpt = endSeg->optimalSpot; + + float sC = 1 - relPos; + float eC = relPos; + + float width = sC * startWidth + eC * endWidth; + float opt = sC * startOpt + eC * endOpt; + + float off = width * (opt - 0.5); + + return off; + + //return p->seg->width; + //return (startSeg->left - startSeg->right).len(); + //return p->seg->startWidth; +} + +float +TrackModel::getTangentAngle(tTrkLocPos *p) +{ + float baseAngle = 0; + + switch (p->seg->type) + { + case TR_STR: + baseAngle = p->seg->angle[TR_ZS]; + break; + case TR_RGT: + baseAngle = p->seg->angle[TR_ZS] - p->toStart; + break; + case TR_LFT: + baseAngle = p->seg->angle[TR_ZS] + p->toStart; + break; + } + + int ind = segmentIndices[p->seg]; + LineSegment *startSeg = getSegment(ind); + LineSegment *endSeg = getSegment(ind + 1); + + float len = (endSeg->pos() - startSeg->pos()).len(); + + float lrStart = startSeg->optimalSpot * (startSeg->right - startSeg->left).len(); + float lrEnd = endSeg->optimalSpot * (endSeg->right - endSeg->left).len(); + + float tanAngle = -(lrEnd - lrStart) / len; + float displAngle = atan(tanAngle); + + return baseAngle + displAngle; +} + +float +TrackModel::getMaximumSpeed(tTrkLocPos *p) +{ + float relPos = 0; + + switch (p->seg->type) + { + case TR_STR: + relPos = p->toStart / p->seg->length; + break; + case TR_LFT: + case TR_RGT: + relPos = p->toStart / p->seg->arc; + break; + } + + int ind = segmentIndices[p->seg]; + LineSegment *startSeg = getSegment(ind); + LineSegment *endSeg = getSegment(ind + 1); + + if (startSeg == NULL || endSeg == NULL) + { + return 50; + } + + float startLimit = startSeg->rationalSpeedLimit; + float endLimit = endSeg->rationalSpeedLimit; + + float sC = 1 - relPos; + float eC = relPos; + + float maxV = sC * startLimit + eC * endLimit; + + if (maxV < 5) + { + maxV = 5; + } + + return maxV; +} + +void +TrackModel::dumpTrackToFile(tTrack* track, const char* path) +{ + FILE *f = fopen(path, "w"); + + if (f == 0x00) + { + return; + } + + tTrackSeg *seg = track->seg; + + // memory leak + std::cout << "dump track data to " << canonicalize_file_name(path) << "\n"; + fputs("startLX, startLY, startRX, startRY, endLX, endLY, endRX, endRY\n", f); + + for (int i = 0; i < track->nseg && seg != NULL; i++) + { + t3Dd sl = seg->vertex[TR_SL]; + t3Dd sr = seg->vertex[TR_SR]; + t3Dd el = seg->vertex[TR_EL]; + t3Dd er = seg->vertex[TR_ER]; + + fprintf( + f, + "%f, %f, %f, %f, %f, %f, %f, %f\n", + sl.x, sl.y, sr.x, sr.y, el.x, el.y, er.x, er.y + ); + + seg = seg->next; + } + + fclose(f); +} + +void +TrackModel::dumpDataToFile(const char* path) +{ + FILE *f = fopen(path, "w"); + + if (f == 0x00) + { + return; + } + + // memory leak + std::cout << "dump line data to " << canonicalize_file_name(path) << "\n"; + fputs("x, y, trackPos, pointLim, rationalLim\n", f); + + float trackPos = 0; + for(size_t i = 0; i < segments.size(); i++) + { + LineSegment *seg = getSegment(i); + LineSegment *nseg = getSegment(i + 1); + + v2t pos = seg->pos(); + float segLng = (nseg->pos() - pos).len(); + float x = pos.x; + float y = pos.y; + float pL = seg->maximumSpeedAtPoint; + float rL = seg->rationalSpeedLimit; + + fprintf( + f, + "%f, %f, %f, %f, %f\n", + x, y, trackPos, pL, rL + ); + + trackPos += segLng; + } + + fclose(f); +} + + + + + + + + + + + + + + + + + + + + diff --git a/src/drivers/test_bot/optimal_line.h b/src/drivers/test_bot/optimal_line.h new file mode 100644 index 0000000..c452e1a --- /dev/null +++ b/src/drivers/test_bot/optimal_line.h @@ -0,0 +1,142 @@ +#ifdef _WIN32 +#include +#endif + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Encapsulation of the data we keep for a track segment. + * We think of it as a straight line to get the approximation of + * the optimal line. + */ +class LineSegment +{ +public: + v2t left; // Leftmost point of the track + v2t right; // Rightmost point of the track + + float optimalSpot; // (1 - optimalSpot) * left + optimalSpot * right + // is the position that is currently believed to + // be optimal + + float radiusOfCurve; + float maximumSpeedAtPoint; + float rationalSpeedLimit; + + float friction; + + /** + * TR_RGT 1 + * TR_LFT 2 + * TR_STR 3 + */ + int type; + + int state; + + v2t pos(float offset); + v2t pos(); +}; + +/** + * Minimal model of track on which we will do the optimizations + */ +class TrackModel +{ +public: + std::vector segments; + + std::map segmentIndices; + + void initialize(tTrack* track, float maxBreakG, float maxSideG, float suctionParam); + + void getTorcsTrajectory(tTrack *track); + + float getRadius(int index); + + float calculateLengthCost(); + + float calculateCost(); + + void costOptimizationStep(float amount); + + void costOptimization(); + + float filterSegment(int index); + float filterSegmentSpatial(int index); + + void filterLine(); + void filterFinishLine(); + + void filterSegments(int startInd, int stopInd); + + void filterRadii(); + + void fixFinishRadii(); + + void printRadii(); + + float directedDev(int index); + + void fillRadii(); + + void fillPointLimits(float maxG); + void propagateRationalLimit(float maxG); + + void clipLineToLimits(); + + LineSegment *getSegment(int index); + + float getOffsetFromCenter(tTrkLocPos *p); + + float getTangentAngle(tTrkLocPos *p); + + float getMaximumSpeed(tTrkLocPos *p); + + void dumpDataToFile(const char *path); + void dumpTrackToFile(tTrack* track, const char *path); + +private: + float suctionParam; +}; + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/drivers/test_bot/test_bot.cpp b/src/drivers/test_bot/test_bot.cpp new file mode 100644 index 0000000..7e176b4 --- /dev/null +++ b/src/drivers/test_bot/test_bot.cpp @@ -0,0 +1,577 @@ +/*************************************************************************** + + file : test_bot.cpp + created : Do 2. Nov 08:49:38 CET 2017 + copyright : (C) 2017 Michael Heinrich, Jonas Natzer + + ***************************************************************************/ + +/*************************************************************************** + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + ***************************************************************************/ + +#ifdef _WIN32 +#include +#endif + + +#include "test_bot.h" + + +static void initTrack(int index, tTrack* track, void *carHandle, void **carParmHandle, tSituation *s); +static void newrace(int index, tCarElt* car, tSituation *s); +static void drive(int index, tCarElt* car, tSituation *s); +static void endrace(int index, tCarElt *car, tSituation *s); +static void shutdown(int index); +static int InitFuncPt(int index, void *pt); + +static tBot bots[NBBOTS]; + +/* + * Module entry point + */ +extern "C" int +test_bot(tModInfo *modInfo) +{ + memset(modInfo, 0, NBBOTS * sizeof (tModInfo)); + + for (int i = 0; i < NBBOTS; i++) + { + modInfo->name = strdup("Hemic"); /* name of the module (short) */ + modInfo->desc = strdup("2017 TU München research project by " + "Michael Heinrich and Jonas Natzer"); /* description of the module (can be long) */ + modInfo->fctInit = InitFuncPt; /* init function */ + modInfo->gfId = ROB_IDENT; /* supported framework version */ + modInfo->index = i + 1; + } + + return 0; +} + +/* Module interface initialization. */ +static int +InitFuncPt(int index, void *pt) +{ + if (index < 1 || index > NBBOTS) + { + std::cout << "test_bot.cpp: array index out of bounds\n"; + return 0; + } + + tRobotItf *itf = (tRobotItf *) pt; + + itf->rbNewTrack = initTrack; /* Give the robot the track view called */ + /* for every track change or new race */ + itf->rbNewRace = newrace; /* Start a new race */ + itf->rbDrive = drive; /* Drive during race */ + itf->rbPitCmd = NULL; + itf->rbEndRace = endrace; /* End of the current race */ + itf->rbShutdown = shutdown; /* Called before the module is unloaded */ + itf->index = index; /* Index used if multiple interfaces */ + return 0; +} + +/* Called for every track change or new race. */ +static void +initTrack(int index, tTrack* track, void *carHandle, void **carParmHandle, tSituation *s) +{ + if (index < 1 || index > NBBOTS) + { + std::cout << "test_bot.cpp: array index out of bounds\n"; + return; + } + + tBot *bot = bots + index - 1; + // track, base, blab, suctionParam + bot->trackModel.initialize( + track, + MAX_BREAK_G, // max Break G + MAX_LATERAL_G, // max lateral G + SUCTION_G_PER_M_SS // additional G per m/s (through suction) + ); + *carParmHandle = NULL; +} + +/* Start a new race. */ +static void +newrace(int index, tCarElt* car, tSituation *s) +{ + std::cout << "test_bot.cpp: Version 0.07\n"; + + + if (index < 1 || index > NBBOTS) + { + std::cout << "test_bot.cpp: array index out of bounds\n"; + return; + } + + tBot *bot = bots + index - 1; + + bot->lastManeuver = 0; + bot->lastMoveStep = 0; + bot->currentStep = 0; + bot->remainingBackwardSteps = 0; + bot->hasLaunched = false; + bot->slipDist = 0; + bot->throttlingI = THROTTLING_I_INIT; +} + +/* Drive during race. */ + +/* Drive during race. */ +static void +drive(int index, tCarElt* car, tSituation *s) +{ + memset(&car->ctrl, 0, sizeof (tCarCtrl)); + + if (index < 1 || index > NBBOTS) + { + std::cout << "test_bot.cpp: array index out of bounds\n"; + return; + } + + tBot *bot = bots + index - 1; + + float angle; + const float SC = 1.0; + + tCarElt *closestOponent = 0x00; + float closestOpDist = 100; + int ncars = s->raceInfo.ncars; + + tdble cX, cY, oX, oY; + v2t opponentDir; + + RtTrackLocal2Global(&(car->_trkPos), &cX, &cY, 0); + + for (int i = 0; i < ncars; i++) + { + tCarElt *op = s->cars[i]; + + if (op == car) + { + continue; + } + + if (op != 0x00) + { + RtTrackLocal2Global(&(op->_trkPos), &oX, &oY, 1); + v2t dif(oX - cX, oY - cY); + float len = dif.len(); + + + + if (len < closestOpDist) + { + closestOpDist = len; + closestOponent = op; + opponentDir = dif; + } + } + } + + float maneuver = (1 - MANEUVER_INNOVATION) * bot->lastManeuver; + float emergency = 0; + float threatSpeed = 0; + + if (closestOponent != 0x00 && closestOpDist < COLLISION_WARNING_DIST) + { + float ownPos = car->_trkPos.toLeft; + float otherPos = closestOponent->_trkPos.toLeft; + + float diff = ownPos - otherPos; + float dir = diff; + dir = dir >= 0 ? 1 : dir; + dir = dir < 0 ? -1 : dir; + + bool isBehind = car->race.pos < closestOponent->race.pos; + bool isCollisionImmenent = (closestOpDist < COLLISION_IMMINENT_DIST) + && !isBehind; + + float intrusion = dir * (COLLISION_WARNING_DIST - closestOpDist) + / (COLLISION_WARNING_DIST - COLLISION_IMMINENT_DIST); + + // avoid car that intrudes into our comfort radius + maneuver += MANEUVER_INNOVATION * COLLISION_AVOID_GAIN * intrusion; + + float dirX = opponentDir.x; + float dirY = opponentDir.y; + + v2t dirV = opponentDir; + dirV.normalize(); + + v2t dv; + + tPosd vCar = car->pub.DynGCg.vel; + tPosd vOpp = car->pub.DynGCg.vel; + + dv.x = vOpp.x - vCar.x; + dv.y = vOpp.y - vCar.y; + + threatSpeed = -(dv.x * dirV.x + dv.y * dirV.y); + + if (threatSpeed > 0.5 * EMERGENCY_BRAKE_DV) + { + emergency = (2 * (threatSpeed / EMERGENCY_BRAKE_DV)) - 1; + + emergency = emergency > 1 ? 1 : emergency; + emergency = emergency < 0 ? 0 : emergency; + } + + emergency = isCollisionImmenent ? 1.0 : 0.0; + + /* + std::cout << "COLLISION_WARNING: intrusion=" << intrusion << + ", maneuver=" << maneuver << + ", obstructManeuver=" << isBehind + << "\n"; + */ + } + + angle = bot->trackModel.getTangentAngle(&(car->_trkPos)) - car->_yaw; + NORM_PI_PI(angle); // put the angle back in the range from -PI to PI + + float offset = bot->trackModel.getOffsetFromCenter(&(car->_trkPos)) + maneuver; + + // half of width minus security margin + float wh = 0.20 * (car->_trkPos.seg->startWidth + car->_trkPos.seg->endWidth); + + offset = offset > wh ? wh : offset; + offset = offset < -wh ? -wh : offset; + + float correctiveAngle = -(SC * (car->_trkPos.toMiddle + offset)) / car->_trkPos.seg->width; + + float corrLim = 0.2; + + bool offRoad = offset > wh || offset < -wh; + + if (offRoad) + { + corrLim = 1; + } + + + if (correctiveAngle > corrLim) + { + correctiveAngle = corrLim; + } + + if (correctiveAngle < -corrLim) + { + correctiveAngle = -corrLim; + } + + angle += correctiveAngle; + + + float speed = car->pub.speed; + float speedLim = bot->trackModel.getMaximumSpeed(&(car->_trkPos)); + + float relPosition = (car->race.pos - 1) / (float) (ncars - 1); + + float difPos = -(NOMINAL_REL_POSITION - relPosition); + + bot->throttlingI += THROTTLING_I * difPos; + float throttlingP = THROTTLING_P * difPos; + + throttlingP = throttlingP > THROTTLING_P_LIM ? THROTTLING_P_LIM : throttlingP; + throttlingP = throttlingP < -THROTTLING_P_LIM ? -THROTTLING_P_LIM : throttlingP; + + float throttling = throttlingP + bot->throttlingI; + + throttling = throttling > MAX_THROTTLING ? MAX_THROTTLING : throttling; + throttling = throttling < MIN_THROTTLING ? MIN_THROTTLING : throttling; + + speedLim *= throttling; + + + // set up the values to return + car->ctrl.steer = angle / car->_steerLock; + + float dv = speed - speedLim; + + if (dv < -2) + { + car->ctrl.accelCmd = 1.0; // 100% accelerator pedal + car->ctrl.brakeCmd = 0.0; // no brakes + } + else if (dv < -1) + { + // dv between -2 and -1 + float cmd = -(dv + 1); + + car->ctrl.accelCmd = cmd; // linear accelerator pedal + car->ctrl.brakeCmd = 0.0; // no brakes + } + else if (dv < 0) + { + // dv between -1 and 0 + car->ctrl.accelCmd = 0.0; // no accelerator pedal + car->ctrl.brakeCmd = 0.0; // no brakes + } + else if (dv < 1) + { + // dv between 0 and 1 + float cmd = dv; + + car->ctrl.accelCmd = 0.0; // no accelerator pedal + car->ctrl.brakeCmd = cmd; // linear brakes + } + else + { + car->ctrl.accelCmd = 0.0; // no accelerator pedal + car->ctrl.brakeCmd = 1; // 100% brakes + } + + float maxRpm = car->_enginerpmRedLine * .9f; + float minRpm = maxRpm * .6; + + int gear = car->_gear; + + if (car->_enginerpm > maxRpm) + { + gear++; + } + else if (car->_enginerpm < minRpm) + { + gear--; + } + + if (gear > 6) + { + gear = 6; + } + + if (gear < 1) + { + gear = 1; + } + + float slip = + car->priv.wheel[0].slipAccel + + car->priv.wheel[1].slipAccel + + car->priv.wheel[2].slipAccel + + car->priv.wheel[3].slipAccel; + + slip *= 0.25; + + float skid = car->priv.skid[0] + car->priv.skid[1] + + car->priv.skid[2] + car->priv.skid[3]; + + skid *= 0.25; + + /* + std::cout << + "skid=" << skid << + ", gear=" << gear << + ", slip=" << slip << + "\n"; + */ + + + if (slip < -10) + { + car->ctrl.accelCmd = 0; + bot->slipDist -= slip; + } + else if (slip < -1) + { + float aSlip = -slip; + float rel = (aSlip - 1.0) / (10.0 - 1.0); + // smooth reduction of throttle on starting slip + car->ctrl.accelCmd = rel * 0.0f + (1.0f - rel) * 1.0; + bot->slipDist -= slip; + } + else + { + // AIMD algorithm estimates severity of slip event + bot->slipDist *= 0.9; + } + + if (bot->slipDist > 10) + { + // Disable engine if slip event gets out of control + car->ctrl.accelCmd = 0; + } + + car->ctrl.gear = gear; + + + bool moves = speed > 1; + + if (moves) + { + bot->lastMoveStep = bot->currentStep; + bot->hasLaunched = true; + } + else if (bot->currentStep - bot->lastMoveStep > 20 && bot->hasLaunched) + { + bot->remainingBackwardSteps = UNSTUCKING_STEPS; + } + + if (bot->remainingBackwardSteps > 0) + { + car->ctrl.gear = -1; + car->ctrl.accelCmd = .3; + car->ctrl.brakeCmd = 0; + car->ctrl.steer = -100 * angle / car->_steerLock; + + bot->remainingBackwardSteps--; + } + + if (!bot->hasLaunched) + { + car->ctrl.gear = 1; + car->ctrl.accelCmd = .6; + car->ctrl.brakeCmd = 0; + } + + car->ctrl.accelCmd *= throttling; + + float accCmd = emergency * 0.0 + (1 - emergency) * car->ctrl.accelCmd; + float brakeCmd = (1 - emergency) * car->ctrl.brakeCmd + emergency * 1.0; + + + car->ctrl.accelCmd = accCmd; + car->ctrl.brakeCmd = brakeCmd; + + std::time_t t = std::time(NULL); + std::time_t dt = t - bot->lastDebugOutTime; + + if (true || dt >= 1) + { + bot->lastDebugOutTime = t; + + //std::ofstream logfile; + //logfile.open ("~/test_bot.log", std::ofstream::out | std::ofstream::app); + + std::cout << std::showpos << std::setprecision(3) << std::fixed + << "off=" << std::setw(3) << offset + << std::noshowpos + << ", acc=" << std::setw(3) << car->ctrl.accelCmd + << ", brk=" << std::setw(3) << car->ctrl.brakeCmd + //<< ", spd=" << std::setw(3) << speed + << ", spdLim=" << std::setw(3) << speedLim + << ", thr=" << std::setw(3) << throttling + << ", thrP=" << std::setw(3) << throttlingP + << ", thrI=" << std::setw(3) << bot->throttlingI + << ", relPos=" << std::setw(3) << relPosition + << ", threatSpd=" << std::setw(3) << threatSpeed + << ", emergency=" << std::setw(3) << emergency + + //<< ", friction=" << car->_trkPos.seg->surface->kFriction + << "\n"; + + + bot->lastManeuver = maneuver; + + + // statistic capture + + float ownCt = car->race.curTime; + float othCt = 0; + float othBL = 0; + + bot->otherTime = 0; + + for (int i = 0; i < s->raceInfo.ncars; i++) + { + tCarElt *c = s->cars[i]; + + if (c == car) + { + continue; + } + + othCt = c->race.curTime; + othBL = c->race.timeBehindLeader; + } + + if(ownCt == 0) + { + bot->otherTime = othCt; + bot->ownTime = othCt + car->race.timeBehindLeader; + } + else + { + bot->ownTime = ownCt; + bot->otherTime = ownCt + othBL; + } + + //logfile.close(); + } + + + bot->currentStep++; + /* + car->ctrl.accelCmd = 0.3; // 30% accelerator pedal + car->ctrl.brakeCmd = 0.0; // no brakes + */ +} + +inline bool exists_file(const std::string& name) +{ + if (FILE * file = fopen(name.c_str(), "r")) + { + fclose(file); + return true; + } + else + { + return false; + } +} + +/* End of the current race */ +static void +endrace(int index, tCarElt *car, tSituation *s) +{ + std::cout << "endrace called\n"; + +} + +/* Called before the module is unloaded */ +static void +shutdown(int index) +{ + std::cout << "shutdown called\n"; + + std::ostringstream oss; + + oss << "stats_lim_" << NOMINAL_REL_POSITION << ".csv"; + + std::string path = oss.str(); + + bool existed = exists_file(path); + + FILE *f = fopen(path.c_str(), "a"); + + if (f == 0x00) + { + std::cout << "Couldn't open stats file " << path << "\n"; + return; + } + + if (!existed) + { + fputs("ownTime, otherTime\n", f); + } + + + + fprintf( + f, + "%f, %f\n", + bots[0].ownTime, bots[0].otherTime + ); + + fclose(f); +} + + + diff --git a/src/drivers/test_bot/test_bot.def b/src/drivers/test_bot/test_bot.def new file mode 100644 index 0000000..3852ca2 --- /dev/null +++ b/src/drivers/test_bot/test_bot.def @@ -0,0 +1,19 @@ +; +; file : test_bot.def +; created : Do 2. Nov 08:49:38 CET 2017 +; copyright : (C) 2002 Jonas Natzer, Michael Heinrich +; +; +; This program is free software; you can redistribute it and/or modify +; it under the terms of the GNU General Public License as published by +; the Free Software Foundation; either version 2 of the License, or +; (at your option) any later version. +; + +LIBRARY test_bot + +SECTIONS .data READ WRITE + +EXPORTS + test_bot + diff --git a/src/drivers/test_bot/test_bot.dsp b/src/drivers/test_bot/test_bot.dsp new file mode 100644 index 0000000..1c12aa2 --- /dev/null +++ b/src/drivers/test_bot/test_bot.dsp @@ -0,0 +1,238 @@ +# Microsoft Developer Studio Project File - Name="test_bot" - Package Owner=<4> + +# Microsoft Developer Studio Generated Build File, Format Version 6.00 + +# ** DO NOT EDIT ** + + + +# TARGTYPE "Win32 (x86) Dynamic-Link Library" 0x0102 + + + +CFG=test_bot - Win32 Debug + +!MESSAGE This is not a valid makefile. To build this project using NMAKE, + +!MESSAGE use the Export Makefile command and run + +!MESSAGE + +!MESSAGE NMAKE /f "test_bot.mak". + +!MESSAGE + +!MESSAGE You can specify a configuration when running NMAKE + +!MESSAGE by defining the macro CFG on the command line. For example: + +!MESSAGE + +!MESSAGE NMAKE /f "test_bot.mak" CFG="test_bot - Win32 Debug" + +!MESSAGE + +!MESSAGE Possible choices for configuration are: + +!MESSAGE + +!MESSAGE "test_bot - Win32 Release" (based on "Win32 (x86) Dynamic-Link Library") + +!MESSAGE "test_bot - Win32 Debug" (based on "Win32 (x86) Dynamic-Link Library") + +!MESSAGE + + + +# Begin Project + +# PROP AllowPerConfigDependencies 0 + +# PROP Scc_ProjName "" + +# PROP Scc_LocalPath "" + +CPP=cl.exe + +MTL=midl.exe + +RSC=rc.exe + + + +!IF "$(CFG)" == "test_bot - Win32 Release" + + + +# PROP BASE Use_MFC 0 + +# PROP BASE Use_Debug_Libraries 0 + +# PROP BASE Output_Dir "Release" + +# PROP BASE Intermediate_Dir "Release" + +# PROP BASE Target_Dir "" + +# PROP Use_MFC 0 + +# PROP Use_Debug_Libraries 0 + +# PROP Output_Dir "Release" + +# PROP Intermediate_Dir "Release" + +# PROP Ignore_Export_Lib 1 + +# PROP Target_Dir "" + +# ADD BASE CPP /nologo /MT /W3 /GX /O2 /D "WIN32" /D "NDEBUG" /D "_WINDOWS" /D "_MBCS" /D "_USRDLL" /D "TEST_BOT_EXPORTS" /YX /FD /c + +# ADD CPP /nologo /G5 /W3 /GX /O2 /I "../../../export/include" /I "../../windows/include" /D "WIN32" /D "NDEBUG" /D "_WINDOWS" /D "_MBCS" /D "_USRDLL" /D "TEST_BOT_EXPORTS" /YX /FD /c + +# ADD BASE MTL /nologo /D "NDEBUG" /mktyplib203 /win32 + +# ADD MTL /nologo /D "NDEBUG" /mktyplib203 /win32 + +# ADD BASE RSC /l 0x40c /d "NDEBUG" + +# ADD RSC /l 0x40c /d "NDEBUG" + +BSC32=bscmake.exe + +# ADD BASE BSC32 /nologo + +# ADD BSC32 /nologo + +LINK32=link.exe + +# ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /dll /machine:I386 + +# ADD LINK32 tgf.lib robottools.lib sg.lib ul.lib /nologo /dll /map /machine:I386 /nodefaultlib:"LIBCD" /libpath:"../../../export/lib" /libpath:"../../windows/lib" + +# Begin Special Build Tool + +WkspDir=. + +TargetDir=.\Release + +SOURCE="$(InputPath)" + +PostBuild_Cmds=copy $(TargetDir)\*.dll $(WkspDir)\runtime\drivers\test_bot + +# End Special Build Tool + + + +!ELSEIF "$(CFG)" == "test_bot - Win32 Debug" + + + +# PROP BASE Use_MFC 0 + +# PROP BASE Use_Debug_Libraries 1 + +# PROP BASE Output_Dir "Debug" + +# PROP BASE Intermediate_Dir "Debug" + +# PROP BASE Target_Dir "" + +# PROP Use_MFC 0 + +# PROP Use_Debug_Libraries 1 + +# PROP Output_Dir "Debug" + +# PROP Intermediate_Dir "Debug" + +# PROP Ignore_Export_Lib 1 + +# PROP Target_Dir "" + +# ADD BASE CPP /nologo /MTd /W3 /Gm /GX /ZI /Od /D "WIN32" /D "_DEBUG" /D "_WINDOWS" /D "_MBCS" /D "_USRDLL" /D "TEST_BOT_EXPORTS" /YX /FD /GZ /c + +# ADD CPP /nologo /G5 /W3 /Gm /GX /ZI /Od /I "../../../export/include" /I "../../windows/include" /D "WIN32" /D "_DEBUG" /D "_WINDOWS" /D "_MBCS" /D "_USRDLL" /D "TEST_BOT_EXPORTS" /YX /FD /GZ /c + +# ADD BASE MTL /nologo /D "_DEBUG" /mktyplib203 /win32 + +# ADD MTL /nologo /D "_DEBUG" /mktyplib203 /win32 + +# ADD BASE RSC /l 0x40c /d "_DEBUG" + +# ADD RSC /l 0x40c /d "_DEBUG" + +BSC32=bscmake.exe + +# ADD BASE BSC32 /nologo + +# ADD BSC32 /nologo + +LINK32=link.exe + +# ADD BASE LINK32 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /dll /debug /machine:I386 /pdbtype:sept + +# ADD LINK32 robottools.lib kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib sg.lib ul.lib /nologo /dll /map /debug /machine:I386 /pdbtype:sept /libpath:"../../../export/libd" /libpath:"../../windows/lib" + +# Begin Special Build Tool + +WkspDir=. + +TargetDir=.\Debug + +SOURCE="$(InputPath)" + +PostBuild_Cmds=copy $(TargetDir)\*.dll $(WkspDir)\runtimed\drivers\test_bot + +# End Special Build Tool + + + +!ENDIF + + + +# Begin Target + + + +# Name "test_bot - Win32 Release" + +# Name "test_bot - Win32 Debug" + +# Begin Group "Source Files" + + + +# PROP Default_Filter "cpp;c;cxx;rc;def;r;odl;idl;hpj;bat" + +# Begin Source File + + + +SOURCE=.\test_bot.cpp + +# End Source File + +# Begin Source File + + + +SOURCE=.\test_bot.def + +# End Source File + +# End Group + +# Begin Group "Header Files" + + + +# PROP Default_Filter "h;hpp;hxx;hm;inl" + +# End Group + +# End Target + +# End Project + diff --git a/src/drivers/test_bot/test_bot.h b/src/drivers/test_bot/test_bot.h new file mode 100644 index 0000000..5b2edd6 --- /dev/null +++ b/src/drivers/test_bot/test_bot.h @@ -0,0 +1,91 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ + +/* + * File: test_bot.h + * Author: michaelheinrich + * + * Created on 26. November 2017, 23:30 + */ + +#ifndef TEST_BOT_H +#define TEST_BOT_H + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "optimal_line.h" + + +#define MAX_BREAK_G 1.60 +#define MAX_LATERAL_G 1.60 +#define SUCTION_G_PER_M_SS (0.5 / 1000.0) + + +#define COLLISION_WARNING_DIST 10 +#define COLLISION_AVOID_GAIN 5 + +/** + * If the closest car is closer than COLLISION_IMMINENT_DIST and + * has a lower race position, issue a brake maneuver + * + */ +#define COLLISION_IMMINENT_DIST 5 +#define EMERGENCY_BRAKE_DV 1 + +#define UNSTUCKING_STEPS 90 +#define MANEUVER_INNOVATION 0.1 + +#define THROTTLING_I_INIT 0.8 +#define THROTTLING_P 1.0 +#define THROTTLING_I 0.0003 + +#define THROTTLING_P_LIM 0.1 + +#define MAX_THROTTLING 1.0 +#define MIN_THROTTLING 0.3 +//#define NOMINAL_REL_POSITION 0.5 +#define NOMINAL_REL_POSITION 0.5 + +#define NBBOTS 1 + +typedef struct Bot +{ + TrackModel trackModel; + float lastManeuver; + std::time_t lastDebugOutTime; + int lastMoveStep; + int currentStep; + int remainingBackwardSteps; + bool hasLaunched; + + float throttlingI; + float slipDist; + + float ownTime; + float otherTime; + +} tBot; + +#endif /* TEST_BOT_H */ + diff --git a/src/drivers/test_bot/test_bot.xml b/src/drivers/test_bot/test_bot.xml new file mode 100644 index 0000000..849c7e0 --- /dev/null +++ b/src/drivers/test_bot/test_bot.xml @@ -0,0 +1,35 @@ + + + + + + + + + + +
+
+
+ + + + + + + + + + + + +
+
+
+ diff --git a/src/libs/txml/gennmtab/gennmtab b/src/libs/txml/gennmtab/gennmtab new file mode 100755 index 0000000000000000000000000000000000000000..caf9ab17ca7695d432ed42f8cbf2b92394dfd0ff GIT binary patch literal 21808 zcmeHPd3;pW^}lc4OfpGe64r!8c|foskPQNc)g&Y^p#cI04HX=Q$s`#`X5uUu5KAC{ zM2rRNu5F;UN@;6x-zf@3tX1Q$F7#IyY}E#r8aIlH&hLBfyLXnCMEmLI^ZSpReBZg} z+;h&o_q_X-d*{8ksl0N&Nl}q*ij#iAg;Qe;4iSrJRZHj|BK3E(YUrk5A% z1f_a9EnG`#6>=5S3>2Akfsu|DdXmZ(5~ce3ORERzC6Y3xaRJ#;TU&&~meIOg($zMu z#u@~bcGNtw+ac_B2s=rOsUt{|%KlN`2-Tv0Mp{NjNRpB+rK)#>wUmD%m9xF5)R$D+ zErlJm^FK{#LUEa>ufMqL5dD)>?uWca&zk9ld5yI>jUI2PC8womdd~F1+<-543hy`h zC*1{$m-Cv)jR*xV9}1piRNixRMxp86>L&khHdTC;|M;Hk|NUqQmC>${`)$V|O=0HZ zB$AQu!9Z#GD<3}okLx!+((#Gyub0=1{@^bkNA@0y%@^Ntz zo!QrqpS}Iauk1(uP(Skae&k#Gky9+RKHB-`e)K2zqd%)3`J3>Q#?smG?D$$f;qnZ2 z!XUo{eg?A?c0e8o;3>T;^o@RKbPUFxE;H^qj4xla+v{wkF;&1E71fIzwQj$=-V+GA z{nd-+HTt~nYUi3pH*+}Zn|xkJAn5c59S&W!Z^b%~*TWpx1R4B08$&_zo=?m=pMQ-f zQ0HlMlV~c5>P0zc7E~_RTY`cGm62?-x4vLH=cbd%hDbpzZ}=1v*7%*?T0fNt*6l@4 zBAPWVbw$(HHAVTR5aqeQ8c#IK+sdUEE{aGi=9NeCHSU(EPEA2Uq#zKibxoQSVR=>D zQN7FM2#~yh+Ue3Y92H9(&7q)+VG}@b5m(n*hpS<&qt5ARWX*n$H(1A-+)V*@kcg|P znV|+>Navc@!^YL(bRgKyM$ZP?Kjd8OpC+fr%N*`nXV8g)HGzOGhsayY7F1T0&T~x3 zEsW#}qS-0A(`Y1_V*XT|OX#ekpIH=~E%ZM+Z>a2YKIX`Vv7sK%KsqCz60#e1--Od8 z&N48iV<6|U9 znHbc zKACvx^2uGCHxW-=J=qHW!h%zeQ3UOqKTN}pwYUGn-u_mteW!Us7F6u*TQjW?7aZzt zgR;G2+Up}V_SlQO=&f!anq}|UN%p#v_BZYAFLt-+sxJ+P>XlseneH-DZGWr#4x&46 z;9hI(Q-|_uLMismN2vOOLzf)xu7G)6>X@ytWUSz$iuRW-wYUG>-u6yURdr>DWhVM& z?-+O(UAA}3nGNOeuos4D?DNzyWn6Txrm^Mr4$H4W?K4i>+use2z%KX&bZ8e0Topc1 zmn;3RI!sZI@^E=M*>z02jJuimE$r=2eXH8rd+bkjUtm{`+F$xMm;r|#;cx&s?9X58 zoU!o3LV5PKIjJx}%g~;I_MLNX0~a24W~j!x7oZPEEi}}XRfpjU{rF5!QeSnlP7ew7 zZ`=O_7uws7nkzfZuYO?Lb<~`wfVEc6I2xR?bGY3#{|q!v&Ukv`$+km^y<@ZU3<~MGPWI=#2()kUIk+Mw9-d>hzwRddI)X;EZ(qw$fQy-e0SpglTqT{!jbm)FC zh}uUjv(M;G-5P|bY-UmLUlp6b38T8y?H^%V(eXqkW8v^)WKr4vUiUj^!r^U)Qnz(c zi*}*{`_Af2Yia63=B&~k=In~LKPnaNAHoNntwQbSgC7H?vSXSTq~LGepTi!)FHqXu zjl66h`LfzKe@*q2b*xoVA1cg9-Kz3h|6OpXr0w0r?i%Qqw0)zD2)F(7{kCT`jD?&b z8oF>;vG2_9_K<6Mpo$OJcW%w3=wPhthg9meH+c|`P!Q3Azkyt`s$^x!s*+1f94A+j zF}0zr<4UD;$LOqrzwtd(@NB^;ic?AJ>;Wv8Sk~V2mlH`x?QKtC?|-R9equ>~dN-L_vSoo^4{Zv`|$0gyRuk$@HzXdsK-jHn^urXlEYX)d2P5Y$56fT!N;uGPHGCXcti)DvuSHea$zo16-0*VCb2hQrr_-T<0`F;)0gIJ^>k83xD$puYq? zMtT?{bnVy*dM)Ts$S5frmNBIzO&K*H(Yi}XOd~l>dAs0m5-Et(w?4yKfUe(qIvj2# zU0d3GTgJlFlq;;Q?1GWAvZrQ_BQf1;;RMNcpnXe8fK&<8053zI} zZMhkgmDtkSOp9%MTCJu{$+on4HfxEzSE9DO3>?FG!cDr-y2~SVqn_;vcB*@kt=MlX zD(O`(@7HL=vj=|kBY>QpO=lx;HUeiOa5e&GBXBkXXCrVn0%s#|HUbm@xt=Q5PjTza zlaV?Na<75kY~Zgm@V6QGUm5rZ4E#X@|Ez&OZs5BO{AmMk79EoPN;U8!4g3THUtr+p z8u*06?LTCn^bW`ro0>)@jVZxwW;pmIH2{5%?2FaP4(u#Z%3 z5tAXgMtqA|H&fOmj@0^Pv-z(;_B zeyG4~(Vw}XmB1pwF9odz)&MSGGtdS+0Z=+9=pO_<1bPH`1~>*h2RskF22lDt=sy6H zH~?k~I#pc%Lv*bKA*PXLq-g8mVB26z^D4tO3=aA2vxY;l0i z1)T>h0PMgIfknVYz{S88pba<(JPjNLjsbrHo(En7C@DChsK8vH444ntfrY>#U^#FR zumxxX4gyDjqrfrXPr&oQE5NIO2`3~Km~%L8r&DBUI~kr}{hj6o031AGb8VQlz-_kfRqI_yIq@E-6n zP>1;YfPVrX19ga<4|osw7^p*>d_WcYyaMO~{t9Sl-$ZIFH3CVVhhGvW(yMXoQ7T3o zD9wirrK5scM7s+>rvX0$eh!=gz6E4*h|EkuOF(}FIDrkoA>dVjuB)d2x_;8n6!d!m zUHb|Fy5^Mtc7U#TKL%QX!@%=E4?yeaUjSbLbPcHj_5t)0%TvI1(I0w_;jGe&j81O=YT&0 zUBCyxzkn}*)4-QN0nT3+0V~9L%n7<3=mOpcdVo)XvDhy&ff8ULa5?ZW@EC9qI0U=^ zbOC<>UI8e*3;G^l!C0CB%mFCXfI5L~z_q|nfL*{(fqlS!-~ezRK=VF(v6&2{0%^byAPdL_@_{M9RA3q~9Vh~304so%z@-2sKWGrx2J8en07^dr z-3|N{*az$f=w9+Z;C|pi;8Ea7fYJ#;EjUI70I5J4Fa*c~vVnYH3NRIz4$J`N0F+jO zt^yo@6W9h^16&8}08gK&JfNO#4fL*}#z&>C016U0>0R_i-E|4#d_bH%LfnuN>*bHm|wgQx{2HgQ%Blr$MZved+xE1IG_5ybT zzXB-TE$DvG-vGY@?g0(}l|V1;~fGOnA6U z1uQ@!kOWXl0UZF?fWg3QfYMw+iv_I|bg`gSf>whr2UZB)0qO)?f_DpAFQ`Y*CQvWn z2LeC{p!9P=_X+wNLGJ4XCjtxR&6}xZ;pxJO+WEOt@QBxJ6yiF7B<^T&&{2{XzABuwDDfYR^laSrG$sl- zKO1uUS(XU`!E;gn)$f(B)X=_a!1r61dNYgU2*@7phn8kVtiV`ZQTuHbT zZAz%-dFyiIRdb?7CQ8D^#F;EEB38?8sF-N3V!n1GWuJrC+(FqytHnwVtOXDqw|sIg zNYX; zK{{tBpPzIoww7-wUzGGD<-etTadIQ+Gldv?GPTH}aHcBxA7pLfOigk*F=pi=kk!ef zNi9KH38o?WHe#$wDM)kjZt^yOGr{Edsl=wVfwUw)OUz(p7D#LI736J*;svuMxr|DN zDr+EXOWsKiG88|U9m)TqlCjElAiI+3hdIkc8iGD^|mb92G ze@@-CCKZ$X7o`xVq1zLPv+e~qfc7FadmxP+lG3i^(j8JdgxPZ7D)j~OH%lRT>UL}_ zv-LcV24);2mvCf3ts*I$kc9U+$Mj1s$YzRHmr;Mh&nGG0? zf~5fO-V1E#?;%#ka2s`XrUniTA*FRvX#*Qt0r$%1b&w>ft(vYhjBePJ`}lq^9=6II zn{bp44?5Q{xlbebMgx*iIb zN2+Bb{wd^#V5|=V(ilmDBOFFpM%|65D{V>-$ZBymYet@uaz_HJM)NSV@dJ>g=ZvSK zZ9H(5$e!zA%687ZNZe(TtB3b<*Gg^xe2&>oA-`H7uj=jtS8C8_pWr_9oNC%ZySS4Y zC~8$GxC;g6u5&ixq@dJ;e*pz!uYqX+?kp&^0?J&4@r7|Eg-Gry}_%Kx1sAp*Zhm?Dy_HYo(@T7c*zdT4$FKy>m=sF;wWP>TV}JxUI=cuJV7Oq!0| zWPs?TUqhi2Aj*_M$^_ex6xBA$wmf~1l7_`Z({QHRW~R(asld=sGg(T?T$JN%BeJ;3 zu%tpt#R=yzB8kM~JUK*BZ7C^at3aAGFyg2PrOAS|R16_DMOK3|fTC0kA&Y@y;Vut_ zwjnT`jg{KeLfr?9E*L?AG$BCsx?s>)bYt)=sNt-PDy(|bsfwYox+bi54$D<87{NOS ze}k04iNzT=myV#CheMStAt%N3_ALrRQ@~r5am^My`<1b~6a^ztz^DMHE8bX)t5H#} zUR#{k(Uz!JME0C$XG~zvjj0ptn3z)3q@^g!)2V4=&p~y|z|^Fyu#jM!g%u1T2jgWc z@sJWJQQcB$f?jrG=?J|OxEn=iaoYGnOq)9}9Az>5q6|62Zn+CX%)}m{ai)BYd6|Ml4F2;%0e_yy>uL3v-%X;w7AVZzzx7?MfS6T{vK@-tF}^;c(1#!9P8-@A2g}P({er zy6cd~@ey3#><;K+)WbD!&`D*=t`9ieet#og#L(3+i1FY&xx}IHg-IE9nwiWdb;Fhw zos_W-p>4&>tF6P-kqgzjx*Ko1IB1=!+TKyuTNiST-|b#$%I0cni*=Wpv1@&E&I;>R zYvqcH6=po}Z#JvHyy`LhmO{DXCTs3Z*7`u`XX<4&&+cvrk&FG8g=U3N>4B@Zm=(3l z6y*6`JU^S~XCwctspjHM>V{xdzB+K%>z}>02d0WzUt>Ct&Im~+c{icnrz+$8>uRm} zE39*@_0~CCP2-^AYL?C;_$njwT|YXpWy`wdb7c+Kio*~O{@ud8iO9M zL%t$`S0TtK(BSh2*M#c0`4mS1-haT`AbxspWeIHR9BZ5bdS@cWoL;oxrFan|)T}Gu z^_O~$Z)j41mn|CIUSG6Hbmyf%c!;t_XFVDu+*66e74kb=zQ$0Km)`QAXAaS@=ze7m zUt=xxl4_0_HROZ88bX;Sr+=;6&qs!r@s>oM^W;r}%KVDTaz}GxNU!}0zbEMC!NPN( z=wg7Pu-1e2@R&p&6P4xx=Q?*E;c(F_CXGJVTG6#B%+chljg1+9a|LmwcT#HIu0|Tu zS>8}%BlgrphE7%E{*d}0;*Y(g_W&03Mp0gaugRTvna}GC%wR zUVMYFrKvGb28MgmJI8|yztlQ0LMR3dZvfS!fz3WdLXTTOmjzfPXnZf|k>CNK{o%#P zV2*V-W*gmdf5fyK{lmEF^9>#J5Ow`%y589;SJgD$h9(A|Ef_H5ZF*tsDAvav8qdSd zd-9_he8`ipDi>^;owaxsiwI&)WU5XIbux{4mr+MiamX7AxS^#FZhFm6n>T4vL7_ey z*K(SIA+LK5BIx$xG2tA<#MLlIe~D1bsn>FrOo^hds$!m&Q|I;N;6YlCE0}}ViktyF zNX(k?P;CWXrfNYwc=?OzFBhQ;`rhC@lDEbi#2HFq%VqYFh-jtEc8R+kGy6J0?E!hhAif;fGLPVQowI-Gy zX~GX8^{tZGNkXKutCUW8gV`C;i!?Rbh*dKCtM6*pDr39;DBAV1pm_PDQ|4b6GAnCl z8`xM+K1R!$jI7TDA&nd+#Xv60f%nHcvnPb;F7 z6t+xeX>pX2%KS>Nu^>DAzUWF4+r+FznbxB7I;Ci+s77T;;ZEc3j?p*n_Lw5Q0nzPu z^?=rKDJhIr65`SVQS`%z%5s^N;Yvp%`bRT}-6eX511hdAVq{DNMT?R|mKNSA zsVum$Q!-11kdC_QUKL7jQd?n5Wj5Au>GvAaXwqwsZhs)EE)>*+cF8>bI7&%n&6_$U zQ!0cCtCHCz5s{qw=yurFilWN|{f(fxB6#vtArCs^P>47Z8Kb|igdaKhjrwxNT_kpV ziJ)bI%7M|Jy^Os)Bgs1?xj130@4I*vBeMv1n8u{ywo|?zA6ur@M7KbreV<-hC3>QY|C!r~Khf*(_;(vKo(F#?fQ@dmbh&L_51WkVwGyT;3h;(G{M ztcd4THjK&lT4M`UHk>i7Fa313x=(pVulLQ#pUN<`?IWpdWFJz}AJvbXmY;hkm7UXD z2#HERru30i`cb8iq%y6S6q78hfqoJ;7SJ&0OO7dJpK@x6i4A0$_&rnl(}aArK|WQ; z>9^6?Q~^d|(Ku84DAfQ;ANze1v-+TP@*Z%!ne-K-Vp=eBf#et^9G-!?E z`8h}xJmqB^hc9#e!M(=qyFzXp=cl=!-s97Tfb{jfh;fkj(R4N)^1i;4fSEDV@cjhi zIG@A)M2}xu-l9|q`8Yirx1~3h`)bcdE{~o!ZWdOX`_aD<@-!9kaQ%V37M^K2u&;Iw{jQTzY)qd%ML_db8p`}@>><98%X&>w_2w`zL7 zj$@&MQX=HEUySiTfwDA={}b67a?v!D>qn0ZS)%dwNG?jJaQQ$cua9m~R4U~1`YCS= z=;t76r~JXA1xsU;Rza@uT>NIe@I$}5Qv%Bqy`0)toavbtP3Xjuhx?H;`WOb*gXCNm zS(iT;2!`tFkd1zD!x3z9xagA`nAsg&2D*=w$P*W3qH6n)NAL)owbvWiPD_K=An!_6*0zgN31}fDLNKZE-5XkbS#-a|HATWM|DXl zRw~4|EsBffcPQwS6Z$ejyi9(-f+ovYMe{mnvWseBKYI~xp)Ud%WVB$Ue@7#}0Kp6R z91Zwt2z}iImC%}tK_z}_PVSr6M^*6I4CAsuw2;d10TmHReJ#d8ALNjq;)wYy2d$(; zEh!%2YaXsZNdJ&ZUuz6<<>@;=+Jc1?mxJ8sUu7|VB*a+jfB20TmK#`4Yn-5X>z307 znZvS*+uzJ`y}qD3x1_WpC+Msf9F|UVX~nHJ$5YF4iED5M8dz@adN2Ix6svr^WS!d| z@c6uOj00tUccYUGM6S6p$a49&W^#kHJ~B& z$9Q|%g<5^^g-tH6KzD}ksIXidf=xKA&{+4HP(AGQwHU#Aymh`v(V8`W_d3bp3r=pC zN2p{LYgjTXJjI?+{>wbwk>L~`IgiQtfTYGdGt!MsPfm5<@D^klMH2AfuRw%WmZT2l>1_ ze%*;nR99?j2NP>A=W&v@#wsH(r(2TVV6d0-JxM<`R4DDFTY6R|wto4(hNR;5N~-r7 z|9gbJj6XdWOi9wUh5;kvuWK>>UOlNudwE|jX_-_M)X*LF5ahA;#sIDUp85~Lo|-If z?;`V7`TaU$dnGUFpJ7W=V`(qnUuhL59;pWvo+K~naY$+GNP9W|klzO^76o#ANjte8 z--a!9QQFJ-@-Z=$l;;nq@FeXeeGk%Dd!}i+q99c|7uiZXNk2wOti8N1UM=if4D&l7 zj`;fmIr1gjFYm*TXX`4m9%(>z#ip;okSsq|h`h8n&aI@qq$)BpAM4_zsJ}Q?h`dpE za6k4F#f0PxT^wSP(f-_i>?hDf8;PFhi%mxR$^F>3P1CLR8w?CAD-!m`_~o3h+dn9G zjA(?kmvo-6k8LC6|2|Wf{Mc~7N;|1f53(6$RJQs8U86@T3To6P-Vh?LSyikX91!sz r2bMxA$CJGN(fQ5LMcvS@6Z!sv&h>69jjz7!Us|YJtTZ&(z_R}X-sh{^ literal 0 HcmV?d00001