From de0948c30bd1ccb04083e1d3613a28dca071795f Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Wed, 12 Apr 2023 18:21:27 -0500 Subject: [PATCH 01/31] Reformat autonloader and moved non-pathplanned auton commands to new file --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27047 -> 27097 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 35183 -> 35399 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 39197 -> 41197 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 24753 -> 25909 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 22691 -> 23285 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/Autons/MoveForward.java | 43 ++++++++++++++ .../java/frc/robot/Autons/RotateInPlace.java | 43 ++++++++++++++ .../Module.java | 2 +- .../ModuleGroup.java | 2 +- .../ModuleState.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/commands/AutonCommands.java | 10 ++++ .../java/frc/robot/commands/AutonLoader.java | 53 +++++------------- .../java/frc/robot/commands/TeleopDrive.java | 8 +-- .../java/frc/robot/subsystems/DriveBase.java | 12 ++-- .../java/frc/robot/subsystems/Kinematics.java | 10 ++-- .../java/frc/robot/subsystems/Systems.java | 27 +++++---- 23 files changed, 148 insertions(+), 72 deletions(-) create mode 100644 src/main/java/frc/robot/Autons/MoveForward.java create mode 100644 src/main/java/frc/robot/Autons/RotateInPlace.java rename src/main/java/frc/robot/{subsystems => ModuleUtilities}/Module.java (99%) rename src/main/java/frc/robot/{subsystems => ModuleUtilities}/ModuleGroup.java (94%) rename src/main/java/frc/robot/{subsystems => ModuleUtilities}/ModuleState.java (75%) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 87e8c6c98d4106580a18c5fe2f9f24ff5e73ed00..eb58fd46f4943b5a0ec2ea6dcfd403e1f44f7d95 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~j#p0{}4n1PuTH literal 17 VcmZQ>zc8b1qOQ9v0~jzT0RS-Y1ONa4 diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index ea55786862682ecb71f3891a8b5731e9936ea7ee..3209e227446eb3462bfa6f6583435701536bf901 100644 GIT binary patch delta 74 zcmZ2}nepaj#tkMCjB7TVO8j6K;M3MyATh5&hye^Tmv2^8Q08DX+^G0Ne51h&_RWqS WX^N9eGnE9&tNa=uiWuhsX*~di7!^1G delta 35 rcmcb4nQ{4L#tkMCjH@@BO8j8o?5ZHm!6>;=@rU?A1CGs(9%+gI`nU|D diff --git a/.gradle/7.5.1/checksums/sha1-checksums.bin b/.gradle/7.5.1/checksums/sha1-checksums.bin index f8edbb20b90616d44fb6ac60f0a394e95ee27129..b24fff70ad9add05876fd07b8ad807b1f64b1c0f 100644 GIT binary patch delta 169 zcmaDqiRt(hrVS<%jFy{CB@|c$Rz9}9{rTTDeFiY-65Q-7lfWVHwnb|B^(N!l5Yf*3 z%}Z52GVyEcEs&VkAOsa$y17@~kb~coh1>MlfA4bFrj2fWtia|>~r=$In+7nBA0iR!Oxv)S1U^7 zetU_KOQ_dn7}K~+27_VT^12(OA^g_f`<%)yuXCFJexILz?Dl@v_j%T{zH9C0`L^4* zr0vEfZA$`5x+@cTJmrJ}GHps+;*<%}lqrNf5f_V7wbQ1NgibR7=Q01c6zC8kexASf>orx|JHG{MxnNC4QT4e#aC|uE z7h^z-(Wl@kTj}v5) zF*Gde#_?y{qE3Y51*mOj&iA8)Dpz}EF05U{1X+X;WIU-)9FU})(l9x@qP^CLcnV1n*&@Nni$o$ zgwm4m%$UJ9&-;J59&BeYom&_J`-OuH{^|$Y zUR?e{(B-eXJ|(W=E1~dgIM~Ry=^(9x+c)aEoFB8V^y_k*8lK$~JZUKys{4Ki7YqKV zOBOKVO8wHSbGN13J`k1+iW$H3Q@A?)mCr?Nh#*4Cgu=-Lk`2 zdczqV7Oc(P*5>86X-gg+xg7jhW&LS~%y4*iFi7AFbB~S1lOAbTZVb(LTGrc^^3HLG zL?lRb8de^9N1wmwj^oO=k5bFTJHaY=&K-21o*uMQO-Z*lpY5?@#jeS>!Fmdiu6&N&3r>P*tgb_}Ix+!I(W)CLEqcKJ<&1;o z)LPe^q?r-Z|{}hS^{(4b|=g zs~a%d5s2E6L~3waqBdBgO-fD)riAZ2EhWLC5C~3#0Jzf*>qPB690XWns^YvWjMxm^ zss2X-;MmQ;6Q-{KJ;AjWq4-lh7$~Y;MF0c^U|`F3aHk(I7!IutfSC!PBh{%g5bZ)b zKFB|Wyi{!go#44D6jA>|5DltZ@&|=&F*j<~zXRYOg`n?8d;vWI3O)k@KnOg07$Ik_ z1c8Rx!H^@s70w61na#irWCk6JKsZbp3B=T@i*C@vAGpB8e*#OY>LSWc5g1HmB&Jb6 zAMakaG6uw8;2j(EvR{BVBct0)Mz;4UmEdw3&>bh^I_QjK_64bZp0K zRUy=~E6#QyB7sO8ED4r`z^rP}4bXResF$ZClyK`xxM(ft0nRYP;bT9{0T%gUL#a8Z zyMYT$ZVvXtVyQ2F3IxA3+$1QQs-QA76HiO5^Un>8badhi4~MRgtLi@{awHZMztV{? zkr+!!J0RzzsKeZnH%ckUG1o4D3w*f~g`W}j4t4qs7=}7S}IU=&8!tv*TV_9DzxC#C{TPoLpF0fAt z@@_6$gor8Ixg;6OB?!@k9I15h@Wxs&7xtZn!t<1ZO;}k^4VVl9KHG5YZsm$Mo`=)q zZxRMLq@0|Ud-#=LFLe>K1gP3eHT?fw>6-uS`l-9*1&=3r@yQC%0xR>K05UDGPA0Og z=f{}l38U;W=awndjBXuaV8Tb2zS%j{`eUb2XL!FI=2)7MnDptBNM<&zd?0ZQm7g(IpoB}Vp>Y3k;LK%MER1kLOVi{G z%5%k>%TWCBrZ4>eA2Y`H(ErI}#@zyIe;4APgQ##rqt&!^n}|=rE%;wM?GMW(Ex$Z= zOuH!GX^90`(^zaaA4GK~nbydQ@>Y998=imIg|V?c?O40*&u&xqFRa&E=pByg2>K!AM>5qT6ACk?D-(~Q zQ)9y@$exy*AK@|}e=Bd~LeZ{KU>(!uebB2lof3=Fv=3u(e!mMsK(*7=Bi|8`lwX;| zh||Opo>2gjzjBIn#naFQf_?6_%0P}{SO`3 zZ6{bKp?{3rQdO&%;FobIX{61hwyzrE- z>q^;)XP&>?fpv7{Zrfl+%_JJx4QYrjO&SCiW606Fug{i0yH%ZOld*5p`Wb9jV%KN~ zpL`C+dl|a{cF?Q=k>TfJZSvn<9C+}{!ND%nd&0e;VW3UGF?!+y9Qh(}GdTO&j2XZ9 z5A%=yt9%ul)f>!7pU3Vrkll3uGD-%one_7T>@rkB6xxEE4>y>>oU5vwWXz$h1&`mw z=|oCaFxhTZ*n#>B(NFz&3)K3A1YAd?CxaX&f-v@HLo3ANb5)e5|C)z>{=$uVsOOzO(3s9 z!C;R7BM4U;DXBPI{z|aU60B<+z$|`CG6;{~AcM3w=pgL){5CME5AgHgiXMdw`d(X# zKhNKjt9{I29&WP)kn-N|=E-ZOb!=h`?%Tjwp1|3ID|GB;p1!t(&-wtb^xgaDdF0se zGC5lwEMadL6Tnq1_I*G5&?~|FK$=Ljy3s^8;PJV#!zIS}zOB)i-KA(TMf^x-za)w-`Z9!J!&NSL`wSdJ8wVrx|rLoh}o3#2oKDX5` zWH!r5XEe!yj;fz~mrAoLuVv5i$sL)8ZpA#;-JHE#BuZ|Fdv{#DAr;-Ku9@J!YtWM8 zf8A5v>=N}^6uQOj2rW126H~NEf(wo#btKhee#f$YeNjj6{t`A zHg?;}R!N zPEI6QZTP@-7+QpV1N#(VDiDM^8E>khrmYXI5KCnOiAtyvDCA0!l8}kS3K=1l5rmQy zDM;8q1@oo;ywk;J0v?~NO&AxKstr!oTkwa5h&C3~|LS{VXSH>R`oQD&# zd=etAb}P@H3tEF@k*vSK1V9itsgWr}gpv?ylrj;Ht0hvYTqY5SL~50ogc(yXFFJtE zNZpCRa@y?Ec-^RYi0%EYZy3D$5Qk7@g826YerW(XK8i9 z854?ud2sDMfMe6B6pBLCy`7( zO7}&L_BgOEW^GI{(W}eSJ%%MQ8OyE#>k7jPlLz{+1C9BZI2!n1zGL$t;GcgF35|822_h6i1l_|IWY-o&2>A?7GCb z{mbT}ctzBmyJr-%kl#M9h57Xzn;~UJH>Q_g5{sGPyv!%mo@hrbJ zw$~~gDp~_$&Y1C|F>B9cJ-gDUgF0VyOaFZl6~3riHl)_hTp()}2nmNhd`#gP4E+kT zp^9p)oh|somhaj5m8*Z;Yp>`Cjl1qPFdu~~t})GTrv&=d$IkhJZ=vMi!b9D@y_g=< zY2&P(z+@EL1{7069ZKE5XOHIEWqee~oPNu~Ds*L*(`}dHCeM`t0C!Ag7FGBkJ|fkv z<*J9=4U{eg=2JXGMV4UU^WfTvBia|DTl0QI_>X$ zAh&kjb(5Dk64E$kJJ0wNG&%<>w!Z$?Sl6CKLoQoC@S0s}hI1OH4VytT4zOV{=l~0T zL?2+!vB^DeeO4+zvtDPV=IKhzS#u+d>HHjxJpM7QtWU`o1yh~csC@k26ndI5+H})M zYWXXAD?NzXb1dMnw(#LNVd-%zogi+S>EHpP5i}na)BnRglRcP>-I_-#7>+ z44J)sUg3-C&lWnK|MA6owgxf_g(+o&yj23ZS|d>^MM8y)R4Aolg-{@up~?d%1cX`y z9mioz{Y4c#ZHpX-rjJ`O`}#~@hraRPaC+#WR-iDoyy=c%;aEfY@%yNt%)WR~zv(0W zv}gH+7h3~_N)Wba!6m5D6cK7Qsg%k|RBfTkQ--URA_*xGDpeY4=QD4Z5n^FU7oXIt zXYJ7BwWk6Lw6U>Oe*WVxzrS?rfT8{|8>+FtH75u!JTXZdpMu9R{9E*oZxZAhC8b%3~1Cc#Oe7!8w)gkqISt`W)QN+pgHV%S@Q zK5d};^ZF&|x9F>Ku{KN&gH|grZ zTO|>bGLj^P5|U8M)FPZv5CXYUC>0A;NJN0e6EQdV-2}{m38H2q))r=HF-s;A{mK1* zPS?$U!V5nvo%%)BG2m}OHrD7u365(Bm0B(&36W4HlMAE@nM9!wAngQK2w?wZn2#|W zJ`;)is5LnomyTT;bZAm|@6aJ5jCHzMNGO2YOhu(3)wo!$lxmc?QY64-Dv_L2%G4y% zbW|dwD!>l$ta1RCPr{s-0MrgyZt&fy6K^7&mWA)0+YPh^sUZNdMxfLP6a+yk3DlfP zB@&?uosx1=qEToxVil}125=?85WwP^j$_6UTQ-ztta@Lx*>1BT0P~O5SU)uYe5Q|qE?`x?6g9K_5MII zZ6?CDM)`C});E(-&6F=YqPmEcLit=Jv^6Ru9T8W~G*3$GWRURCOP9}8CCw89y$wjL zmd58wi)N~W9UBxqK35zxQ$)W3ZMBR9i4NoMrW4V*0Dh&<8#F})8!+lCR&$4 zGow1(mYF7m(25sTdAqMk)&_|w%p_xI6ql%gyM0U>L2E+Pg}Wx=8#Np%v&J-hN`7yU zDS^;pM}#Sckx1qkYLX$OZl#kIZ%p;N)_Xj;>h&^X+lw6Z*yy}|9;g;!!M43}1J0lowwf(osW;!lz)%^F0UOGf>&C7(?iW-x_;uNe9eK8O~ zt$IEtox5>P)>ooWOP)TsX$`W`SMx@VfvtRa&9>}|SG4)Qd~dJ>SvS1>dCgw`6;-t1 zjlv~uKxTvGL>)Elg`54h)AX~mRWV>W%*@5yyp#33W;^TNf`22U%ro6VA?y){b?~8$ zxZ7siBHE~8T?POU4%cHY3>0^VTeCU{fxiUsvV$ETU&eqw26>xGM=FS*# z*QA;;y7a3Ziu_D6<~D*3irXF3F-8@CwFS%3%)>^+3>+7$O&Fe}AA!f~b%VY}^(n6z zf3`7$GG04^Jm@XM;NqEhAl9Q)t+^30z)Ili|{->-G#$7)xK$$Ukn4oAV=WoiUdh_Ogdn#%=TimHtv_d6T}_{W930 z2(muqsHHHrF6AUR^^xIr*msXmRIPD zgS8_2hcvKY-|?@(a_ZVW4~qZF$!h*J`f0aUJ*_e?(pTTS3bM+)N?$E_C9mJcR?Ykv1MwE*FK93wKlAJf*W|0fWAR27%g-Er5Ng4;hFXxla`=Sd9xjnw!{GRZ&Ew6OL))(K90 Nk2#bbdWYQ)|6jLBG_3#t delta 14117 zcmeHNd0Z1m8+LX%6>PXcKtzfhk{B*)6;VW`UVvBCHll$92#~`h94ajyt*u%WTxIYE zLG&wLd{|MdAc_}C!2`r97Q9*z6^~l$`OVG-2&hOg{r=M5&&}?>&ph+a%=^wf%+A(j zhFg~z?sneo;`w`*E}p-yix7Eu^Jgc-%e*5c36b7mab7XA<7bCOd(7&B|B<12<99y< zhRfnb5`i>c!FKoX_Ml$;@Mr5c&~G@n-R9NK6u>21??P$-In5?4hObZv6}%{Z9G@rV zizK{oc^ClSpz9-yaDF3#Czpmwl>jV3xPJKlMLT>9nwLreTcVtRs|Ju$N3}!C3zLe) ze2GB88!i{c3H_NT4lY5m5*F)(Y#5R>f1bW*{IDPEwV_=Aa3an#v~O;Ys(y<9b5CQE z^{vI?UR?upjL#)44tUmBV-L#%fGbhi0Rm{h8rUO-aazv4#7MaLo74U`^r|kEQNb(O z#A$|LWP1(smr4{$zC@|`cw)Rz9w%&SlRSQTk;To~KMXp%!(!a8Q|v(ov|j~GsSO7W zh+>9!aEExC2&cT%Yy3WJ-j^leg(nv*k*|S=R|9MC>fK{Rl^*gS4l`AL+Fq3t=m2P{ zF8{x|EZ1h%<{q&pN}0+&YtLmxY^Y+kLMaq0cqpc1vC8KDUstwe=j^iE{O=;ebBdnk z7=ug-2yJyS*%^j;LVn&BP(lCE!4x2G8M%hX)+Exlo^aC9t6833YWaTP-~M zlCU!s<01;S2QOR{BSgO(cOLXR0L(0S@iI}2C^U+V)6;|N;mPshc<`Y30B{C82u`D3 zdQ{8?4AIax5eWsc9K^y27s(Y$`cBMOM$)|WZ(CmbpJ`r-b;$}Qtp#jwwH*gpXMm|W zPofNy%7txssIkXsnvCO?)UxAlDi+^&dl#ml$#~398%|hv2Ddpy40=%0PPl@`c6^hR zf*v-!FuqtQ=PT$$leb}`z-77vGr9qb)EuA-XRZexpaL@vc(2%6r-{RA z=9il2HM6>BfZ&OB@5k8WGWK8PE|oIDiu(K)7c|MXI}e!}1PbF-tFVFxjhumbS3I+xmvQIz*CUbsM7NIWX$k4PSi{VQoLOBObZ9EE;n>4ebq0X&!Lg59< zCxPmAyt2;(Cg!{eBR(H13RMeGcaA4wD5nE-R){5Bnu!vo9MgEe&7Ux(T*#3{N`(?p z0*ZMN!{*KLL?Oe2yDD(jtT+bDsNH8BK_w=8;}k(Rt`!-I_O^V~?(4K~N$1R|=yR^% zayu@fwn4LRR|_-dV6;R=+%il*H1t3q9rbit()IT|hyN}6~m_Y70YHp96S{s>z6 zQ@aKDcET5dnlG?%D)8w9N0>TC@-$!y6Q=>^P7uveC(2z2Ojl+)`;`9UfxPT-_Y5%V`1;&ISz-xY zOA)OGb%~|sB#9Yy_wqkzGNVP_B}rV~SB*5z1+MK)V*fyiSpGj$NN18*U4c+3XH4M= zcVP5yDG-XE=%!im;U@<@sWUvIe?x#kqa?HnX-a*--_E1-(~$h=1=n9dZ11Z%22T`tO0^^l{2r5+;n36hHGYHv`eeDuPJ%VT~bj= zXaTfu`sj97hY4mSKlV6i0sTq{H>#i^3ci?6=)t?c5kv6Pyj?^$xVeTHjGuN*BXp>t zPjukqTGgAGUl2O*>s)lvyVeA{MvTGeD|-liD88`XACgq;j!800SZ#>LqMc~z;4YC{dlURW z&}F)Cs0^6=Kg5{@RS;~_iRn&VpwU}IPXCrTqqm1R8dqoT|AsO{TSKmz6K2UrQO;;c znDsW^=zAb2HLmCt-|O20cdn*p6Yne1EUCtcCK?fHXrHK$=7bs?gZ6;lPpQ?J67Bwh z)nTQ^)`UtkgS}iqXHxVvsdi6k(g}xJOEt7ZWI@?Y?uM^wt!6W&zKSz>KiwAM1{^h_J3edzwfeI64?j+Q*aYhM=co^xK=fy*51T+5(i$H& zf!_Z8>BAQ-) z7W$CNoc@BjSAZ!ix-roz%f`8;ykFkHlPQ76=<{A?EBAHRp^F`5xQ4%z%P&?bmEhxK z9J(CI+U)uJ&_$E3cMb43VQ}J6=tm$E+6MrWmHV^}T42XI?CNbNVifqB-sGA%;BC${ zpZ-$0Fl`bq?_kc6poSo=nQJ2ClPR#U2f(N9CR)CYgOV)dfrglVPU+vx(X4w1xA1(5C|C)1_gnPnU4%V!(x_Y=Whob)zc% zF?3-QpXqJ7iEg=j^(oeOj&LKJF!~g~*scX&^DwkJgx7`D_7L7?T%uSP&hLSG>KR?Z zR{9lWaR__u`}np$hv=>S3g4FZ0M;;pgHX$E(Wqq{qL*dcgJnxF=XGaz3|S`?(*ANA zoX9S$5p`Lkb-y_HZ2rvH2d+u72k4bged2@d!dXjgzKg5P`*W%>dgV{@P6w_h{3b=% zpSWW5XbO7O_Mu4)?+5I)H=Mtvw%`rh&*|-2?$vz0lJK+4m%Rc(8y&Y@Tc_|0 z^NNthg@?uoqzO^pkzzE1^iSv?7RP5!y?!(k`fenq!ND7eP%sAawa5+Tf|wYRwR-w= zn}tt*THDoos($0c&0_(!k;q2hq??eeQ{_I1T_?}YomiDJZN|O|H;hOJx9KAoZg7Y; znPf_`stiUCkR=Z*czH9rat<(@pl(*XPqdH;LP=Id_0zC~pDQPL7d_N_%=e$7j?hAg zDp;#auCnA{56v@c?%7%te#bX;bG=qK@DgCG+O)L^h*^^sI-e3;>bv)o#*^V^PgQuT zfLa`6g*Ce5deoTuEBZR(J4TtdF!t7-gBi;KHqe(f=9py13&G!wODoGKN8EQAmbAnb z1J=+^Rl`|EBpXKRlQPuc-%no~d2iSB?fXrx&ssS0nLGO6)ofO~7c&iZ{Cr2oCxH*D z5-Qgp=*c-CP)E?*HB8}zZluftH##=>WWkJ?fBR(r;p4QXbmD!K>zJNKXdy()qXIc| zcR{*^gTCYBy;&Dejt|&ptAFD zaM0m5l@*6X6A+JPwJzal@Ke5tmMh=iBTM^%3dn>6i$!iiAu;=7@y(L{1qJCQfPY-E zaUg)S0UlKCkc!yW~jCoe0aGmt!#?gGA236caL&$wBZ_?xeC=rdb6czBN-qB$olU z;T%MuY*f#|kJ7(-mR8woY3hqbc_rz4Fd`dVqk{s%8dI_x>iIGSEUQ%NrN`?^wbIW_ z%u+Y2-Rm(!JJRk*)U(jq#DfoeZkv1ExQjXh`?uSS{12)+^&A4t%cZi(ro-RxJCV)!uPW_5KGP(tdQWp*TeFsGQ z+?r$|+S)yKf8@Wa>FI4|xbTlMV{6c2ZN4*PXy;;FObFHS{ zR&@pqz*&R)7(LN_9Cbb|J^TB!-)`QM$ctGs1f_SgmCn;NXzf7iGqly*oe_EE{Wuw$ zZtt&KQgR9>D+9o^O>hH5Tc1Cu`}!p}yk_iyIeM#a8Mj~!=nA$ljNw^cD~Bq3`7b#4 zWbl`_d?w?{w>0^<4e{Jxys^;QJaN~g{kMG=JNW#75!-9roK3(7dX4J1l$MxwY8e_$1 zZ9wXPLlcXvqa-3G_r$UyN>K%F$xpY*xEhE&az9Yw6}0 z#_tK*S-kJ|QO7s*`Tn8I`1nwEaadHd)mIhBjPI61a@G{<@#N(M|%BbcSX>5*Rg zC!1Ap7T;SLEPGK?F5c{)riNC~3XYBBG%2-UIw|<-(J9wsw+M0$n-@qpl|N%C#~`$5 zgn9woIdyCGdmp-geOi9r+U%>B4y3m#fDmvdhs;F7H7sS}^An@z{pc9^=&GzCQ6FFf zeMx7gufcBBfqK4Gr}vL7-#g#@w;&8ir`wN@)4RbOE*Z;6pJ(Q}?$bwK>Q_1EZiSXt z>R{$`DubZg-%=m}wZHnZ&Z-H2n#v7&j?mt*FE>ODoq*;iq*2*0)SZk)Q2yK6E&e&H^?p^B`xVhWlA| zOIE}h3iNYEIxSxsfEK@ID_XIzLSE!a#v{t$et~kmYgH8s_#0L)clhY3x|yCa*l>^+ zITumd>ios6i`Uy=Q?_u7YGeh$`0~j!01pqU)1m*w$ literal 17 VcmZRcbt>7YGeh$`0~j#R0RS_71g!u7 diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 8369058f9a02f4629cdab73db61a86e59dd2fa14..94f5309bb4367991c809b4092f40f269b63e3085 100644 GIT binary patch delta 4078 zcmZu!3p`Y5AD$VP+-eTfMbpKgiQLMqd|RYau_a-pC5fTjZ>wcXMoC!3$Hl-*3*DdCv3vpa1)R|L1+r@G2=|Vv5j0 zlYCQTso6rN>W@r{(-}`l#F4uJe4M`>OC7qaZWKNCMFN%-TGJ_5mUouchw9492+R|R zEEh(QF+T5xFx;Q%h0uJKh_}d|f|UxVy*jNr!4_tmzQZ_Pzm-d*rfCjtA@#`?v| zKRA*Z{_{9|8qq(0&^CDDcu0hf!rpPs9lA&h%a#5A%cV+u8&l{IF4k;E)`54?<>sk;0_neN{{ zBW8hERAf9LhcRv?l>Y1GPr3-r83cz+2QX%g`h7uDr-H;N7r2>eQ@1^{jh46WMOc0o z2sQgcH7*`i_NZDc5jz1d^8w7bbnjRo(z%1PL z$pGp8ko1NqZDy&Sg; zbc5FhnpOp14)?0kT-MqqA&`3!Fs+&~$J+%D!n*HFAu#`oXwV7{KjhS+ zs#^n#AAMzNypL7sgiwAe=(i5Qeks$(#E3Qn7q+FZJ+olrE**adIGL+L6!Nhwqk{2Pj_urxy- z3$HrIfxCb6z-Lu85fuDr$5_3m$J5`$XXxr7gT?j}JARh1^6;o65`7ss9J5s3Aw9MX zJV~Z&D$TbXKYghrOt;hXs%U4e55dmh%L8u@x+ec+*@xcsD|Ek%TMOryKeUnDTnJ8j z&>7nv{I-4LWqbBk-tq9q1txlun-<82asX7MER~PR$g=?bF?8@_>TGEGO8|&Wr87#; zM(-%VZfr>80<*e!>XfT7sj;tcE#UMA()wNBB zxu*#c*u&%_)!G zmKO5JPmQoVHEu@PgfwwHW%OiI~&RQQs+XFjv0z0rg*u^+Krk0@BSK>vMnxM2@ zUa7e)n-vv|bM=1pdk2>-ToG?iD0-^i0QF27i0~siB`O4rxzfQ*SE4{sU7~A#Hx@J8 zd*n{rKm3~BmsA%0yn4e=s6u>wA_7T1NW@D_m;)^1sO-eZvHj=cTKkRL<;ZLmI1A9K zaVN4ODRsyu7Zdg1`m?#A4KE$u-DT0#(Lz&nW+w`*I3%l>P~sUhoXT0}ebH~4qT$p{ zc!5)S47xl*;iklA|J zq`Btp%?&;)HV3q4{!<~}FRQqnB1{(?`}aNClj4(NRij4|1<^(UaMxKUD^barKD})c ztMpfVNX?vQ+eSzlhlMD01^ksyDlOqyJ?Wqx@6GoP$zP9Xji9xrW~Zh^A%ML~(>giO z^tR0D=JHeeEamtV#AzIo3|3{4Ob<`YhTi1Op@QB^}mg$Hhlx*|B|Kj*W)7U9? z3r_#=n$?x{4{_b4Vv-2HB<=o0hxTD~$I#i2G zNF+rOfT=sb#O}7M?QL;jnBM{iYIY;e@NvYU;zN)stc!Z*3ucU?YrcVxVw#=da+C<4 zXim5cECJ+T8k>G;+S={T`*;_Jr=85LsAw7g<;Z2rMxEz==C_jc-RXvP!JF6EZ25tR z$~o3SG2?f*n=T*>h%t?m3J+6q`jz?v`3Tx9m zp}f%}z<~W}b!xOEYAnH#i)(;asi;MAvDzV`+%RX4kwJl=`7%-*9U&Ad`#7?ScZWm# z@*-}3J-5N!{o04ul6@_i*vREc@`p(HilEhe?cIT}l-H!q^_H>gm%u+gu!}gyP!~~h zmGCZNHw1+*IaK_mSCiYmq1t_t;P8*y5!NKKyR5aZxBZR6Re5W-&w6|#NcFpMi9!=8 zD8I`ps?6?XdzY`jo^(8=u6ASmyT5zD%F#{WJ0*=>9vpJ%>MvPSRUiE{D!;-16fjx7 z7@q@p$iILmsj8(rdrWm-U%+uW=UX$u9Y-_F7h4em(j2y|BDXNBY{o~gm=mMddre;X zDk6_Rx^M-gQ{Wq_w8_T}VNOrh74=LU9E=lrQsF}j)2cD;!zD1SAb65Wi}R|uvDa|Z zW)G)^j@47nafCwLjarpNag$Y;Q`>AN*{X}54XZlE)0siegrsrA9=Cudr{PJ;#EUgM zqr6)*PQ5E`+E`pR6Pc7t#xB2CRwXbf;&KuJiLWX F{sSL}1Zf4VGx^^%0TV^3XyH#YnPi#5J@{+(nJS z;>t$I;SWD5#2*HYQE;MBiM1FXA?~V)D8z(le}GL8@ewu}caxpDmr3T{IrE+GobQ{N zsX=w*y1K8xD4eH-#vuXA$adO9z6!KuMT0<-GPbBuZR$n3IYwv94FZ~0iFmathy$zJ zp|kjCi{-f*hioDIXY0k?tV?KK6GM4+kd9}Y1f<&s@sHg|tvN+3HCLpsaz`|Hm_Lde z1*w=Q_=pdMBN+R^2wjm6Ekz-^bVH9C<3IXP{ZotF7Y@fl3|=Rmf_}+&rhPcNXi}KPR7kve-29nKE>ASLbn0 zLcC86Hp>xJi%KPgZPSaJS&)t7Qgl~sg(hIoSiHj^fiw(-4O-iD-aN#^1ol=A*58D8 zAHkD=4DO?Yfi49+MSp-`WvMV%{hp`|rb*`D{xs!YM-rC`d2QRdJNhp@<1B&I6 zlh_lqE?^daC>#3sm<(9C&1V#!A;=0bn|NtA93f%~m#;|sHjq8~wc*UM(%X})abA9Y ziISQ<7ti)uk?zSb<;@(fxwGxXYTnrpW+c{{^IZ>C zvestGIk);bbQJ_u6&$!?IKsg4I;H8qkXuJ^wt^RwXp~jzI#rmK56Cr)RCf+fdW)UR z?TwODRf%FI@yx}W+}XyH)Z@omk>^*MOkls?u4Sj;Mw>qk5t%0y$l_MPiN^}hos3U@ z_R`9^jK?U4qkSLI!8;IZcVR%{-fknNBm$kpt9P5|?fM59rceFLHn;w2>VGkD_-3GZ U$KdQsdnAjfG=I~`sS`{82b>~nMF0Q* diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index b850247a19f3b9e5250883d2b8bd63b29cc48ff2..6c4afcc6a6ad61c14582180b2fe8fb1720614add 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~l~t0RSvu1N#5~ literal 17 VcmZR6A~{9FsWEOA0~m0`0{|>T1K$7u diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 5b0d63e2817d78b4b3db9ab8f808978bd21999b1..f40653aaed9f8ad175609d364c8657331ace08ed 100644 GIT binary patch delta 1925 zcmZvcc~DbF9LI^EBDUlagM`$OfCj>$C?Z4!Dn&&=B8m_Jp>kvdQAorChGG&5SZ-l~ z254-Bv7*sxNP|Yj7(_Y50!na{LnxPuv`_^^iZ9empm&d=8!7$1b93{Mk z@}+iH2g#f{7)D%{uuP3GSoUJ0F6BW4vX+2DL;)naC_7u%xXB4Itt4~A7Z_}-VeQ!1 zv)c%K4SJTvV9#X|@^`zW!Yq6}a3%4e=0_Y;M@v~60#hVX(k~cVuP8BgDP9JU<*}Oj z`@GZ3q>9sfVICC&e92-gTT*kev@ITHkwP%3VnxO!tuCVqWPL6WQYg@L{Dw&*R}su~ z!b2od3JnA4|0A#cwvN<{M7Bzl=GGW&msGs9q-Hq+sbJFL3bgD#b9>&P=mY{U0papS zh-P)(0>>d{!Z6Yqgsz}NwmJC@)qS}A2uudKD!4N3-THyOxKJ4Gn*_Bhq9Mm=`onG_ z%K~r-2+I4k6{>Y0(z(d z$nX0<`|D*Q0t}pT!y0dlQ)v}mRmp7(=8A_50A2{F9a+ic|#M`0c{0p`=_P@E{EV#@ieY#2t&2kU4Q z8aw`at%pHt69U^c3GBi5zMlT05|~H!178{sIy&iQ`30S>xoKT+mlh2rOA|ywR%$Vl z;037G!SnEZvH4pQ@oIst#C;o^V76BE)l@Z&)DxOLu%IVSK9udB<&?IgXV(GX9qfmw)LLb}5VvlF{DgFsse+kPU1@?ggvZvEjk{&C#>-q1@|Cf1 zXrqdeZgplb|K!)q@@J&HQ1$VUUAUU!>|=+5uz}}}$jtm&?<7C?{iC8WhOB?gdl*(0 z{%fL;eCJxHx}mj8Wp>_7nBjUXm*%A)f5l?{x=stE0j%f7+>XUP7#MK3-2jZ3K+ zXrtW!QpJrYkuSKM&Nl(2p{sh)O2tdNJ|DiscUsgDnr!5terdEyt-r#EZ#m*J!U!wg z`7p;u-C|k>KB>KcQdHx8f1H9R(o@u(`KiH$-1-AAJeZcF#IeGWlY#|G-FzeF2izY^--WUtwb;ou#XBMamjgZr^P zMJ?3k$_p*GTxNFL%$t52p;0~>y~7xLOb4!8o)YB^EUr4D4ZVf}wtnJ7xLjSdfhkuwfjCBYL5rQqNso<{T1JTKU3lKW zEH+!~T-PCP>Pm?ZQ)($Hqr5OFL9h+KSs>C0%yOt^qKzp<T83_TC)*6qq zeH-BclXe|olkOdNvwa?z0h2`_sgnjFsFTJa7qj&tAOW*2BOw8kW+bPRB_+U<{UtDy zMJ6zljV54|1t&?9W+yL`ttUv64JeS4wJ1oFEh*TO9V&Q}#VRn94J#OvRV#Rtcl}9%Z0~oLh0{}Eq1PA~C literal 17 VcmZSnIL9>cl}9%Z0~oOE0su7k1g-!8 diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index 280d7a8f6e3f39081be44cdb59f5af3e9979111b..e6f975d6cc4fb58de6017108deb037cdac3c138a 100644 GIT binary patch delta 758 zcmZ3yk@4$R#tp_2T8vzc*)<}^{?(;0fI;B?|4`60KXtp~-9S@_c*M=kt`ZuI0^Q3F z+`JJHcmyi?cXFy^gTTb!f%~@1=lTH=jO5>ZSMnkw|9Z|3KJ$+*gsNOTd8zaXfz{EL zSA6cwT?!G5x;QyiCO}}_p_YaThR1F|wLhP{Rz^W!WBNMY3DIATp@QL)kIE?W2Xm?2 z-Ew*nRQK%3cV%Ysw;kYBTrlYhRLpU5uIx;HD=&`C7lgB+dN*$VD;vqk-#P!?KNhQ> z5W$G!lYQkI1g_k2^gsT?#R#f<|K_9e4vYe?%a0gdUT}d2s{iNYzX}ieE65KK zj0&FIs=Psi_bDUeHtl`8pn~PFpyxY!N8;V3T_2z(yxy#<0t|Wy$;V5VZkZkf5smDf z?5lc0Kt;mc!S$#LRB2S|W?nU*Qey^V>z}_wKSGtxpRB8{$iK-vkZsXAU8rT;n_bm` zUbT1Wi>|MEdlRbCb#kqSg@)tj%5AY9jn_hz%EH{`vQ+c2!X5V{sQB8+Yc)=)bM6hS z^;Zjp8gLkfkpURKBr73|zRRlA9a eNs@7+qJa2jM~@_d$>+mDCnp4|Y&HnF$p`?^Pyveo delta 173 zcmV;e08;<;wE?5C0kAU|5&-FeYkU;O{(fWt0000008ziQT^J| chooser; private static List pathGroup = PathPlanner.loadPathGroup("trajectory", Constants.Swerve.AUTON_CONSTRAINTS); - public AutonLoader(Systems systems) { - m_driveBase = systems.driveBase; + public AutonLoader() { + m_driveBase = Systems.driveBase; eventMap = new HashMap<>(); eventMap.put("event1", new PrintCommand("event 1 passed")); @@ -38,49 +38,26 @@ public AutonLoader(Systems systems) { eventMap, m_driveBase); - for (String path : Constants.Auton.paths) + for (String path : Constants.Auton.paths) { chooser.addOption(path, getAutonFromPath(path)); + } + + SmartDashboard.putData(chooser); } private Command getAutonFromPath(String path) { switch (path) { case "rotateInPlace": - return new Command() { - - @Override - public void schedule() { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 1)); - } - - @Override - public Set getRequirements() { - Set ret = new TreeSet<>(); - ret.add(m_driveBase); - return ret; - } - }; + return AutonCommands.ROTATE_IN_PLACE; - case "moveForward": - return new Command() { - - @Override - public void schedule() { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0)); - } - - @Override - public Set getRequirements() { - Set ret = new TreeSet<>(); - ret.add(m_driveBase); - return ret; - } - }; + case "moveForward": + return AutonCommands.MOVE_FORWARD; - case "PathPlannerTest": - return autoBuilder.fullAuto(pathGroup); + case "PathPlannerTest": + return autoBuilder.fullAuto(pathGroup); - default : - throw new IllegalArgumentException("Make sure you enter a valid Auton name!"); + default : + throw new IllegalArgumentException("Make sure you enter a valid Auton name!"); } } diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index 3925303..298bed1 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -6,7 +6,7 @@ import frc.robot.Constants; import frc.robot.subsystems.DriveBase; -import frc.robot.subsystems.ModuleGroup; +import frc.robot.ModuleUtilities.ModuleGroup; import frc.robot.subsystems.Systems; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -27,9 +27,9 @@ public class TeleopDrive extends CommandBase { private final ModuleGroup m_moduleGroup; private final DriveBase m_driveBase; - public TeleopDrive(Systems systems) { - m_moduleGroup = systems.moduleGroup; - m_driveBase = systems.driveBase; + public TeleopDrive() { + m_moduleGroup = Systems.moduleGroup; + m_driveBase = Systems.driveBase; } @Override diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 5e56e14..b92adcb 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -1,4 +1,6 @@ package frc.robot.subsystems; +import frc.robot.ModuleUtilities.ModuleGroup; +import frc.robot.ModuleUtilities.ModuleState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -10,9 +12,9 @@ public class DriveBase extends SubsystemBase { private final ModuleGroup m_moduleGroup; private final Kinematics m_kinematics; - public DriveBase(Systems system) { - m_moduleGroup = system.moduleGroup; - m_kinematics = system.kinematics; + public DriveBase() { + m_moduleGroup = Systems.moduleGroup; + m_kinematics = Systems.kinematics; m_moduleGroup.resetDrive(); m_chassisSpeeds = new ChassisSpeeds(0, 0, 0); @@ -34,8 +36,8 @@ public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { @Override public void periodic() { - ModuleState[] targetModuleStates = m_kinematics.getModuleStates(m_chassisSpeeds); - targetModuleStates = Kinematics.normalizeVelocity(targetModuleStates); + ModuleState[] targetModuleStates = m_kinematics.getComputedModuleStates(m_chassisSpeeds); + // targetModuleStates = Kinematics.normalizeVelocity(targetModuleStates); m_moduleGroup.setDrive(targetModuleStates); } diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 67b2cd5..5a55b8a 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -5,6 +5,8 @@ package frc.robot.subsystems; import frc.robot.Constants; +import frc.robot.ModuleUtilities.ModuleState; + import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -17,9 +19,9 @@ public class Kinematics { private final AHRS m_ahrs; - public Kinematics(Systems systems) { + public Kinematics() { this.fieldCentric = Constants.OperatorConstants.fieldCentric; - m_ahrs = systems.ahrs; + m_ahrs = Systems.ahrs; } @@ -84,7 +86,7 @@ public void conv(double[][] unicorn) { } } - public ModuleState[] getModuleStates(ChassisSpeeds targetChassisSpeed) { + public ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond / Constants.Swerve.maxVelocity; double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond / Constants.Swerve.maxVelocity; @@ -134,7 +136,5 @@ public double[] getVel() { public double[] getTheta() { return theta; } - - } diff --git a/src/main/java/frc/robot/subsystems/Systems.java b/src/main/java/frc/robot/subsystems/Systems.java index f85b775..e63ea44 100644 --- a/src/main/java/frc/robot/subsystems/Systems.java +++ b/src/main/java/frc/robot/subsystems/Systems.java @@ -6,30 +6,33 @@ import frc.robot.Constants; import edu.wpi.first.wpilibj.SerialPort; import frc.robot.Constants.OperatorConstants; +import frc.robot.ModuleUtilities.Module; +import frc.robot.ModuleUtilities.ModuleGroup; import frc.robot.commands.AutonLoader; import frc.robot.commands.TeleopDrive; public class Systems { private final static Module[] modules = new Module[Constants.DriveTrainConstants.wheels]; - public final ModuleGroup moduleGroup; + public static ModuleGroup moduleGroup; - public final DriveBase driveBase; - public final Kinematics kinematics; - public final AutonLoader autonLoader; - public final TeleopDrive teleopDrive; - private final static CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); - public final AHRS ahrs = new AHRS(SerialPort.Port.kMXP); + public static DriveBase driveBase; + public static Kinematics kinematics; + public static AutonLoader autonLoader; + public static TeleopDrive teleopDrive; + private final static CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); + public static AHRS ahrs = new AHRS(SerialPort.Port.kMXP); public Systems() { - for (int i = 0; i < Constants.DriveTrainConstants.wheels; i++) + for (int i = 0; i < Constants.DriveTrainConstants.wheels; i++) { modules[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i]); + } moduleGroup = new ModuleGroup(modules); - driveBase = new DriveBase(this); - kinematics = new Kinematics(this); - autonLoader = new AutonLoader(this); - teleopDrive = new TeleopDrive(this); + driveBase = new DriveBase(); + kinematics = new Kinematics(); + autonLoader = new AutonLoader(); + teleopDrive = new TeleopDrive(); } public static double getLeftJoyX() { From 30926d7d585e288ebbdb6649617bfc6d97ebd51a Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Wed, 12 Apr 2023 18:41:34 -0500 Subject: [PATCH 02/31] Added the PhotonSybsystem --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27097 -> 27447 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 35399 -> 36155 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 41197 -> 41497 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 25909 -> 26079 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes src/main/java/frc/robot/Constants.java | 28 ++++- .../frc/robot/subsystems/PhotonSubsystem.java | 107 ++++++++++++++++++ vendordeps/photonlib.json | 41 +++++++ 12 files changed, 174 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/PhotonSubsystem.java create mode 100644 vendordeps/photonlib.json diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index eb58fd46f4943b5a0ec2ea6dcfd403e1f44f7d95..37f44e6fd9c98d20948834233af40068c9f64819 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~jz*001!_1TFvo literal 17 VcmZQ>zc8b1qOQ9v0~j#p0{}4n1PuTH diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index 3209e227446eb3462bfa6f6583435701536bf901..e5515821b2a67e0c1037dcc43ae7e4c6a7a0faea 100644 GIT binary patch delta 524 zcmcb4nQ{9y#tkMCjGH%`N^r0XeAbj+%NL)^$p8jfmp7ZrS1<{vi>`_>cMLcT73JQ% zRFRR1U-XfwuI|}hsNllMbCoRw94>F@xnjEW0YotK=H|1?4vYf6`*zjcwM{C8D$SqF zs`@}6F7!{*^ofP85W#HL$#+#}2qd4lu}fcb(LSh3+s(OZpBWi%ZdCjszR|#eeY2y- z48_UIGx_SREN0I#w47UWD&&|%!-dIg3=E9z<)@E#Kr}Kg0MdHKKtdCU6+u|w`&A2p z-j0n^^FG{4xD#n50?~0I1}F&xf){`UnvN+?x9HwrDpZkW-8(JOS^5=J#~q-`dLWiQ zh^a+$9%I!1JJ}U;S6k^99k+iE)Y3X7MdTDvB@noO0}^PKEO;<)!QB;VK_V>&8cYAD zw4mrm-sRF Vo&WG|>RE^m{V1>o246vt4gg6%l5qe4 delta 63 zcmV-F0KosZ)&bem0kAX}0j#q%7zhWmRUdEyv)v*20h8_{FS9iyI02J|CD@a7CXllo VC+h(LF|i@|7qKwt2eUy)m?0&57~TK? diff --git a/.gradle/7.5.1/checksums/sha1-checksums.bin b/.gradle/7.5.1/checksums/sha1-checksums.bin index b24fff70ad9add05876fd07b8ad807b1f64b1c0f..3e7ce19e585a7416027951c20c727b4882bc70bb 100644 GIT binary patch delta 1043 zcmX>;g=zOJrVS<%jP8?7B@Xhp?D2o5p}LF#47%JWzm?b^&{cM1|2n1%u@J$YnVY9d zUSJfMbH!raLMM}HP|@?7W2H|p3mi-_l+{T(I~gL{%eq-s-ja=9Q+h35d@d(Mu>1RF zU)2jN0!lxWYnX|nt5 z%nhPVKMJDFR}fR1isbiZ;k`32HU_j#mz1=-#bNg7Cr}qK37lB~(Y1?r^1@)@`t3XF zIJT&j$cx!FsGa|}{b^)BFGSs08;CkHP_9EuDhth-65d!yKCqU1C!)1n<9W!HdY~$h w>6d^30RjuB0tqx-Tw;91I^D0_XI1{G70}U1Tz~#Yz6ey?6R<7@onVkQ02|;g761SM delta 91 zcmV-h0HpuBngYj^0)h}oPlT|TEv$in@0h84-sI!$bQUd}VX|W-w60<=_210{cBMtxn diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 1d3a1dec9d2ee311133c9be59aa241b2dea58d21..5684331ea76dfe58dc9845d430ccadd1841fe513 100644 GIT binary patch delta 3958 zcmd5At@3i8o(UIN2s&AGc25i1s176 zEzm?cMiFIzAtpqq0!5=@h-^#MkYM5?R-@)bizR6qURF}0B}j~|o!RB(N={DysC56? zZ@=$8e)sPE-E(h!u~&VuS7Uf%L`yn}!6p0lTYa1na+&vR3EL$wf620r}7 zKm3gMYRR8&etS#MS$y!V*DsdX`}()4;IQ-k-ImQ^i&9yO={akLEr-d-v7tr_W3wrc z1jX71H68`wdzw764_}NDgim9^Z<0fnKi;wpS?qnRED8G}ZVt)&-n$YLExp$x9~k#z z3kN)I{?;J$CL8n~e0-^jr;le(|Kkn$fi>=%if`dF)*z7yzl0hmfU4u3Fvv$YuM(ge8k%N7I87?@WU4B`-Yop~qZxyxE##%~-zPV0*=u!=?|1e7iFyH|5T& zD;oEazWYC483hM9C#ah-1jnOH6hOo0FQ6 zqDo_O8Fem!5gJsB5^91pPzF84VtRv?ASo@*;iSf-#|YBMajb!2bvjzlVWx5Is7CCd zX-u{Pi3&k?M*2iuZlAv_GoY}I*bDod$RBn;swOlhfwn>cvn#40tf}^Vvzu`H8~IE= z{-Do~Me;(b;gJ}$IL%Rd4X!bmXxfD9aUG`D(G*VOw1zb?m|jD&1V<0WFl=EIh7)~$ zxoJT&vHY`uu;Uj3l6`t3p~{JVRDnNoZlSO%2Wmxa5G8v5 zH%Zqt#+zjX5HpZCbS!)v8H?HL#RUt2G)ZoYb16$|p4xLog&m7$nTNhSstA zQB-3rs6-1xN%}Z2sjUI+o*MbZ-8ok)Re4Y~oTafYhZU~;RGF|FSfiLXjaqNw#Ij(F zCb4EQhM)|%LB|m^r`PIe4Tmu-%Tg4jF^u4oG{tOaoVW(QD^60DtMV>c565qOePIHW z55rwg>DnyO~huh)9ayT z!e5Gyx0m2h3ier7K&_mxR)IX+DS9E&9{&WaeeCT3aqR|TR?JpDCUI8ue@T19mGusK zr~S|9jkvO#U)P~|{6t13kKyrErO4yQ#8kwUs7QO~M`f39()0gTbjMHlHz5zUujC*W zX1!b2BJ0@UonNr-Xls?Z*3f^c{39m#B@Pq`pfnsE%37Egi%R>l&vZ8Kbn2(x+Seat1eXro|pe03^H4vo!aL0oP6Gk zrGY^W-BZ`xvV$9>59LkjkfjrsGS}`SkDTy?J=&|KhIn2fN4f=OKD=nRTn9{n+$-m2 zvMR!NRa+SywC|r!@$hP(kW52?ebJYYq=bJ6yPt;D;jY83G_fj>&V>5Y(E|a^{(C;o z|M~r}C*goJ8BcQ&7H-Xlbv$y)U4Tv~K>Xe6KDl;~mEXAEw|&W>Reuq7cS57buI_e` zLvKxiGQOtSTS)GL>59RwcEQ#Kixr*X;r%X{C*R*8^mV~pMXPuS=my-!b-{V9uH&ST z+zs;*U00g|@#QX7IT`kjtP!E(tvIqrR-9c9&A%#znr@iw@4Ddh*GhJqr|?M8Q~9oY z&bN-oca$KEI$i!NHal0GN~L{DhPW<3_Z=F4cZcdxjd8ah8REMA?5J~-pcG9J3IoC0 z-*xe+bcFF6ahi{UU}fJ|?I#7H2S|nJQ24Y*OzPL-&A)meLfPLbp62!f>Hd4izjAb{ zunv{`3kAJ^D9(4c3w$qV6^&oF3+H+vf^Mvr+AvIF8E=IQ`J{i@U`1To!p$rEww-yqFIc?F(=I)RZmfLc%SO+wABJwIQV#c6B#E5NGrXzwJUR;HOfS`infz+YPw(& zC($A&(Lz5}@2t>rIEsKn>WFZEZFm448m^LSNr@yZFfvlD()b0C9QLs|Wmu^n&4aRX z`dXe3txe(^it@${vGNg?KX!^(w(7_k^jRR2-h5>gU3$r!wkU|UnrfHZ=lboP z7eDzdYAknJnhkQ<^!b7)TAN}c$ZS8;{orfSq@Fjf`q7eVH&o^Yas)%i;BQcFj%HH% z+I++6LXs{j%n*EcpNke&|NTo%{F%_E0uLP>e8rLWGHn!iKTKUQKW}05(!+a#>oZ)Y zprSM|)=+CA_@IlP_nQ*!X#vA%?TsM@_Zxzt#=}0q2O8q<)bNa(m44>1?yPzjxo_AwkT+}%s8nZ4#hiDkiTjs0b7O*Lw0gEKz65Fbr!XQV2X z3PRzBixpBo924UdNvNf`2$SO^C6UO)QZ#oLoG>6@?hU5J(spPjxRiV_Hq!E*=gN~I zV$0lRyg!&q7(vJ|m4pzAyh}=%Neee9%GJ<$heB zW)Bh>TB%b8hp1VwrZ3{^)tUpz&$blbwesk)sm$pIiw-23f#d;lg9e#iK^ybpo{v!?wQp_WEU+Ec~X|WW(yxgbwH`d7~p0(J_-pwVS-?Y3!#m27nCmlr7)YhFE?T4Kt-y*~T$Y5)+Q1@i=h+0bstrtsB}r&|8<@t`mnNaJ zZ2%iJ*o*Wb_hRp$-XGyC{Q5osRQ?Q=MRUJRXW-?0~iHV7tm=vu}B*%N0!E<>fPyGuwfT z8@uBQdeF|a*T3L*0B2rhJhPH?Fgbm5DhlcVGr2{~BE17hxrNLyDi~~nF!7YGeh$`0~j#B001+|1o;2} literal 17 VcmZRcbt>7YGeh$`0~j!01pqU)1m*w$ diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 94f5309bb4367991c809b4092f40f269b63e3085..b924c41c4545cc0b7a71148dea3f4adda7e85de8 100644 GIT binary patch delta 963 zcmaERkZI--rVS<%jAu5RO6=eeSZ^(P-P-*#HvoPh4IeY+(*7b1Q)#B z+^U?&!9TUQdF!er%21Voo9}8@FbOdHsru6T&ZHG8`h2smUIVj$$V$Z+(^`)?P|>ZM zj~brf7bw3krx;Ok)eb7!yt&oUi;+p&aHHY_md%bHc7h7xx>o~^UMQ|-o>vf`Xm?DE zfq}8T{JQ=Whz|b4ljldSkT{sU#K8U=*Xu<|(_h3TS#UuV1$~$tA0;K>DZF(`guy*; zWwXhmzb?BvLM6XUo){&iVCUn`qf*$?y88Gj;aW9=RET7-1jKk|rpX&S)L6=|ZFn>J zZn@NC{^&~PYny#0cSq|3X^Y8wqoovj8S73u9ey6yYIpJ4l|DZUhzTJ#AST%VoP0h| zZL(~PoWiDMyBO~F2-i0xY&(&o9y^S}uGABa)%>tAJ0wxt8fuxpqNf{k6>k5R+IyDFn^x)$Xmk{!}<q#?x3z3y!Uy9zbIbu&DSW*81mdWu^QW6VZulaJ*$w2w%58Dl*#<%UDk}{JgMoB67-I@~M zy2Yd3XKqja{rP(8P)QSr@k~{dH+HD8l=sgunS8fgYBGOxCG%0E1CzU>^?~$;$$O)v z6qLTrPiLCv@+&E`D`ov6k5yn3jv76Im|%Qy^7%lu$+9tW3Tn%aJUck+x75CJ^WV8H zJS7l?4ky40Lr-qo937*iE)jl%dAi5#KYe#v*OWZjJ E03^A?od5s; diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 6c4afcc6a6ad61c14582180b2fe8fb1720614add..0887b9ab994477c9fee020a16b5097e5ca661ae6 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~m1b0st(a1Umo# literal 17 VcmZR6A~{9FsWEOA0~l~t0RSvu1N#5~ diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index f40653aaed9f8ad175609d364c8657331ace08ed..698d4fe876a00fd421642cd8f431c66f38d5e4f5 100644 GIT binary patch delta 312 zcmdmbit+ww#tkMCjQ2O2N(3+p^m&MVf36w%iUAB#D>v6l3NQ*R+R=Yxd+NvYP|>}c zTctc$1#a)-{TzS4ZUaspQwx$?~bXMxv32r&vla{;Ud80RY&GS1tem delta 56 zcmV-80LTB|%K^2@0kAX}0ot=Q7*GMTof!xLv%MNX1+x_(Q311lAqNVxohyq00fDh0 O_!qG-m;}%@kw+ diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 79412b2f0219a0a8f9795dfd50bd7b04deca871e..ca6903e21b529945937fe2623a1ce7041e783ead 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~oNX0{}E;1RMYW literal 17 VcmZSnIL9>cl}9%Z0~oLh0{}Eq1PA~C diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4a17d4d..e649283 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,6 +8,11 @@ import com.pathplanner.lib.auto.PIDConstants; import com.revrobotics.CANSparkMax.IdleMode; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -64,7 +69,26 @@ public static class Auton { public static final String[] paths = {"rotateInPlace, moveForward, PathPlannerTest"}; } + public static class VisionConstants { + public static final Transform3d robotToCam = + new Transform3d( + new Translation3d(0.5, 0.0, 0.5), + new Rotation3d( + 0, 0, + 0)); // Cam mounted facing forward, half a meter forward of center, half a meter up + // from center. + public static final String cameraName = "OV5647"; + public static final double maxDistanceAway = 2.0; + public static final double forwardKP = 0.1; + public static final double forwardToAngleRatio = 0.5; + + public static final double CAMERA_HEIGHT_METERS = 0.72; + public static final double TARGET_HEIGHT_METERS = 0; + public static final double CAMERA_PITCH_RADIANS = 0; + } - // Josh was here - // Hi Josh! + public static class FieldConstants { + public static final double length = Units.feetToMeters(54); + public static final double width = Units.feetToMeters(27); + } } diff --git a/src/main/java/frc/robot/subsystems/PhotonSubsystem.java b/src/main/java/frc/robot/subsystems/PhotonSubsystem.java new file mode 100644 index 0000000..dbdb6cb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PhotonSubsystem.java @@ -0,0 +1,107 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.geometry.Pose2d; + +import java.util.Optional; +import java.lang.Math; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonTrackedTarget; + +import frc.robot.Constants; + +public class PhotonSubsystem extends SubsystemBase { + /** Creates a new PhotonSubsystem. */ + public static PhotonCamera photonCamera; + public static PhotonPoseEstimator photonPoseEstimator; + private int cnt = 0; + private double[][] cameraInfo = new double[9][4]; // 1 indexed + + public PhotonSubsystem() { + // photonCameraWrapper(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + updatePose(); + } + + public PhotonCamera getCamera() { + return photonCamera; + } + + public double getYaw() { + var result = photonCamera.getLatestResult(); + if(result.hasTargets()) { + return result.getBestTarget().getYaw(); + } + else{ + return 0; + } + } + + public double getDistance() { + var result = photonCamera.getLatestResult(); + if(result.hasTargets()) { + return PhotonUtils.calculateDistanceToTargetMeters( + Constants.VisionConstants.CAMERA_HEIGHT_METERS, + Constants.VisionConstants.TARGET_HEIGHT_METERS, + Constants.VisionConstants.CAMERA_PITCH_RADIANS, + Math.toRadians(result.getBestTarget().getPitch())); + } + else{ + return 0; + } + } + + public void updatePose() { + var result = photonCamera.getLatestResult(); + if(result.hasTargets()) { + for (PhotonTrackedTarget target : result.getTargets()) { + cameraInfo[target.getFiducialId()][0] = target.getBestCameraToTarget().getX(); + cameraInfo[target.getFiducialId()][1] = target.getBestCameraToTarget().getY(); + cameraInfo[target.getFiducialId()][2] = target.getBestCameraToTarget().getZ(); + cameraInfo[target.getFiducialId()][3] = target.getYaw(); + + if((cnt++%10) == 0) { + System.out.print("FID: "); + System.out.print(target.getFiducialId()); + System.out.print(", "); + System.out.print("X: "); + System.out.print(cameraInfo[target.getFiducialId()][0]); + System.out.print(", "); + System.out.print("Y: "); + System.out.print(cameraInfo[target.getFiducialId()][1]); + System.out.print(", "); + System.out.print("Z: "); + System.out.print(cameraInfo[target.getFiducialId()][2]); + System.out.print(", "); + System.out.print("Yaw: "); + System.out.print(cameraInfo[target.getFiducialId()][3]); + System.out.println(""); + } + } + + // System.out.println(result.getTargets().get(0).getrFiducialId()); + } + } + + /** + * @param estimatedRobotPose The current best guess at robot pose + * @return A pair of the fused camera observations to a single Pose2d on the field, and the time + * of the observation. Assumes a planar field and the robot is always firmly on the ground + */ + public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { + photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + return photonPoseEstimator.update(); + } +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..dad3105 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file From 96c2a0be5e993a6fb24343c40295c8a692621f92 Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Wed, 12 Apr 2023 23:12:47 -0500 Subject: [PATCH 03/31] Fixed issue with Null Pointers --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27447 -> 27797 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 36155 -> 36911 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 41497 -> 43347 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 26079 -> 27167 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 23285 -> 23321 bytes networktables.json | 1 + simgui-ds.json | 97 +++++++++++++++++ simgui.json | 80 ++++++++++++++ .../java/frc/robot/Autons/MoveForward.java | 86 +++++++-------- .../java/frc/robot/Autons/RotateInPlace.java | 86 +++++++-------- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 57 ---------- .../frc/robot/commands/AutonCommands.java | 6 +- .../java/frc/robot/commands/AutonLoader.java | 23 +--- .../java/frc/robot/commands/TeleopDrive.java | 21 ++-- .../java/frc/robot/subsystems/DriveBase.java | 8 +- .../java/frc/robot/subsystems/Kinematics.java | 6 +- .../Module.java | 4 +- .../ModuleGroup.java | 2 +- .../ModuleState.java | 2 +- .../frc/robot/subsystems/RobotContainer.java | 103 ++++++++++++++++++ .../java/frc/robot/subsystems/Systems.java | 95 ++++++++-------- 27 files changed, 441 insertions(+), 237 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json delete mode 100644 src/main/java/frc/robot/RobotContainer.java rename src/main/java/frc/robot/{ModuleUtilities => subsystems}/Module.java (99%) rename src/main/java/frc/robot/{ModuleUtilities => subsystems}/ModuleGroup.java (94%) rename src/main/java/frc/robot/{ModuleUtilities => subsystems}/ModuleState.java (89%) create mode 100644 src/main/java/frc/robot/subsystems/RobotContainer.java diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 37f44e6fd9c98d20948834233af40068c9f64819..a1993477eff8c3a502da195afd77b22cbcf253ad 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~j!`1pqNS1WNz_ literal 17 VcmZQ>zc8b1qOQ9v0~jz*001!_1TFvo diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index e5515821b2a67e0c1037dcc43ae7e4c6a7a0faea..3da1217fea652f4ca0f1cf0389e6a42c8619a9be 100644 GIT binary patch delta 252 zcmdmfjdAKt#tkMCjJr0QN;IAX zarNyApA`jAvCPeT6)Tze17H6CBJb7+6`Z|USLGH9lVIaU#UJ7u4Q8-ycJ%nBH2G$x z5=+Gf*&~zxXX*hNa*rk(W`P*;lO`tv8H+V-o=u(#VLYFF9L%s?JNajpp1@*FV?&79 JzJfqn4*)DWQ@j8G delta 63 zcmV-F0Kos1*#WoK0kAX}0kg9;7=Z+{&Ky_*lRh8UlPw^Slhq)3lV%~!NB_cWaEdu}cp44+<0E6Db&8-TIO#J1x(wm(pl|u!sC)X+k z2;BYr+566_yldl35PWkZTCcD@ZsEJ!Q*Q&l`U#g*IDo&oN@sPj5oHK5Vi4Rm|{pMWFg^c`+t8Z8MtSEp8_TJshs(q1#Kk()M zFY<1UP?esW&+0#57D&0stGwvJkKa%;!#6KAu3!=<|DyW-JoDTuP|@DWqGp$v6gW33 zatLg8^!Vj6xwcDS$>@cHD|-9|u$?b^wTJs@Sqh7e`Gf|C~p3r}9rBhFK? vLD3H)Xcjv;zDEmah0?Ogy**Ns@AiP~Q)8U`v&TrFVuQK`M43)7kTwDUv;?SS delta 92 zcmV-i0Hgn}paQ#^09Ncu!gL?f>N diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 5684331ea76dfe58dc9845d430ccadd1841fe513..6475e5e34a778939ce316e410f844fc3687a8a6a 100644 GIT binary patch delta 19256 zcmeHO30PA{*Jg4Fi+~1M6%di4fF$fz*^$K^sc6+UE&+lDNrFjOidtM+t5z*|mBAfc z&=xm}Rj~?+yQp9l)Cvk}wSrnvTU*zEW^RBO6;q_&|2_X-pU33n&O7g%nK@_X+?m6s zMMj$z8D*FP-Aub-7Fs*(w6q4>YH4YSv|8k(f?3%XnHhR?7fUOkLwj3}qF7!<^digd zAAG#7s`E3e&8PzY_ax}0+CTI#7uPPxn2#8S7X}a1`Mx(u)zSi~D^p@WlX<4dh0$q~ zJQ5RS;y4dad5lNmRG}9k)6&w`Iu3)@u)5gEMdG9cfkGse@CFN#1!1WQp-du3h-!XJ z=MJ<38iN}L_AvgYg)>}~AO!ay-Ip5cjh^yl!UUl}E}RgLn&QRFr4lVpM8%yUItNRh zJu0|y>1?gTn$l(8AARihoUIK|PrfV!@gK6je=%~8lW)0}mM?4NA5#qgL>A2bM8tg^2E6%kYQ=iplCcc0Xbs zIN@8+1r9f0nSd)qqkuPQB#cesA$53hBAHyllPF@OG9jP&Qm5T}W8|1gonpcnLXn9n?!SGQXt!%!t@$wmu~3F4O_nT_sS^e7Cq%ghz=EE6QWyG3 zfC;E1jD6qT&B=lUKKidlN?$pNhu^Kt47wE~I*msSjh{rQz_W_J)JYFs$8#Z_1}tHX z5t<8TQiI;DB%(`Fh!QX@{#NIeoWi_0Qs4q^v@(*M)8>|ZL845Qpb$(_=Lk5BJA81Z z30+;L4d0>BD+R5!&gie$zl@) zGR0^7I9Uv~X1+90D2Yi{eld-D@%r%GxL$lZbF&3q9BC48R)dD5vPpblyf}e;aFHJ; zCJ6a%JTK%(c0EC_(Ty5##7M>bIDs6kE~$tpUJx?{c|V_+x$*}$R-h0>izKmpy3UTb|UoD0lIZY>&+cP6ZR(4M5D+{L`zL#f%6Mq`;zzdp9x94{#kSBq zU;U{UvQO8;z!TKGT|HV*Z(`0L5%%$`4DLKnKD|8(Yd3YgC4E221XK_b@3$!zqmT)C ziIb&5i6|A>yNFT8o9Btl1^ILXrpi5U2B8V+Xcggl<3fQu`U)2^bags7t_RCe8@m9Z z9Cj2KgUfI4Bg{MsOw2I6nFy`;ZajBn#s^GUSTozIF7PR)xkhN-xKbl$z#UXbCCMT= z+BW%-$>!kjb6|Jy4l%E||p?HCWu$Va-hb3EkK&Y{r#X~;lYS)6FiLLvBU3$Xl8 zgh6dtztlq@Q@YbF<9{j)q6O1k+CK{FMxWvv{}YiAG)_m`%ln2x_VmQt|5PN@owlhr ze&1-wseK|L6FQ-xeFGsAxT*(=t==~fvY<=uw0j)HZLbiB-geWtwM9m=2 z4)qAg3`mQng!+!0)_Jbz}eu6Wu0DIP9hA!+KbR(n+fkHS)5}6=~5h@9@f4*ots~BC|o=YPN+puq;de> zO*j&r_em2&WuoM!0T|)g=iatImS}Cc`*dsmu>oD+OGF6%!bgNrmTyxKD(Zj_c+@(G zmdtSjzY~nt+lWTs=~i~_^Wy2-`*pa#%CjKXZPxSBkucZ-n7^}Y@}y|_XXqG1Ea#JM z^+zXTN0_~Q!9b^Sr7froI}2=Mg{-g z@FUOL?}rEwSP8zxU4?b1^qmWh${0p5R;27*;EFUy?zU~VXO7+T?VnjxLcff)fQxPc zLl&qdCf)6oE^*xM>NuvkP8FQp-F39~3;$9$%MV3e^%y$VC?-K5mkZ_0DhI!w53Hc` zZD5{my`t9Z@($^f8LqLymj)q=vWgxZdF=K*3oIcEpwjV4Z7t5gtB=FKyx7t3g=^ot zek)ub-7EMDf&c0bmZ&$fj9AWzXZV`E( zEFNU0bLoKdrQYAbBe#K*_C?e-d~h51YFA>(y&m+@uEx?7BE!6`H8P8X$ zJLZq)U8^%Ue4cb=>LI%aqb*(%b&qv~xp#nVdcm)y-^B0L zDh{e-8WtIL_|eiE6MsBLbGGdIcE)~!t+EA+ly0ZQr`Vs*T(BgQ^3$=nH$3Au0ao*v}nm+Ri84dKC(2T(=sjmuC>_$at4|p))FPkps zyG$%5;GNn~`z~W&MUEps$aH6I!O z;=d=X{O5;*%A}(L)}cp(##(GyNcYaa_1(|GyK-#@qDOwL!M);~2S(X*5AJgbD)&W? zpu5D}F_x|i%bsYVV}6-Wcbn(LI!^p*Oz%lTbWDkyIF>J$Cdp!uQKExOyf31Yvq+f~ zJ&;RfN<*!!s&{Ptx%#ixm&k<|1INxy*hqT3V+Oun^6f<1EpTX^p*yGa{LJ&)M}{bpE(Mnx zmIo}_QCf-|Ve2eFD5_!O128gcKT8*WK9r@u`%pUTs^RX$mmt#HX;_4oR@nJ>URhDy z3qRWN5Ev~=L5JVc6gk)3!`p*?zVP=~pTOWy(4h8cTPAR0`7#%5*2Er~+@6FvD8d2Y z3-n30&asYfL@+TB01FV#mBd5O$?(6KPZ=>~(ZGgvmYL+Ct3*$QkIK$KanyGP z+IFdhM$L^M0_-$0D3uVPC4f}AgZj>c;=P`%3Z@T3TI+)fsrnv8an$!HCUU}Yqyf!2 z!e*Uf8VJ*{0o9F-;;3(IIB-GXq%PwEnye5iy1%v;Qm<`=U{h0IX~YDUMC&%Sqq2)q z9Q9pPr5zPqB{NjLG#n~A?MPIU`D-ESf z+a+tlr+pJ9XnCdMdtVduZ}(IQn!4cD+G3;G(SEKIO1kJe8o0FKI9>gG-vX`}4SKb~ z|JBd;>57YvZP4e{<9(Fo5YPr))KeV%+Q9M3@jl6Jm_8P?<^ELtjNf$0ddK%W=D)oO z^aa%>JQ{|WhQFCo9yYswp?d#Q86T_MSr8o3?6UQ6+ zMQ7BQviV*(-|GGYOo4vW+=qJfy&5-go~ZN2l&d#THEAeJ6~-X7P%_x+hnR8A6@TSf zqy1}<@N4Hmt)$Ot60H84aq_p)aR1FXc`e)fzb@mX-NI6I3*LrhTshMUOs=MDPFjIzSKK0LBYse7HIeomZ(nq$S&Z(Y}j;A?U9r zcmNW z+rIBOqb&iV_igApTGopzs^&3f3?Ve_?>shh@=gAK%I(U!KzyEOEP(aEk zREC8d6N+=9!f$$qQPT>dE@qCMu&2_6AaXzhev@{Afov)r-PT+*9NafCqkr*>U#3*0 z0izMB#VYc2i^SMyic?whBqsI8suA7?AL{)f2pO%4&_;-huuhj+YRMxSny;_@&BmhS zD}kwt^R?SPmjF}B#-&X_!kRwE@kH#U-n%}$e?0E&iAql;P@9KJVXZE;3U#KwT3<(e z*Lb}ROuD&qU+!W+3h>8rb3(H9dF)^JzpknnHL205fBFJv0$7ffs)19DDK4C>PbH!b z-zvKn{@eC(TZ&AsPn{F~)Eym^HW#bNVy43mA8*b5Fyg_*)T&i`yYcqMsvtGdIH68d&3>-CUhm9zl^0cuRpcp*-Y4VpCzx(5*=QV*UB9hQ zMORe>)|*^X5$Fja+opb$mA3R?8XWC-snnwlm%7Zl>NhROF@pe&N z72X>T|8#ZW)FCCWVNk=y911E;;(S?}_2kZtgCeR2(O{qbRb?qLi6$VK%^F>nr{NE~ zO|+c_As!pp50pU0Bsd&uEi#GEJ`n$MvQKewjtLMPm9Oyu5EsA$Olg7Q(V!cH%;P$4 z(F)T!B2EFM045q!8&KUlQ9CxBi*~*sUDD%u^0G7!Knn0jE@MuOORvu?spg(6t-U+R zvfJe(0?4J1axl|`TF>|(_@Q8A`Bc*l#p45NM~|0W2BeJmAOhu~cJ_73nejBcs{7Yj z&*v7N%-Km0dFbpJ83ly3rj!91`EofduTtpcr07bub52EVP%T!G^_Zy*w=Io-8eKPi z-@|U3r(N%?rHUZ>?J%RhMNMa2xPRa0&=A9U#WA)Mg2kT#hzsyX9@9mkrT*Nk$U=!V z_q?LZ$odij$ivog9Tu2T*>JxFQLkg8)f9Kb;q@O)={4$tZDh=Hlir%@< zl210@_)6T%!(odm0&_E6x1EIE&Aj@!F)PKV=aR-|486GE*tA*H#f(x{WtR+9K*|yO zxeHNAczXKdo!g&{k9#~xI^gi2{H8I$#dr>~;o`1TM>Gfn_T^=M`b=VVe#k&s&SX6y z8Goj<8E3!@hse?P$)fZx2GE(_Su?odQ(BD#meNPISX7t2wCcfHUC{y;JDz?+jcT!qti#yiH(qu#16Gy) za^v$Sf7 z&dHm1_RDoUr}Gn*2O{@wF2#PD4Xx}ceTKHiZQG>63Rhn4+J>Ur3r?OO&dLA?Z4=xO z(bn(w?YL^e4X+71VZPq7o1I&*hPZ)^3}f5|uO&f>?jf^(`qTf@n|`B6;~SfF+>Ce{ z=dUTTGM~PEWKl!Fe0#rz1hJWDW}7UE6-t|}ySAkAaP5}-UFng>&gl0^M_U)x$50id zRt=dO#YwKSH~Xc}=QEwM`rFM)HKWz5VVfkTzNIHIp2~nf4zIqQ>2l;;V4;8Ly^TtX zz@^PNrSUxSiaPm)^mOumGdRq#de5^KC}ZkqS9OKs9jSO`F!bi#{V?lLxg?CMD_U(4 zokzA-20%un2`)y%^2M-OtMtbw^qXxDCe$whWi41S+8a=Gz{A8Mt9Xfs@qO{qx?;E0XaUT%g!>qV|PN(YWp%pA26c{#r$Zs==EN7sfl2t)E^i zDSbZo!~y$j5oe5Vte2t9yt!CKR&M=A=4c_RT0?5YMIJuE=^m% ze{!be0`uLdBqsEEYOd=(Ztx}7s@N1l!|E_1odw!0ltEe5|b;jEblH61~{ zjtv^Sc@VJ3Ub?#w!LF`g&2*5T%XIW4GHWdaJF==cGLvedi!q z5tvd2_lUvWu|I0aE%qw6*>LYr{5Wx6B@UM|N<4qT`fT_97pzZw*t09pTLiT3M}5HF zMZ~avlq0*6h_ZfEFLoml*Yu<8+11A|_Gmw<2m1*T@Ajj5vNMk3ru;VOweBTVLfUc8R7LfL#OxZ{9Q@F*4a$)cN35zv86qlV(#7@4H zE4zk>A-)vP;4T*3W>)JW-ikz)Da`Oi3}*=7TVJX-`ynB_d#6dp729`KS{*j{ZQYp9mQB1 zf7JKcr0=2r2)mq!Qh(ImOl8r#2Y62T-Q4 za{yAIjug8DP&{@65hnzo-poIN8~X-}*+eV}pxh09z@ppCE$(1B3^qp{;{+mx0|ek4 zh_F8s@e?dwC*p!Y%9*_ozbC>X^t3|`d1ahgn@7R!K}ZEw5F#id<<>!H@DC9&B8YM| z_?dxj9tN`EHfzcPt_?!HxkdoHF!mV{e+@#K&nv?%SOueY^N2Vk7}?MsBBmnIpcIR4 zGi$>U+1>7ltTdQ1(I(}Wg3(OhBBD`$M38w3bNOR&9TCO-5#Rt3bFp}Vh?N+ScN$~a zAxJDFqIU>V>?9EF((X4JKVnQh8 zz%C|YRw#0hDk7GIB4a2zjH}%WMPwI=Xc~s99U&$X7={3|&tSl$FvMFx#KjnJnutYV mh_{A_*D>~sa*QSKx1r3JMlrm zg56jn_6GiJ*b+^!MI(rc4Z9*L#&>s*J09+0awo*sKl}{4v-_QyZL>4`+?sigYvws- zG;_4I!v9`{T3PwKX1eyXCxpALtRi2-((ym1T1=PgY3BvJmg=S3jgajp}o-|QFsT0*0 z(aI`wr*9<4WbJhG1ufinwsZxOd>dJym6a?|8@R=nu#utq3>M*4rUeyztxuVgHmP(t zy=4z@0s5jCI>MfC(K;0`quWITCrE^8)2|Ju*F{67hqxWL{Cj%de)wkz{*!aMsrGI8 zm-M%exLp{1HG;6wrk)%^mpT$ohK`-+&_ZCX9d=^~y|@s#(1Z;TXgR0j>AOt{r-J0z zI6|9$yO0iahP{ltvw>C~hD-z5U<>$_9^eGT^aDS_LF;|5AHDks#6HI;EV?&{?%9lR z0e=_@l?z;9Kjit9^oe7@DXU@=J*ODB?1Z3EO#KUzN>glhcHx0?P>_GS+&ZhGP@DNg zKrizrrtS0;0e2R`si4V@AcB5*6}UUIINvz$4$r2n`yM{5o}1%5k44|V3jDQxl&6;a z%uDO7@X#*W;p#DloItWTN3YynJjpt$lCuf?aXIduJ6NgB*x}7am3MWW#<~K#tX)m% zh%#Wq0l5do+L-=VGEa4IpBdh11?}AfxWZbk7v-hzoklHapJ2kF79%%nQhZL&W?ggE zTMeGW-}*IJN&EJImP0Hi4YfmeJ1i&%)=-bD6(9(dIA1CJUfEmgw^K-$UIVS^5-Au( zCzpeE^avS920AIVJ&W9H(GG9vuQ`;b=}wwnu9YeR~Bu|^NEM%wD47PY+1 zCY{annZKj=$wxlTJf+`{2kW&DirO%FdltEQ4N4#rs6d^)Laq(|>2#Ge(l2>`++f?W zM_`MPA|LxYmP-iZOKqXzBhK3XGur0gNT1o|>)g_oX~PTXFn8cakLn00PSwHUo9BYO zTW0+AX;izhUbpB`Gl4gAi0Cp8V6XK#+@w}g>tmTAE_06ix-J$Uee8L)2^=RtxgrK_ zlCsY9s1+S?6MPKK*XK~ht26r^UoXk9P2RP3)f8>mVRu{y9?+Scz)-rh7r~*M-v(jy zvyNaG{mUKT0Gp+B?cL+-AlsZb7rO23+bgh{_N~$#)j5L0NVTp`91ok-Coit2h&wfv zb9-+ba)nL_20w^rhK*0q#KeQFewqnXcTL&%M}BnWUErIP@nH3#OYOV&7#dM@rqi1N-`&6s&RK&jEJj}qsiflc@I+?W zk;fY%js`9GRAD!DmS5h=KVk2ctwGk8R1~F&?HZ?^K%ob(n`|}8UOiW>1q*0RXTp_UcMmvQ|FHVc19ag%(8)R(QGeY7 zV%rQzz2KHv(-ED4A3dTHaBQEgLPb05w49l(>0DT1{Ro!On=3(}bq%7fRRW1^DsDK_ zj!y9eE*>F$dpNWj_rr|!$4--b)&;M!?6^yALJE4uO;GS+-stBigc<@hM-s|0nF zIzdgvgL%3Fy=rL#hQ2vp*gRw4TKV2ri>eRp!lJxwhsF#w>=CCKAFF3h+&ngF)2>O` zZ?->II&b~!(#7itvJMmbK z#9?Q3#q3G@AA!_?8o4Gx-z_iu@CY*QzIySRkR0dqV7py-D*W7(9u-JFa#NEt z_omb0H}}Wy6#RS&*WR?DbM6E0?n}-O-P3Vx-1T*5I&5CFb=-|DSfm0+kl9BaLq(Gb zYDK&rhkuXr`>zhFsNczF{i_%B8Pnza!EUpQmsEPeqRGO3+XJWM zEG*sP@Zor=R zS#5WQb^*Dd-8?Y$_~(aijhGQ~_IycNf9b__NV1nn)&QQ3ojyq)>0UB+VO!ga3D46sC_(ai|IKiaE(IRE6JT@{uHdZrkQmkZjLbUFjXCIsJ~xkw_DNX0=3ab!%qN)s1NDT5Z| z6V2#*HK4VBBpDU2jvhIByjrIOi?e{2CHNuDuYKi|-+mOV*w`I3q<<&l{$F781t^qC zkwhp{DFj@uh$j;Wgi@(Y#8Z)SzDOnGE9*&1FUu!9vAEeQ3Abh}&XCX!-6npuWAE>O z2os}wrzk+fdj+?!U_nZ`P%h;1xB`KclJW%5mNJo4AR{S>Od(V#h4loBBx&CoAo1x* zMvaLv7;su2QvUL5uYG%`kH|_9d%S5#*N)7tZ@j;VEnz?U0p9ZanjT;HoSTs-ds_W$ zKwCa&K)+24{ocXC3HhX4L`r!o1y3ncO8F!ojxWgro?G zxGk626S&3YC7k5q-xWwLnA&;BGU(!=E$@8+I7R@vW%Rzi`RIDnPlJFnp>0!D z_jZijJ8}Q3DPh-YSJM0T?#S#Nt1W4d=BZRc%J>R}g2&~Fq;jc9F6QyM5(!DE>RXlE z4tGs#RYkm!RUaSq>Da4m&&H8=?~HA~t=lP`Ro}CFj#2m6rtjI8kFGDhMazd4JlZJi zGOG08>gHEof9|TY=zI34TAzpo_4xSOX!vALJ%{KnwW4^651gd4i)73ddQ>fsJ+gGMex7qex-!acF zpPZC8)r1E;!>d*2PYlT=TwtRk;}=YGj#WPGciFDO&vX4W6CRoe9R$>55|rq>=L4?U z{&g(v;Oohv5y!G7gSuLs@%K-|Lvwb~+3Wo8@{1lNTlb9UTyVK|Xi%+A!=&kWs8ue- zOLlFZ{^*YH=?>Mylhw5vqFTgP03jFKzqk@QAnWz*5@pmE5!ES}rJ$aecNSq)I;FfE z<0)aUn6!3rka+*DohCf`f}m=U1XX{^@t$zJ>ydS>KYKF!&8KH=nbq{;8N|;J{iTm; z&M%9DzAepM`F&5%Vh`xgzzU=*s9TR-lD5AU{14WmirS5aUO$tFgD{0J zwk5Pr7<=#n88+?8;j_8S>e~Giu}0BnW)XIfpX<4+U}1+c&t*@ie3KvfIKqUd_B^9~ zW)lgJ|M{6h(A)cqbyI#BQL*R7r8Ugz+T#SV#IMxNaUSi9h~bnw(>EnG7DRFG%otu|wq zZv(P~n08Agra+d3Zp-72-#iiYk=^V8tCl^=z;@Er>L$>|R02(}`G-4p=`iI`mD5s% zyxi~0;M(bhYW0JYGmk)HH{rq9YBHENGk#?E^T9n@ZDdx1$GT}gB8~VCvQ{2>`^(EC zL8aprr$EV~k0vmy^-IJzotO++bJ|@0X~mj_E-z25;YpRb7U-UWV`=ERIQ=t{8t3}i{$kO{fr+@cRM@}ZlcDdZx;It64Y}12BWn$SXlqJKb zoqhJmD^=2uFUTIwtk&;OL0LpHld*i|HV9j9u>rc(%=Gp71cW9PJC&YrK4 zv&S)ZCL8Acc`^MnjnBntpVA;?hZb(J#{kWBt5r6&U6W#Ml z+kH7Rz<&h~1NW8kH+x~5%^9Fa01?LDYQPC35r?DSSkc%H& zd*rSm1Khopm~iyX&`T3v<$qQ=_RTs1)X5U_fefIN<&QcWaN)R1q;N({xpGQ{LOOhj zTqTeRr4p`KA?DUw$h8rj_n8IwP()T3|81W|_w6o>dFb7F$Do7O%9xFyO4@$t5C@iX??bF%%S;3N>R}sC5O@|dFxk~u6d$aph5Utv(2oV zZ%iL{$wHV(VWtsSO8*6~fVlZ-h#n34cNtDntgS$FUq?Ah}3cdtJ=eabf0zSRWm@}GKDn>D3B#MB- zQw!z4Se%>PR<~%azSqQmpzDdBJomZdM<12|i{A^l)I#-onva4WBtVZ2pUXXKzU(NQ zX49m1yG{oNb{midEX5VQ5AJ_Bb~xIpAG-@~`zL2R{5f&^koB#mA3EII;<2+pI|eX( zo1;5g&6}AX?=m5s@+0A)4Xw6;(UW8Jlwm)8vP_WTx7_Z}B8MW-=pc>Oq0=ki@riy2 zPPT7(!7b_PTy2lJCE`9+O&U*b1Y@Ivf$y3c$Kbpl2v6;jDrE5f3%CFLm22zKue=8I zRJq680F4d;u3EUpd%SC+1l8RmYqzjybnnn@VCQ`EgnQ@gZ#(=#Qu`KbQUWZ76}$(m zg~sY*K7=}YbZnePVNlP1R7B7Zq3Pj8F*~g%+oh3~j+5+tp>J9^di~7@ice4{C?eG{ zqYYYKU--Dy?sA8bo72yyPP){_4Ooia@%_;)+!!o4XjM^UOpHOtwYRb^+;{$=`qNu; zxfQEc47GHtkeII`6S%^{as1Z^hxWnq<}fC{Eo_rO^l{m&0q4}ymYo^)3;0ifEL>>+ zc-FYLP-_gML8Ru%C+xSG>byI6Pj=PJQcKs>KO@;0kQz;am?yx~aO{9Z>tAgg;To`{ z&n26CUegO2PkIESKXxoT=Pg(mWZZZ~-B3Iw#syq&Qy}?uRSa8U6|?0(To_DXOShPJ zkNDmhL<>*-d30^k&dyst&rNjxNbb}6b)KiiTiHM3e5Av%RQP{i)#)!h;i|qDvHkF; z2h@3$BY6c+1@3JvmfkEN#y!egKq=*1QZ5m4xm=}Cpx{$-o>(Fjid71dORDGruYsRA z*ESfyilGwV`et#mR-THp{i<+p);(AG`heg3EQK@YqGo`ryL_9a%DYFma2DZ;Bgfpx zKGQeN%zIYSW{%TuC!Vch+^-t}f{51`Vo~05u0){{%4B?=R7}Bld;%$tD-pvxC8^{p z6@0qOY{K8100Fedc=)Yk>Qpo|VAe%(H!3)wz&@?byp8BX2>Mp)Ap%N zi0XqHGCV$J0hlvJdH_5~bH5!vJNI}G8+Jf$+d0Z7yXsO(<^h^BNDM%O*XSjZQvwun zqj-Gy%1%_F@j`FJTXiWkTS85q<8|TpM=j7$mJ3hUm}soNKz@Ft5b;<@7~HD z&m-LFrP;(sct2akO7x8?$c-5=^!CmC{NioQ$<|mvF!#kc5~Wh5L`4auDw&kz@}w}= z0rlZh3Z6`fUbL#{UGoUY1jxDQ0Q`lckV5R z--wARisA_=r9!OWlS-+QE0OU;0-hYE^61cX!i|1e4L=`x*Mg^arx7i%;F{vyPbSCA zdT7<-fGF|vR>MH!<3Qn=aW0;aBvneeLc*hze4bb=;fkbU_%@si)2JkjQHG@xKJONM zN;=UT3-12)^7(7hMx?dhKek73NS`n3g|r%u0foL{u(*$kf^Y66GWc#CJ|pFlVmV(z z$;1i@riA5um^`D248rf-LWg7!EwRuFhfFv6uj5Bw_jXS2v28|M(6|`&g%+r|G8I>< zR8le}36Pq~2;i zY?$(=)dk5*-!56#WF1o%v!JE#nw^X&&fh^ZXDCl5R8W)z+q6Di&0NVR@NcsuU{(3a(Hg;!!F}qEtyl zeE8HwDx#D;u8Jy|YZ%6JiWoL}PQgq9Tl|V4#EkV}ct~LKh@tMj{~j+3%(>Mt zUly1=EMTHroPah5otbJk$@DYLZZWz!W;dk2O;YbnU(=j?xCO~1Mn2pm9nQEDnU#j` zZpPfI&0*E$sX4}JYD<;10F{x)tW)9T`_^vsQsQ*N<9c<6I!eI2CB& zjIrRM2s2B@G0x60yhgKB{#Vx*rOlj7vStAPDQfc z7V5tnDrVk>W1KItbauYmh$xZcQM;)a=W#4W`S%F{GcL|1375Jf)~r~+IftqtRbi4C zsq-ex0(-aNQ0~G7x{b!E3xn5aR-D|Ni%Y$@KgT%HQ0Fn51=z)mUAZn1z%kAPFq{Lk zV%p~Drw=jLdcMYidc(EQyr8x@3hEJ&+ix61H>_xLLeA!#aRci|xjDu`ZiY!4D>RJm zn#Lh$#$s-SI4(jk4iz)QS&b9eMJUE`U8bYh@aQN)Fb=>rZYUEW7)MJP2xbKs&3R{_ z55O^Fn;hfNCUc0)2>hAjS?F;Yfj^UgBg3<3geV=dsBs|B5WZ>DP#Quqj@mJFX@s~J zIzfy>Yjo^e@F^(P9^BTMzVsG2B&EYoj~H8)mC2q}TytR)tvJRz z>a!P!eOHb*msc8)(sK#KH5ns{DWB_Q$_&2l6%4C|&PeW~HZMOkA zj3&$lVP&!+en|VDEt0z04lKfZkj&8El-m{n< zw3UeqoqPkf(I!3TS(hVX+w(y7mUD=@^;~3Kh6takK=v|B4XO%Y@4JY~W>oR5FC$`a z6`x&+yLeK?WzYH(m9>2lz|OdUs39-fvoo$BYW530yW}RKie3b=i*ehhsH_x~wW|(f z&&OrMtGVo=Yp5)%x*hv4S|!?p)y zXZ~CkNQ0vTnQK2>voel$deuBCf~?i4OBVw?lC`{j^j$OKktiJbY&DdXTl_lP9S_f*$jhaM-#3 zFJfp%_NMcd%c|%WGFB`0F-&%ou|8!NV=_+0YRmo|lS^bQe|B*R;yo&3`LRndd0)nA z!#?8tJqzlgfvCO>k8TMXRrKB2Q=Df=?+ctb~4 z2lh=&{@IZgz`l;jk2|q~*tanm-iZ~+zJtjLh`fc#b)8u4*`=7g)QQ!>{wgB5$v4>O zN9}|xSGsjD%h|46SDMPE#|5)o=t03OS9aPNq(pQugw4X_l3=LB^T z7%IFPtJbnJRQLcUyLN`W2QjJX3>7|Ft8h91^J(A=F$y|E)vjQU6P;P@?XK%M%ED+~ z2>hKV=MX5u16*irlwzHV*69y;OZFp*vlL`nrr;IerfND#c`HL|(&W^yhH8+`wc$B5z^x9AdbGNp?6SA7D}x4t0Kv z$fyss8P%zz8KZ?i;cxpWjha;bj=YwZgNG`!-SXVd+mSQrhE2}ko4JH?Kg-d8X zCJVd5x%12KXbf+4g`NG1$!6VPvz2&|yL5vLf9YiC2K!CJXlaNc1CzVE!G5zac^&a) eW757mBv)Wk&>hayRhS&v9X4Bw$vNFwE&mT_%8$?h diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 28ab1f150badf4a0192978cc7cec9a4838c8a21e..eded620c37b4323456ee9b9ac10905b60d51c11b 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oLf0RS@f1S9|e literal 17 VcmZRcbt>7YGeh$`0~j#B001+|1o;2} diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index b924c41c4545cc0b7a71148dea3f4adda7e85de8..4eb8a908f872fcd1efa4e772d2b7d27829d4eaff 100644 GIT binary patch delta 3344 zcmZWr3tUXw9-f(oI!QB`K~0kyDMimy4s|;vROo~}OE(G;DdHrdlc?Lvqx|(!&NNL= z>YDDpt{V+bohXEKDu{8h9Fw$|L&`0A*UmGn_a~;MwcfuMxNXe~!sj(Y zq57vd9T%30c_pS)7sVa3xDUa4Ey5P^IEU99}}=gNCMTAf#LXZ>IF~i+yKC z$>Q9~nOG-i)i>audFFODrqi^Pu~$K#{y~1pCI1-p*DlitnfrtaHt=HesloQeMX#d^ z30sr|N=Yb3Y%8W=6Md<7~ZH-CuF`V zWn^5;;k9nxH}LfPC4?*ppsG!FviYytlloVeIIUsWTFQJzJe%JgVQN(N(Y2moZK%?j zT5O)~j`LK3$x4Pyhx29<-mXRAo|EM=!pV!NIx~q1&rVV!p6?q^$UHebw>-#K8@gSt z^6+9JA#)=sU#q!nKJWhDvnlZ*6~g8RQK{AuV>HJO9Lepxx0#Vr+}WmV{;apN>-H@F z_bw(hlroy*&F1O*{@^$MbTnhX17UV$ypdj|}vS1LD4mJE$7h=0I_4FMGSuCLQj)8pt+)3{HZH7`9@i{PeGB634;%e7< z@t@4>i#EEYR% zuVSJfaquP36()eaG1kkcr0lb(+_F2NzSp*|!tAf9SXi;&n+P?N(YJ*Sx|pJmg#Pfi zhvi(b(mbgV)gW@{37VI~MDIYIsHt_qV9Wx8kS0iAn;pKoTJwgW`L$-r|Hq0RgQtfv z6&t&U0}*S5+`US1J*2D^fgAa82iBt>k%xsEsHcGhbGJeCIsufd70_fWAyqg~7H>@Z zpF(T6$iVBbVNpv;-cMnVLaQX*YbN7iYl5PPTGb8weUMs_!-pgv5%7G35S%atoPC6j zDB>DXO{Cw-5p&L-xv8U}BTjy0cffro?|*D(a`TD9Q2}>6jewJ=Z=e!qwbHg*Tyw$r z%CUdhZv8-nAvduww8{jW6X^nqI1rR*L4)t$Gy$vN4gsOSPNKWyEnt&mp^gAbK`j{S zK?qSv=yoac_>hi8dHPU`L6HIm38)71BecKR9@Y~u6H*B9g(?Dqq3?)=2GWqdf^&<7 z3KPJ*qXN379`HVji))4?42pF?iGkD?Itkbcl4G<+stYTQ(N#(p<0|`cmC|E$mGTX6 z0gL2^;7TAkQA62ylTi0{r3GaXK+C2qzjsSDFwklkvP0z%3c~W#B#w z@J|&2kiPS9Hkr=Oy9%uYJOEw_W}bmF2KgGWn*amIPoeJsvxvsnvq0uAfbJ9l8aaPH zPUqpd7@nX<&R+|EJ3(jVZ-8I|4uF(^c&I0!08{{L{tL4RsDv#**PGvr6V)L!Q3w>^ z#20WAiwd-0n1HEZd6M=Q%mse})&LMt0M!IgXmTRZN)@8if*UyX8z@T_!u(V^wV)Y% zQt6DML^w)723)~FdKkJ0NCU$(S|dFNtJ3hpyNrYE@U51m(LvHCD955)J9tHa7wDza zHnia_#h{P_(F9lnMSur9Pp1B%9w1#S(lI?bvH`#eiLhBrDieL6R?%-(0?G~@$-mZ?$K=DxB z&+l$cz>aG(@wvxku~Z|#dZWP8d&9x;9vhD~2bmAvTH=g01r%H3<3y@feKk5ZvE}Z^ z6Q$!j8;+d`RO$U81UnQH{fR^L9G)x@M1>CdRi*t{_k71Vo#2q`=mfz#_y(fk+#a2} zvrM;p!bNS136TT36FR1#)q@?u#KGP++EM;y@xttQtFt}acLDK#q}yZ1MO(bBp*@pZ zJK7?f5tg2#y3)0wC-BtxfaaX9Ei4wFwvM>#5#x+n4?D#c*`4ea2k$GAGG+aUl&JB? zq00zI&h1I|4|SVfI!#lT-ixk#`UG3b*R976*4PhlA&1^YrI+AIjtByB@cY^ZkvXC$ z@u|nQ4;4E;L|SeN_%v?*QEXL;(}w;>4rPU9wCLb`&nRCXO+* zb3}ddO*a?ol=Q#1i#~8M+w9X-#zAL~p+7PxTP^Txd+!>1UsL0w<(xMTr!0H1=(5*a zBtVTHfi5Frwcfg92)j*dqb%lKb-YuE-Y@z+3fhAWEY%_i%Em|CeMrqlTbEMYNP1K< zz(a<6S|D78d(dK_8#fM;q{e8MlRhBC#3+ak7r=HYIz$!HtAylsfKqB4g$U-~Ajc7P zj3}<`Wo2=thDc!T;9O&or|XJ~Q;N@#h_TAi4v%FnzxJMN_q%i*_$B`-j$|CP5;a~E zx_mtb9L$UM$R&d?A*AM9F{HEi_m@mVpED==6&;wVBhv1=11n0Bp zr=Md6b*QILCe)Cd+E~#eu(-o*=)4zr-hJ&p_LnO|y=Cpc*FQG;8Ashpti&1?XBd5d zT?hJyWAl85o_z>!skX3f;AKHdF?Lk!U&}aZLAFoesgYH9@gt KPs6Z+db}8#(VYdh@j4WL!rse5^P#S}lQYca*P!J?alllOf*4Bnn zaC!4{c^C<;RWaBqcq6C@5@V~;f&{cRDMi#i4Dms>i56{8XU^d-Cz+jZ=lsrh&%K-d zW=`%uFOO_BN>d`yI3kJFdrrxizITg=-QXa-(W2z)Epu8lB#8@8*g*Kx&X)}6Y%w8w5;PNJCTeN$F=enDX%l?%+OD(>aV#hBRuXSXqMfLJKfIBB1GN&BR zd|<0?Wn|rzv$HBLXT`?&L39O|_9)>kvk3O$ghi_tMI4EYz70T~DHa;ixxZ zwbrn{Vq?PH-00ne)#6Chyc~+MIzyV;lFkMFuj=!Eo3*@gX87B?dPJA%qS)}X)26`= z3^3$ih9MtPi&l!sK|+hoR|WOh)T;)wFyL5=x}%VoLiBQ|7+(@7MYzMT53#MJei9`N z<>+E~3ZF4ljfTtXFS-tWlhi-S%B1jsm# ziyT_SD#MS6dxiS1qm1EafME$27=BkbXF`C>pv~zfs}Dvy4ajtn^)~joG>4LpHio^J zB(Mg-%@B(49xY=HLqQMycu^$UPDg#ZM-y2Ud_ukj1R3^w)Bc=zE^hJSFJK=FfM z*nk_ono$WPqk)dl-lrLrAnfd;(ZX?+q@Bk2>o$8C;71Tzgcf0c=3{$y9V27xnAujkyfw={wXX1txeS1yzP)RTQPm?I`b#ZFI_mAB+ia6>#E92f=OF}{=Z`)g?i5lt7Iyi_rhzi!v3 zHnlJzsQ&kxS(PFf1z7*siU-yonGV(ecXO!n4n_f?_l|6mw!dFPMfoSQsw(nttN1Eq z8R!EwbLM7QRbc1~ZV_pI7sU%zczd#~nu364uVr&Ug@imrFhg;2sM<;X+_f@0?_cDA zh^2LI7FCa6)NuD&d#QX{_jjn7p|BwJw0}~oz5O24u8ftFQ#BMEyl&=(hWf-o0}39e z|9x2JOcfHi{{OR=PWTf}PL7Yg%L zejT_3G+Pd2sU#2!g0R4hTW_=%o=)bU$Xt3X-%KePQ$=ve`ESza+E?5V=r0YZx?GH@ zqH>4Q|NYaa6wT4Bm1RG=Y#&@jJ;(|W7Rcoeu&i$Aw!d)JEcOOe-`?Khj%<3Jw|KTbUfh@N83D8aK{DNg~D-6|NfJuA2ZlMODxljbg9lU^^$0u=eNA@~=uFdzf7K}en*=*u@g diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index ca6903e21b529945937fe2623a1ce7041e783ead..c172f759c6efbd1781815855989ed5839c88c4bf 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~oNz0su6e1X2J1 literal 17 VcmZSnIL9>cl}9%Z0~oNX0{}E;1RMYW diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index e6f975d6cc4fb58de6017108deb037cdac3c138a..c9b25e9d7c1803318a2d2f01c0a2efc24bb97c02 100644 GIT binary patch delta 73 zcmeymm2u`a#tkMCj69o7B_f&lTblN!FFCfA0SuzPZf=#z6y;YIUix;zV;hKIH2dbK X`reF;h8q3x@yz diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..69b1a3c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,97 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..31473d4 --- /dev/null +++ b/simgui.json @@ -0,0 +1,80 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK MAX [10]": { + "header": { + "open": true + } + }, + "SPARK MAX [11]": { + "header": { + "open": true + } + }, + "SPARK MAX [20]": { + "header": { + "open": true + } + }, + "SPARK MAX [21]": { + "header": { + "open": true + } + }, + "SPARK MAX [30]": { + "header": { + "open": true + } + }, + "SPARK MAX [31]": { + "header": { + "open": true + } + }, + "SPARK MAX [40]": { + "header": { + "open": true + } + }, + "SPARK MAX [41]": { + "header": { + "open": true + } + }, + "navX-Sensor[0]": { + "header": { + "open": true + } + } + }, + "RoboRIO": { + "5V Rail": { + "open": true + }, + "6V Rail": { + "open": true + }, + "RoboRIO Input": { + "open": true + }, + "window": { + "visible": true + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/SendableChooser[0]": "String Chooser" + } + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "height": 332 + } + ] + } + } +} diff --git a/src/main/java/frc/robot/Autons/MoveForward.java b/src/main/java/frc/robot/Autons/MoveForward.java index 57db7d5..d0136f4 100644 --- a/src/main/java/frc/robot/Autons/MoveForward.java +++ b/src/main/java/frc/robot/Autons/MoveForward.java @@ -1,43 +1,43 @@ -package frc.robot.Autons; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import frc.robot.ModuleUtilities.ModuleGroup; -import frc.robot.subsystems.DriveBase; -import frc.robot.subsystems.Systems; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -public class MoveForward extends CommandBase { - private final ModuleGroup m_moduleGroup; - private final DriveBase m_driveBase; - - public MoveForward() { - m_moduleGroup = Systems.moduleGroup; - m_driveBase = Systems.driveBase; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_moduleGroup.resetDrive(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0)); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_moduleGroup.resetDrive(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} +// package frc.robot.Autons; + +// import edu.wpi.first.wpilibj2.command.CommandBase; + +// import frc.robot.ModuleUtilities.ModuleGroup; +// import frc.robot.subsystems.DriveBase; +// import frc.robot.subsystems.Systems; + +// import edu.wpi.first.math.kinematics.ChassisSpeeds; + +// public class MoveForward extends CommandBase { +// private final ModuleGroup m_moduleGroup; +// private final DriveBase m_driveBase; + +// public MoveForward() { +// m_moduleGroup = Systems.moduleGroup; +// m_driveBase = Systems.driveBase; +// } + +// // Called when the command is initially scheduled. +// @Override +// public void initialize() { +// m_moduleGroup.resetDrive(); +// } + +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0)); +// } + +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) { +// m_moduleGroup.resetDrive(); +// } + +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return false; +// } +// } diff --git a/src/main/java/frc/robot/Autons/RotateInPlace.java b/src/main/java/frc/robot/Autons/RotateInPlace.java index 4345c28..c4217b9 100644 --- a/src/main/java/frc/robot/Autons/RotateInPlace.java +++ b/src/main/java/frc/robot/Autons/RotateInPlace.java @@ -1,43 +1,43 @@ -package frc.robot.Autons; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import frc.robot.ModuleUtilities.ModuleGroup; -import frc.robot.subsystems.DriveBase; -import frc.robot.subsystems.Systems; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -public class RotateInPlace extends CommandBase { - private final ModuleGroup m_moduleGroup; - private final DriveBase m_driveBase; - - public RotateInPlace() { - m_moduleGroup = Systems.moduleGroup; - m_driveBase = Systems.driveBase; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_moduleGroup.resetDrive(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 1)); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_moduleGroup.resetDrive(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} +// package frc.robot.Autons; + +// import edu.wpi.first.wpilibj2.command.CommandBase; + +// import frc.robot.ModuleUtilities.ModuleGroup; +// import frc.robot.subsystems.DriveBase; +// import frc.robot.subsystems.Systems; + +// import edu.wpi.first.math.kinematics.ChassisSpeeds; + +// public class RotateInPlace extends CommandBase { +// private final ModuleGroup m_moduleGroup; +// private final DriveBase m_driveBase; + +// public RotateInPlace() { +// m_moduleGroup = Systems.moduleGroup; +// m_driveBase = Systems.driveBase; +// } + +// // Called when the command is initially scheduled. +// @Override +// public void initialize() { +// m_moduleGroup.resetDrive(); +// } + +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 1)); +// } + +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) { +// m_moduleGroup.resetDrive(); +// } + +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return false; +// } +// } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 67d9122..315ee9f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.RobotContainer; /** * The VM is configured to automatically run this class, and to call the functions corresponding to diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java deleted file mode 100644 index b341c22..0000000 --- a/src/main/java/frc/robot/RobotContainer.java +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import frc.robot.subsystems.Systems; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. - */ -public class RobotContainer { - // The robot's subsystems and commands are defined here... - - // Replace with CommandPS4Controller or CommandJoystick if needed - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - // Configure the trigger bindings - configureBindings(); - } - - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. - */ - private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - } - - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return Systems.autonLoader.getAuton(); - } - public void runTeleopCommand() { - Systems.teleopDrive.schedule(); - } -} diff --git a/src/main/java/frc/robot/commands/AutonCommands.java b/src/main/java/frc/robot/commands/AutonCommands.java index d2d01a8..bce2812 100644 --- a/src/main/java/frc/robot/commands/AutonCommands.java +++ b/src/main/java/frc/robot/commands/AutonCommands.java @@ -1,12 +1,10 @@ package frc.robot.commands; -import frc.robot.Autons.*; - // This file will be used to store auton commands (activating intake, etc.) public class AutonCommands { - public static final RotateInPlace ROTATE_IN_PLACE = new RotateInPlace(); - public static final MoveForward MOVE_FORWARD = new MoveForward(); + //public static final RotateInPlace ROTATE_IN_PLACE = new RotateInPlace(); + //public static final MoveForward MOVE_FORWARD = new MoveForward(); public AutonCommands() { diff --git a/src/main/java/frc/robot/commands/AutonLoader.java b/src/main/java/frc/robot/commands/AutonLoader.java index 7308369..c9abb06 100644 --- a/src/main/java/frc/robot/commands/AutonLoader.java +++ b/src/main/java/frc/robot/commands/AutonLoader.java @@ -13,7 +13,6 @@ import frc.robot.Constants; import frc.robot.subsystems.DriveBase; -import frc.robot.subsystems.Systems; public class AutonLoader { private static HashMap eventMap; @@ -22,9 +21,11 @@ public class AutonLoader { private static SendableChooser chooser; private static List pathGroup = PathPlanner.loadPathGroup("trajectory", Constants.Swerve.AUTON_CONSTRAINTS); - public AutonLoader() { - m_driveBase = Systems.driveBase; - + public AutonLoader(DriveBase driveBase) { + + m_driveBase = driveBase; + chooser = new SendableChooser<>(); + eventMap = new HashMap<>(); eventMap.put("event1", new PrintCommand("event 1 passed")); eventMap.put("event2", new PrintCommand("event 2 passed")); @@ -46,19 +47,7 @@ public AutonLoader() { } private Command getAutonFromPath(String path) { - switch (path) { - case "rotateInPlace": - return AutonCommands.ROTATE_IN_PLACE; - - case "moveForward": - return AutonCommands.MOVE_FORWARD; - - case "PathPlannerTest": - return autoBuilder.fullAuto(pathGroup); - - default : - throw new IllegalArgumentException("Make sure you enter a valid Auton name!"); - } + return new PrintCommand(path); } public Command getAuton() { diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index 298bed1..a8f2d5e 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -6,8 +6,8 @@ import frc.robot.Constants; import frc.robot.subsystems.DriveBase; -import frc.robot.ModuleUtilities.ModuleGroup; -import frc.robot.subsystems.Systems; +import frc.robot.subsystems.RobotContainer; +import frc.robot.subsystems.ModuleGroup; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -27,9 +27,11 @@ public class TeleopDrive extends CommandBase { private final ModuleGroup m_moduleGroup; private final DriveBase m_driveBase; - public TeleopDrive() { - m_moduleGroup = Systems.moduleGroup; - m_driveBase = Systems.driveBase; + private int cnt = 0; + + public TeleopDrive(ModuleGroup moduleGroup, DriveBase driveBase) { + m_moduleGroup = moduleGroup; + m_driveBase = driveBase; } @Override @@ -40,9 +42,12 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double xVel = Systems.getLeftJoyX() * Constants.Swerve.maxVelocity; - double yVel = Systems.getLeftJoyY() * Constants.Swerve.maxVelocity; - double omega = Systems.getRightJoyX() * Constants.Swerve.maxAngularVelocity; + double xVel = RobotContainer.getLeftJoyX() * Constants.Swerve.maxVelocity; + double yVel = RobotContainer.getLeftJoyY() * Constants.Swerve.maxVelocity; + double omega = RobotContainer.getRightJoyX() * Constants.Swerve.maxAngularVelocity; + + if (cnt++ % 50 == 0) + System.out.printf("xVelocity: %f, yVelocity: %f, AngularVel: %f", xVel, yVel, omega); m_driveBase.setDriveSpeed(new ChassisSpeeds(xVel, yVel, omega)); } diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index b92adcb..b076789 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -1,6 +1,4 @@ package frc.robot.subsystems; -import frc.robot.ModuleUtilities.ModuleGroup; -import frc.robot.ModuleUtilities.ModuleState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -12,9 +10,9 @@ public class DriveBase extends SubsystemBase { private final ModuleGroup m_moduleGroup; private final Kinematics m_kinematics; - public DriveBase() { - m_moduleGroup = Systems.moduleGroup; - m_kinematics = Systems.kinematics; + public DriveBase(ModuleGroup moduleGroup, Kinematics kinematics) { + m_moduleGroup = moduleGroup; + m_kinematics = kinematics; m_moduleGroup.resetDrive(); m_chassisSpeeds = new ChassisSpeeds(0, 0, 0); diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 5a55b8a..c426b16 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -5,7 +5,6 @@ package frc.robot.subsystems; import frc.robot.Constants; -import frc.robot.ModuleUtilities.ModuleState; import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -19,10 +18,9 @@ public class Kinematics { private final AHRS m_ahrs; - public Kinematics() { + public Kinematics(AHRS ahrs) { this.fieldCentric = Constants.OperatorConstants.fieldCentric; - m_ahrs = Systems.ahrs; - + m_ahrs = ahrs; } private double[][] computeStrafe(double joy_x, double joy_y) { diff --git a/src/main/java/frc/robot/ModuleUtilities/Module.java b/src/main/java/frc/robot/subsystems/Module.java similarity index 99% rename from src/main/java/frc/robot/ModuleUtilities/Module.java rename to src/main/java/frc/robot/subsystems/Module.java index eb406a4..2906a59 100644 --- a/src/main/java/frc/robot/ModuleUtilities/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -1,9 +1,9 @@ +package frc.robot.subsystems; + // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.ModuleUtilities; - import com.revrobotics.CANSparkMax; import frc.robot.Constants; diff --git a/src/main/java/frc/robot/ModuleUtilities/ModuleGroup.java b/src/main/java/frc/robot/subsystems/ModuleGroup.java similarity index 94% rename from src/main/java/frc/robot/ModuleUtilities/ModuleGroup.java rename to src/main/java/frc/robot/subsystems/ModuleGroup.java index 167f2a3..5cbba42 100644 --- a/src/main/java/frc/robot/ModuleUtilities/ModuleGroup.java +++ b/src/main/java/frc/robot/subsystems/ModuleGroup.java @@ -1,4 +1,4 @@ -package frc.robot.ModuleUtilities; +package frc.robot.subsystems; public class ModuleGroup { private final Module[] m_modules; diff --git a/src/main/java/frc/robot/ModuleUtilities/ModuleState.java b/src/main/java/frc/robot/subsystems/ModuleState.java similarity index 89% rename from src/main/java/frc/robot/ModuleUtilities/ModuleState.java rename to src/main/java/frc/robot/subsystems/ModuleState.java index cb93760..1e7605a 100644 --- a/src/main/java/frc/robot/ModuleUtilities/ModuleState.java +++ b/src/main/java/frc/robot/subsystems/ModuleState.java @@ -1,4 +1,4 @@ -package frc.robot.ModuleUtilities; +package frc.robot.subsystems; public class ModuleState { private final double m_vel; diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java new file mode 100644 index 0000000..b94c5de --- /dev/null +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -0,0 +1,103 @@ +package frc.robot.subsystems; + +import frc.robot.Constants; + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it u.nder the terms of +// the WPILib BSD license file in the root directory of this project. + + +import frc.robot.Constants.OperatorConstants; +import frc.robot.commands.AutonLoader; +import frc.robot.commands.TeleopDrive; +import frc.robot.subsystems.ModuleGroup; +import frc.robot.subsystems.Module; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and trigger mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems and commands are defined here... + + public static ModuleGroup moduleGroup = new ModuleGroup(new Module[]{ + new Module(0, Constants.DriveTrainConstants.invertedMotors[0]), + new Module(1, Constants.DriveTrainConstants.invertedMotors[1]), + new Module(2, Constants.DriveTrainConstants.invertedMotors[2]), + new Module(3, Constants.DriveTrainConstants.invertedMotors[3]), + }); + public static AHRS ahrs = new AHRS(SerialPort.Port.kMXP); + public static Kinematics kinematics = new Kinematics(ahrs); + public static DriveBase driveBase = new DriveBase(moduleGroup, kinematics); + public static AutonLoader autonLoader = new AutonLoader(driveBase); + public static TeleopDrive teleopDrive = new TeleopDrive(moduleGroup, driveBase); + private final static CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); + + // Replace with CommandPS4Controller or CommandJoystick if needed + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + // Configure the trigger bindings + configureBindings(); + } + + /** + * Use this method to define your trigger->command mappings. Triggers can be created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary + * predicate, or via the named factories in {@link + * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link + * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller + * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight + * joysticks}. + */ + private void configureBindings() { + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` + + // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, + // cancelling on release. + } + + + public static double getLeftJoyX() { + if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { + return -1 * m_driverController.getLeftX(); + } else { + return 0; + } + } + public static double getLeftJoyY() { + if (Math.abs(m_driverController.getLeftY()) > Constants.OperatorConstants.joystickDeadband) { + return m_driverController.getLeftY(); + } else { + return 0; + } + } + public static double getRightJoyX() { + if (Math.abs(m_driverController.getRightX()) > Constants.OperatorConstants.joystickDeadband) { + return m_driverController.getRightX(); + } else { + return 0; + } + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autonLoader.getAuton(); + } + public void runTeleopCommand() { + teleopDrive.schedule(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Systems.java b/src/main/java/frc/robot/subsystems/Systems.java index e63ea44..facf98c 100644 --- a/src/main/java/frc/robot/subsystems/Systems.java +++ b/src/main/java/frc/robot/subsystems/Systems.java @@ -1,59 +1,50 @@ -package frc.robot.subsystems; +// package frc.robot.subsystems; -import com.kauailabs.navx.frc.AHRS; +// import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants; -import edu.wpi.first.wpilibj.SerialPort; -import frc.robot.Constants.OperatorConstants; -import frc.robot.ModuleUtilities.Module; -import frc.robot.ModuleUtilities.ModuleGroup; -import frc.robot.commands.AutonLoader; -import frc.robot.commands.TeleopDrive; +// import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +// import frc.robot.Constants; +// import edu.wpi.first.wpilibj.SerialPort; +// import frc.robot.Constants.OperatorConstants; +// import frc.robot.ModuleUtilities.Module; +// import frc.robot.ModuleUtilities.ModuleGroup; +// import frc.robot.commands.AutonLoader; +// import frc.robot.commands.TeleopDrive; -public class Systems { +// public class Systems { - private final static Module[] modules = new Module[Constants.DriveTrainConstants.wheels]; - public static ModuleGroup moduleGroup; - - public static DriveBase driveBase; - public static Kinematics kinematics; - public static AutonLoader autonLoader; - public static TeleopDrive teleopDrive; - private final static CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); - public static AHRS ahrs = new AHRS(SerialPort.Port.kMXP); - public Systems() { - for (int i = 0; i < Constants.DriveTrainConstants.wheels; i++) { - modules[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i]); - } - moduleGroup = new ModuleGroup(modules); +// public Systems() { +// for (int i = 0; i < Constants.DriveTrainConstants.wheels; i++) { +// modules[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i]); +// } +// moduleGroup = new ModuleGroup(modules); - driveBase = new DriveBase(); - kinematics = new Kinematics(); - autonLoader = new AutonLoader(); - teleopDrive = new TeleopDrive(); - } +// driveBase = new DriveBase(); +// kinematics = new Kinematics(); +// autonLoader = new AutonLoader(); +// teleopDrive = new TeleopDrive(); +// } - public static double getLeftJoyX() { - if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { - return -1 * m_driverController.getLeftX(); - } else { - return 0; - } - } - public static double getLeftJoyY() { - if (Math.abs(m_driverController.getLeftY()) > Constants.OperatorConstants.joystickDeadband) { - return m_driverController.getLeftY(); - } else { - return 0; - } - } - public static double getRightJoyX() { - if (Math.abs(m_driverController.getRightX()) > Constants.OperatorConstants.joystickDeadband) { - return m_driverController.getRightX(); - } else { - return 0; - } - } -} +// public static double getLeftJoyX() { +// if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { +// return -1 * m_driverController.getLeftX(); +// } else { +// return 0; +// } +// } +// public static double getLeftJoyY() { +// if (Math.abs(m_driverController.getLeftY()) > Constants.OperatorConstants.joystickDeadband) { +// return m_driverController.getLeftY(); +// } else { +// return 0; +// } +// } +// public static double getRightJoyX() { +// if (Math.abs(m_driverController.getRightX()) > Constants.OperatorConstants.joystickDeadband) { +// return m_driverController.getRightX(); +// } else { +// return 0; +// } +// } +// } From 11c1131cce6bfea537cad331aad2158b482429a3 Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Sat, 15 Apr 2023 21:53:57 -0500 Subject: [PATCH 04/31] rid of ModuleGroup, moved ModuleState to Module --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 43347 -> 43547 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 27167 -> 27575 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 23321 -> 23375 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes simgui.json | 8 ++- .../java/frc/robot/commands/TeleopDrive.java | 19 +++--- .../java/frc/robot/subsystems/DriveBase.java | 26 ++++++-- .../java/frc/robot/subsystems/Kinematics.java | 42 +++++++----- .../java/frc/robot/subsystems/Module.java | 62 ++++++++++++++---- .../frc/robot/subsystems/ModuleGroup.java | 23 ------- .../frc/robot/subsystems/ModuleState.java | 19 ------ .../frc/robot/subsystems/RobotContainer.java | 13 +--- 17 files changed, 109 insertions(+), 103 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/ModuleGroup.java delete mode 100644 src/main/java/frc/robot/subsystems/ModuleState.java diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index a1993477eff8c3a502da195afd77b22cbcf253ad..0c0081b79883627ac2eb2f16233987db94ab3c1e 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~j#x1OPEd1Xln6 literal 17 VcmZQ>zc8b1qOQ9v0~j!`1pqNS1WNz_ diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 6475e5e34a778939ce316e410f844fc3687a8a6a..bd5235b3e6b640d4393eaf194a7e0d10ce613238 100644 GIT binary patch delta 1378 zcmaizdr%Ws6vlHlf~X)#93HK$L;(S3j0z+v3iUCfL~7BYf20+~JT{OR5=a6`unf^y zE04@T7r3=38fyiH@{l;)0@k9X1tLyI3fdN{q$8!4Din(%2<;|lrxRz|{bP6T+3(zM z@BY41ZSkqL_%sO;!i6*%2rG5!sA#d)n5EaG9pH+TkvxMmLoA6@DRJ7|wUEe4sD}}h zWyIc&l;Yy-v=oh6tk%fY9EDb`qA}k4{Gz0ybLdf@(A3>u`umrK?>{h4u00I%fHJ^s z4!J(iwp^U*$xfoyN&jI;oKCNW2QDM-oRRCEDp$##dmZXBT&O#EVKBiFv1yWBq^fjz zotyJYu~`MCK%4OJBmIgfo-~`U&r<3%X%enFS|y1Tkh< zNn88$hpnM8DV6D3T#+S?*7La{EnkiE6uCw&KT0N(8RxnngIt~tC3MoK6?TMRi?1N& z9ng?hZ^Jw~L-m(3C~@s~$?S9M{&?##M-cailkiPeT1B?x_t9!;G4qS))rZs4Zl zQ0_gh=rh|tyZn7yk-cld4OdT<2UuL<;D3an@v$fDduDPs&Rkp&e(>;Dw_L&r4)x_$ z0p^Fw9zdjY%DfORn`&ck>Vk8QD+-R>cZu$?s1pbE83mD}JuPQw7dXZ+fgFgv%8SavLe zOdf?%2(HL&u5V{{-!*#CC$<)t)Su2ee{fx%b2?(=8VRmVL2w(EkiDaj3CkK3_U^JR zm%^}D`gs;v@4?4+?^DCnpZ^O!c-H;Nq(m=w5&nK4(uuNdMvqbBDg#bk5 z-xnpl4j$w;J231eraK)6vVH-Z(XLk5Pu#l0Bzt<`RZp@;f!*+B@5eqlPv7h~Iz-H_ znMNi?@zGNhuEIl|bSJR*U^HmNssheD?%B6%RB@dS?a>_n6k)HHrdf(ry6$Cff)>oB5lAnFw6`x6c}JwQlcfaVbQQC zPkTuqDIiM`XhE!xT{;D{p@x`(btzI31O-6?Dg{jKY^D@H^nJJ=PX7P%?m55vZ!Xhr zF4OMd^O)f@8ieB}7P1B=nZav^8C+nG_;~0duvj8-LiQ;a@=LzC&T?Vjn^o7ZfB_1_ zNh(qT0@QUV#nQ0LDj|-=wt=G+_tsUNFQ_{or=Guul9ine%dK-oUu1gZ%ESqIr`*%iGgA}YJ!C@n^bD~l zP5m*t(l?I+Q-0~{=z7=CR#mU952{WQibW~1)U=e$glv%{El27pN$}2-c;yH*u4|N4 z5}c<~+Px5Jg9tg+n}$qk>=7{N#BmSrR^5mBK~+?!b|; z_QTa>*$;)#K!VtSv>e`pYi6O8*fPH@KV4MLyLAPR#!479Hl8F$t-! zt#=RB7(tWLWxDrY&3~*>S%NIF##+0#cK%!;dox0-yKP^t{S}Rsq0~@1a}W#cpM7eg zgI+H7alCwg42OV8Xi-i>Rm=ki2-hW+L!L{9;0|T-3FB`+IWF)82mJC0Ofr`{H%8X+ z_*2+-P;Z%ArN8R8TsSI86Q>HYBtjWS zm?DtLXhc{fclgD{+4#PjB++)g<;;Led)ETW)XRITYi-E-xbY88_@4F-)LTaH8CB`# zoS<_1l2uLlG{U{)BU5}LX6r!5gl*xJcS)D}2D{&?)qB-kVOpGP6&=M0S&iHHz<9zq zeo!U8#!$>>#YYBs`5Cy2{&Hc;t2^B6KU<3mq@5qsD*Ro!mw7|QUZ#%eJ@)J7jz3uA zk4dBb>lGz_j8R)D>D)Sh6P}fAf_m#QT4lyGs=|u=#p0FY;n&DoTYSD zfjVL{@xR%iTXr4mSOfg%L!?bT^+rwz`7C2%1!Ayeh=SqQhfyG%*a@F>OE4f~d-SLc zUvzsHv6HJJ40iZ67m@E!#NP%Zdt4iWDu|unZ~zbOhda0~1Ql7RpS_9${T*n~tjdPg zHj&1*4IiD@iBDrw=i(#}WdgAi9`2k2BdmypE{^aACbcURA!EIrIA(C&00iO4UWljq jg@|EjL@#zd;kjydi?X{>e$`X5IX0yV2X!eAv48ys2Jtkt diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index eded620c37b4323456ee9b9ac10905b60d51c11b..438cc30645c2b8704bdb2b41f23723d9e0f8639e 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oM`0{}BR1Ze;O literal 17 VcmZRcbt>7YGeh$`0~oLf0RS@f1S9|e diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 4eb8a908f872fcd1efa4e772d2b7d27829d4eaff..d410a00e49d0de372bafda195e5518fb2753ed77 100644 GIT binary patch delta 736 zcmcb7iD~u~rVS<%j9)jKN@OzfFPOP_kM=1m1~6E;ee+aFMP~l2q#WZ7Y;R9rtqIW~Yd<;AS8a1^YYQ_=d-}fTlX*Mq6jWVp=A4TD zTV?-u52r?T(_x5~eRm*Q#7iePP7s!_+O+n7;K?VI9L!%`IvesKYWC|u)Yz?_d_GWh zGE||XFr1gw}1juiSAS`h9 zt+({CPPJVp!tN|}nRd?^YOEAc5(ot6Y`(s@g%RZO)sy3wN-3;3KP~a1u2OUTgj35; zrtgk}DBt%8qMWs1a`qgx&HI;nF$U;Kubbr~U3;6vw delta 547 zcmbPzh3WDorVS<%j2}0fN@Ox_-X={&lpH(04EHwZELBy(cB+Wx*xCYiq=3iD2b)fFDr*zDLGTEcA#RsHXUyVFy}kfdjlj~)*B7@ivQ+GgcsDtIsg#1?JJ%T^pEOU_>p2~;JHMO- zVsCaC)ZP`7v*)O7-oMm~F=*q9Ln{yQNC_L-IG)g3BJBoNxmRXAL?zo^AdMD8z5%XB zzD#+Nu>1IAfi>G!)qrt8q;`W1Bw_d-^x+Rt44?8&_i xFSOp5CxUhCsg8o^5J;S?*en0J#tp_2OpM%{O(kY93iPtxbnn>z@hk%vWCw0OD(T3`@4np9>gj?!s37m; zzfuhXiw{;RKKQ}Q1QEF6xmyRw7VQ`OUJb8<+*OUzwQw7k%Lh5zW@$ zd{iTXQNUr9j-S8#eyFk8#gj#~P6&85^H_D{tX={&AbImuEugWXXG{eJ43D}&4Vb$5 zt2WSE0y3d@l0Ym7!UA{xXW1pPX|?S6 zy>J`TYl{UyK{+%P9j*(RXeUj~;uggI}ie^T% z>+1DsEBRRy<9+Ji3Nsf^#Z;1LXrOaM^U}W_mxiW%_haMF!QIet(aW8 z84sq4mmjyyn6cl}9%Z0~oNb1^_hb1fT!_ literal 17 VcmZSnIL9>cl}9%Z0~oNz0su6e1X2J1 diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index c9b25e9d7c1803318a2d2f01c0a2efc24bb97c02..1df07ab1aed68e755b96a1288f61ee304a2f9243 100644 GIT binary patch delta 96 zcmbQajq&_8#tkMCjQo>LB@PI*FPOP_kM=1m1~7=W-TYRffKg!5!(yAfZJno~qS~87 pr6f26-o>bCXK}gsVIQ$OZ diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index c3e17c74b903134374b17c4437580dd47dc35f82..eb88c3322450370faaa1ff8339aaac6d3f659f03 100644 GIT binary patch literal 8 PcmZQzU~F$sSUCXz2hRej literal 8 PcmZQzU~Dh%um1=D2q^;p diff --git a/simgui.json b/simgui.json index 31473d4..23f00b7 100644 --- a/simgui.json +++ b/simgui.json @@ -68,13 +68,19 @@ "/SmartDashboard/SendableChooser[0]": "String Chooser" } }, + "NetworkTables View": { + "visible": false + }, "Plot": { "Plot <0>": { "plots": [ { "height": 332 } - ] + ], + "window": { + "visible": false + } } } } diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index a8f2d5e..22a1f7f 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -7,7 +7,6 @@ import frc.robot.Constants; import frc.robot.subsystems.DriveBase; import frc.robot.subsystems.RobotContainer; -import frc.robot.subsystems.ModuleGroup; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -24,30 +23,28 @@ public class TeleopDrive extends CommandBase { // Called when the command is initially scheduled. - private final ModuleGroup m_moduleGroup; private final DriveBase m_driveBase; private int cnt = 0; - public TeleopDrive(ModuleGroup moduleGroup, DriveBase driveBase) { - m_moduleGroup = moduleGroup; + public TeleopDrive(DriveBase driveBase) { m_driveBase = driveBase; } @Override public void initialize() { - m_moduleGroup.resetDrive(); + m_driveBase.resetDrive(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double xVel = RobotContainer.getLeftJoyX() * Constants.Swerve.maxVelocity; - double yVel = RobotContainer.getLeftJoyY() * Constants.Swerve.maxVelocity; - double omega = RobotContainer.getRightJoyX() * Constants.Swerve.maxAngularVelocity; + double xVel = RobotContainer.getLeftJoyX() * Constants.Swerve.maxVelocity * 0.3; + double yVel = RobotContainer.getLeftJoyY() * Constants.Swerve.maxVelocity * 0.3; + double omega = RobotContainer.getRightJoyX() * Constants.Swerve.maxAngularVelocity * 0.3; - if (cnt++ % 50 == 0) - System.out.printf("xVelocity: %f, yVelocity: %f, AngularVel: %f", xVel, yVel, omega); + // if (cnt++ % 50 == 0) + // System.out.printf("xVelocity: %f, yVelocity: %f, AngularVel: %f", xVel, yVel, omega); m_driveBase.setDriveSpeed(new ChassisSpeeds(xVel, yVel, omega)); } @@ -55,7 +52,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_moduleGroup.resetDrive(); + m_driveBase.resetDrive(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index b076789..6aa8eaf 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -2,19 +2,24 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class DriveBase extends SubsystemBase { private static ChassisSpeeds m_chassisSpeeds; - private final ModuleGroup m_moduleGroup; private final Kinematics m_kinematics; - public DriveBase(ModuleGroup moduleGroup, Kinematics kinematics) { - m_moduleGroup = moduleGroup; + public static Module[] moduleGroup; + + + public DriveBase(Kinematics kinematics) { m_kinematics = kinematics; - m_moduleGroup.resetDrive(); + moduleGroup = new Module[4]; + for (int i = 0; i < 4; i++) + moduleGroup[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i]); + m_chassisSpeeds = new ChassisSpeeds(0, 0, 0); } @@ -31,13 +36,20 @@ public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { m_chassisSpeeds = chassisSpeeds; } + public void resetDrive() { + for (int i = 0; i < 4; i++) { + moduleGroup[i].resetDriveAngleEncoder(); + } + } + @Override public void periodic() { - ModuleState[] targetModuleStates = m_kinematics.getComputedModuleStates(m_chassisSpeeds); - // targetModuleStates = Kinematics.normalizeVelocity(targetModuleStates); + Module.ModuleState[] targetModuleStates = m_kinematics.getComputedModuleStates(m_chassisSpeeds); + + for (int i = 0; i < 4; i++) + moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); - m_moduleGroup.setDrive(targetModuleStates); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index c426b16..6dac18a 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -17,6 +17,8 @@ public class Kinematics { private double gyro = 0.0; private final AHRS m_ahrs; + private int cnt = 0; + public Kinematics(AHRS ahrs) { this.fieldCentric = Constants.OperatorConstants.fieldCentric; @@ -84,12 +86,16 @@ public void conv(double[][] unicorn) { } } - public ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { + public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond / Constants.Swerve.maxVelocity; double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond / Constants.Swerve.maxVelocity; double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond / Constants.Swerve.maxAngularVelocity; + if (cnt++ % 50 == 0) { + System.out.printf("vel: %f, xVel: %f, yVel: %f", targetAngVelRatio, targetXVelRatio, targetYVelRatio); + } + if (fieldCentric) { this.gyro = this.m_ahrs.getYaw(); this.gyro *= Math.PI/180; @@ -99,33 +105,33 @@ public ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { conv(computeUnicorn(computeStrafe(targetXVelRatio, targetYVelRatio), computeRotation(targetAngVelRatio))); - ModuleState[] targetModuleStates = new ModuleState[4]; + Module.ModuleState[] targetModuleStates = new Module.ModuleState[4]; for (int i = 0; i < 4; i++) - targetModuleStates[i] = new ModuleState(vel[i], theta[i]); + targetModuleStates[i] = new Module.ModuleState(vel[i], theta[i]); return targetModuleStates; } - public static ModuleState[] normalizeVelocity(ModuleState[] moduleStates) { // keeps the ratio of wheel velocities while ensuring that each never goes above 1 - assert(moduleStates.length == 4); + // public static ModuleState[] normalizeVelocity(ModuleState[] moduleStates) { // keeps the ratio of wheel velocities while ensuring that each never goes above 1 + // assert(moduleStates.length == 4); - ModuleState[] newModuleStates = new ModuleState[4]; + // ModuleState[] newModuleStates = new ModuleState[4]; - double maxVel = 0; - for (int i = 0; i < 4; i++) - Math.max(maxVel, Math.abs(moduleStates[i].getVel())); + // double maxVel = 0; + // for (int i = 0; i < 4; i++) + // Math.max(maxVel, Math.abs(moduleStates[i].getVel())); - if (maxVel > 1) { - for (int i = 0; i < 4; i++) { - newModuleStates[i] = new ModuleState(moduleStates[i].getVel() / maxVel, moduleStates[i].getDir()); - } - } else { - newModuleStates = moduleStates; - } + // if (maxVel > 1) { + // for (int i = 0; i < 4; i++) { + // newModuleStates[i] = new ModuleState(moduleStates[i].getVel() / maxVel, moduleStates[i].getDir()); + // } + // } else { + // newModuleStates = moduleStates; + // } - return newModuleStates; - } + // return newModuleStates; + // } public double[] getVel() { return vel; diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 2906a59..7e70786 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -30,7 +30,7 @@ public class Module { private final RelativeEncoder integratedDriveEncoder; private final RelativeEncoder integratedAngleEncoder; - private final int module_num; // ZERO INDEXED + private final int moduleNum; // ZERO INDEXED private static final double kP = 0.4; private static final double kI = 0.0; @@ -43,23 +43,24 @@ public class Module { private WPI_CANCoder _CANCoder; public Module(int module, boolean inverted) { - this.module_num = module; + + this.moduleNum = module; /* Angle Motor Config */ - angleMotor = new CANSparkMax(Constants.MotorConstants.angleMotorIDS[this.module_num], MotorType.kBrushless); + angleMotor = new CANSparkMax(Constants.MotorConstants.angleMotorIDS[this.moduleNum], MotorType.kBrushless); configAngleMotor(); integratedAngleEncoder = angleMotor.getEncoder(); angleMotor.getPIDController(); - _CANCoder = new WPI_CANCoder(Constants.MotorConstants.CANCoderID[this.module_num], "canivore"); + _CANCoder = new WPI_CANCoder(Constants.MotorConstants.CANCoderID[this.moduleNum], "canivore"); // _CANCoder.setPositionToAbsolute(0); _CANCoder.configAllSettings(returnCANConfig()); _CANCoder.setPosition(0); /* Drive Motor Config */ - driveMotor = new CANSparkMax(Constants.MotorConstants.driveMotorIDS[this.module_num], MotorType.kBrushless); + driveMotor = new CANSparkMax(Constants.MotorConstants.driveMotorIDS[this.moduleNum], MotorType.kBrushless); configDriveMotor(); integratedDriveEncoder = driveMotor.getEncoder(); @@ -70,13 +71,37 @@ public Module(int module, boolean inverted) { this.invertDriveSpeed = inverted; // if(_CANCoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED) { - // throw new RuntimeException("CANCoder on Module #" + Integer.valueOf(this.module_num).toString() + " is not green!"); + // throw new RuntimeException("CANCoder on Module #" + Integer.valueOf(this.moduleNum).toString() + " is not green!"); // } } + public static class ModuleState { + private final double m_vel; + private final double m_dir; + + public ModuleState(double vel, double dir) { + m_vel = vel; + m_dir = dir; + } + + public double getVel() { + return m_vel; + } + + public double getDir() { + return m_dir; + } +} + +int cnt = 0; + public void setSpeedAndAngle(ModuleState targetState) { - setAngle(targetState.getDir()); - setDriveSpeed(targetState.getVel()); + double x = setAngle(targetState.getDir()); + double y = setDriveSpeed(targetState.getVel()); + + if (++cnt % 50 == 0) { + System.out.printf("Set module %d angle to %f, speed to %f\n", this.moduleNum, x, y); + } } //angle to normalize between 0 and 2PI RAD @@ -91,15 +116,19 @@ public double normalizeAngle(double angle) { return fixedAngle; } - public void setDriveSpeed(double speed) { + public double setDriveSpeed(double speed) { int invertMultiplier = 1; if (invertDriveSpeed) { invertMultiplier = -1; } - driveMotor.set(speed * invertMultiplier); + + double x = speed * invertMultiplier; + + driveMotor.set(x); + return x; } - public double getAngleInRadians() { + public double getAngleInRadians() { return _CANCoder.getPosition() * (Math.PI/180.0); } @@ -115,10 +144,10 @@ public double getAngularVelocity() { return integratedAngleEncoder.getVelocity(); } - public void setAngle(double angle_in_rad) { + public double setAngle(double angle_in_rad) { // //code to make the angle motor turn the least amount possible and drive direction if necessary - // double targetAngle = angle_in_rad + Constants.MotorConstants.angleOffsets[this.module_num - 1]; - // double currentAngle = getAngle(); + // double targetAngle = angle_in_rad + Constants.MotorConstants.angleOffsets[this.moduleNum - 1]; + // double cudoublerrentAngle = getAngle(); // double normalDifference = currentAngle - targetAngle; // double difference180 = currentAngle - normalizeAngle(targetAngle+180.0); @@ -130,6 +159,9 @@ public void setAngle(double angle_in_rad) { // pid.setSetpoint(targetAngle+180.0); // angles are in TRUE BEARING ( angles are negated ) // invertSpeed(); // } + + double x = (angle_in_rad); + pid.setSetpoint(angle_in_rad); if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180))) { @@ -137,6 +169,8 @@ public void setAngle(double angle_in_rad) { } else { this.angleMotor.set(0.0); } + + return x; } public void resetDriveAngleEncoder() { diff --git a/src/main/java/frc/robot/subsystems/ModuleGroup.java b/src/main/java/frc/robot/subsystems/ModuleGroup.java deleted file mode 100644 index 5cbba42..0000000 --- a/src/main/java/frc/robot/subsystems/ModuleGroup.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.subsystems; - -public class ModuleGroup { - private final Module[] m_modules; - - public ModuleGroup(Module[] modules) { - m_modules = modules; - } - - public void setDrive(ModuleState[] targetModuleStates) { - if (targetModuleStates.length != m_modules.length) - throw new IllegalArgumentException("Arguments to ModuleGroup.setDrive() must have the right number of swerve modules!"); - - for (int i = 0; i < m_modules.length; i++) { - m_modules[i].setSpeedAndAngle(targetModuleStates[i]); - } - } - - public void resetDrive() { - for (Module module : m_modules) - module.resetDriveAngleEncoder(); - } -} diff --git a/src/main/java/frc/robot/subsystems/ModuleState.java b/src/main/java/frc/robot/subsystems/ModuleState.java deleted file mode 100644 index 1e7605a..0000000 --- a/src/main/java/frc/robot/subsystems/ModuleState.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot.subsystems; - -public class ModuleState { - private final double m_vel; - private final double m_dir; - - public ModuleState(double vel, double dir) { - m_vel = vel; - m_dir = dir; - } - - public double getVel() { - return m_vel; - } - - public double getDir() { - return m_dir; - } -} diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index b94c5de..f283664 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -10,8 +10,6 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AutonLoader; import frc.robot.commands.TeleopDrive; -import frc.robot.subsystems.ModuleGroup; -import frc.robot.subsystems.Module; import com.kauailabs.navx.frc.AHRS; @@ -29,17 +27,12 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - public static ModuleGroup moduleGroup = new ModuleGroup(new Module[]{ - new Module(0, Constants.DriveTrainConstants.invertedMotors[0]), - new Module(1, Constants.DriveTrainConstants.invertedMotors[1]), - new Module(2, Constants.DriveTrainConstants.invertedMotors[2]), - new Module(3, Constants.DriveTrainConstants.invertedMotors[3]), - }); + public static AHRS ahrs = new AHRS(SerialPort.Port.kMXP); public static Kinematics kinematics = new Kinematics(ahrs); - public static DriveBase driveBase = new DriveBase(moduleGroup, kinematics); + public static DriveBase driveBase = new DriveBase(kinematics); public static AutonLoader autonLoader = new AutonLoader(driveBase); - public static TeleopDrive teleopDrive = new TeleopDrive(moduleGroup, driveBase); + public static TeleopDrive teleopDrive = new TeleopDrive(driveBase); private final static CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); // Replace with CommandPS4Controller or CommandJoystick if needed From a3842f25de002c9bdd1156f14128b06902e6be8c Mon Sep 17 00:00:00 2001 From: Joshua Vigel <56195553+marvalarva2929@users.noreply.github.com> Date: Sat, 15 Apr 2023 22:00:39 -0500 Subject: [PATCH 05/31] Update README.md --- README.md | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/README.md b/README.md index 9312bf4..cb43e14 100644 --- a/README.md +++ b/README.md @@ -4,14 +4,4 @@ Team 5417's code for the swerve bot, _Vira_! [![CI](https://github.com/frc5417/CustomSwerve/actions/workflows/main.yml/badge.svg)](https://github.com/frc5417/CustomSwerve/actions/workflows/main.yml) -This is our custom algorithm for swerve. Here are the developers: -* Krishna Shah -* Jordan Escobedo -* Rupak Bhattacharya -* Pranav Gattineni -* Brennan Coil -* Lucas Hatcher -* Joshua Vigel -* Matthew Caldarola -* Jeevan Adhya Vinoth Babu -* Ethan Wallraven +- This branch is an attempt by Joshua Vigel to refactor the code to more easily readable and compliable to Java standards. From bef6d8431940749e23738b383fc542dbfbd13a17 Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Mon, 17 Apr 2023 10:57:50 -0500 Subject: [PATCH 06/31] Attempted to fix the 5-radian problem to no avail --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 43547 -> 43697 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 27575 -> 28289 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes src/main/java/frc/robot/Constants.java | 4 +- .../java/frc/robot/commands/TeleopDrive.java | 10 ++--- .../java/frc/robot/subsystems/Kinematics.java | 6 +-- .../java/frc/robot/subsystems/Module.java | 42 +++++++++--------- 12 files changed, 31 insertions(+), 31 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 0c0081b79883627ac2eb2f16233987db94ab3c1e..c6a75a0615a896910c2d7f7ff589bf88573817bb 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~jzL1pqNq1Y-aI literal 17 VcmZQ>zc8b1qOQ9v0~j#x1OPEd1Xln6 diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index bd5235b3e6b640d4393eaf194a7e0d10ce613238..3a1ce65083f6d982bf00d98823b38d24f40d419f 100644 GIT binary patch delta 1928 zcmah}dsK{R81MO-X=_!!X>^P*%|uz3%_NC3YI3QFj%s%^DQae7LQ|$1XR#F)>lE^? zd7Kit91cQEYf3U9qSoDFcQF;x-JN2$_M2%rE`OLm=DhFk`Mtm2_dL)0K6|t6_Ga4^ zO%ED8K2J8iQ+mfMl=AsY7RcmD3%nCOQ{+mEo*#=O2OWgVHV5nJU*@^pSATr&xe?h!Ws&rv4E ztK1XRszeKVtK3LMXxPijvi$a+w~ii_Wj0MWd-|;`Q|ilLEkw50g81R6#WK`n=*8>6x4IB?+|l!&3dC@$fg zFTi1ofWs^#WJ6sv?pwr=9-GgJ>X&17IkUZC-qIKCNq2n&&bBk3laSRlbI@o3b~GB$-GLEEZYo51B|{2hLlIoYnH(5{ z?Jkrpac-H8rDqx2KRy5}TEa1?55T& zk3?&E(2EEvzedFa#%`tj{U@U-f}szgH{)e&uG4ahBZW__Hb34!TX>5|D*KvaWYDUQ zPVgMG;sxWTw*3%R9TR>!^}wna*nyHDY5>v=%++tl9AA)6ZHEw<<*1)*9#G^FXT=w-!nmaAme%|^Y}V#EiLG#T}IF4*@Ezd z5o@FKo@IuC7FA-(5r;N|1H0nP%I*&IZw4QBiFujc3|`j1o0g(l7KVz!1@AQjZCz-F zj@b};jVtzV0h+zV%onx5K&N#xyf{6oq)@m5E5uRIC<622db*M%iTnZ9?KP0_*4vh_SSq;j_obr!rH=Qf_+-SbMzG^ z+YZrg4&+QzCMp)irYL1?E7W*-gg!v-l^6 z@YfF2`};v2IP-iwc&>Ln?*@2IU$I?1Le^@wt6y%Ka^67c!`mP^&~@UTFV48$)w-EQ zjx0)SnyFzVX@eE1C|00w3TTK-zXUyesj-Axl7?@6sWHS*l16`euZTySqfT1NX3?Ma zT&F5er{E2d9GgWtDh4_5rfmJ!(qA;BsMBNTo9{(8p^&s!4A(`T5(1`8qJ|)A8Z0Gc zXVUQ8WX5b>rRK9H;`S-j5cr2k&d9JL1ir%cfs_ODI6rMCCI?a@aYzWL=}$X|enHeA z;&`@% zH^Wal#j2~R1!jxEtSq*gO+J!8{d3p4%pb#|=iOYFug`8w+eGAC zfMEjHU6c!73u@(}TH$ZL!Bptbyt<~*u)EO!&)gMt2eYBbsxK}pNUTUsVy($${lwz? zS0p7T6c!e)5DkTr-4R8w-j{@RV8lw2fNB4N;83Qb)L=9e6f26^a+cAkGzrQ?rG~+| zEf5O~c4d+g_`zTz1rfxJZWl2ntgXlu6VWJO@?V5uIM1KR;1gk@aeCcyC7niDXs}pq zN|Q}%w&)r0;i`hoBK)+H5*TksbE`XoLKv-;2f0e;9~366TcWqH zR=uTMZ*j$>y&+&4{2`QE)jR-GO(27#0(fjsLyK4=Lw{Wrw9XMsR2Pz9RJ}>Ty{?k3 z*QB@c-lY#067oPKd4d^R%n%Qsi))vB z5F%&TQj5W8W0$$gtFw7|u4#h|OR~uzoVypK@FV6|k;qg!F+U9>Y!G8BGFWt_ti`s1 zDYR($kYLQEdXu)?{tz`MDkmycibRI1dm)s2Gh2pBs)#1QYA7i+>KTKnwMLo9;Lb8o!X*JL?+c9G zIUpN&9g=1NHFOG)_`V>H`Gm>sb8&SG#=$kghkQ~5;hu%WpZY4|aL8b%l(@K1@L*I zi#)8MEf`ToO*|&x#@>LVo9zn4#bbnk>N1fK%z77O*u9OKJAEYy!aDjjn2(U_AW9P| zrJC;X-XOs#;m3yIFYkgBoA!txc{H`rW;T@@tOmN**}|9W>n#73Xm&0Vrp-D5BXRqC z)Hh%AHYW%6fOFDALH>*r>f{MNi|OzfH1DS%F#~YD4QL9O*z!sR4PB>QP*I=aPN|ss zaiD-BeB`SG3#~NJt@eQBrZ0>Y!?;kVH(ei9Hw1Ct*klff&KZIo&ru!;zgsHebWS+z z%7qY|GoCtyPYt5g;e;v@hTF$e-_nr<+{$tpHdT?iSQ-y9Pic7=u5Y0k>=7-G>z%9I zWMnz2bTpA_V9K;PdC-s(#Ikqa^b1DwwS{SJ}bd1t!nx`J{aU{32(>pp=d%(nTp0ltF z^G#l}aEylp9Lnwa@|O5l;VJcU+rN-{#Jo)((-*Zqm^T=wp0rbCfGhptX8JD&uqXef zMUm)UxHo$jL^;!LHx7v*rM`c7i=ea%7KB?ZTBd|GnCQz0eeI=Zh49jDW73w>Gt0_g z-Y1Yw_PHs1=@ZB#?~1|^eQBm=a0zSDQQc2}3Rw=_Zir85hYW}Koj29KP;b^Xl&#w7Vn z^JgKI*hAZ`X{LMuGoW4Any?-#l}s*tKd&q9zE-H-F`@fnZ7!}(rDS}87hM#nHL_N# z-YO;yuKf#)K}nQ&YXZ zdE0?yjZ4l5!mIp!eI!2Lt!;Sia`@P@UE_9cZ!k#l>N$w>J1!`WKMyH`kMbW?PYr<0 z9NQR%>(0Z_fv1GA_S8Tr=^RC=K?aJ$eVjpMN-6;#mF9^A}}o_S%8P>Ux79j#r7&8 z(lEfIRBi&_N3mTILa%pycG9lrU=(Q;k|sB*AGNgNbZ~HM3X$PUDI|p)@(7nPnPfU3 zv)!8NhBWZKAq_ZnK`@?fhXA}bnfTfHBDen~pNC+4Ih7=k&*V_34DMA1D1g zkZX7|mA+&0wAR1tCe_meYOgzU*Ult~ZmC?71rd%Qis>(ioX@?6bdo_n6z#}#6XZ%w zH1u4k3C*)ej3b8JwuNW0Am1BU47S_PyxVa`1{q3PL~CcdX{|H$`AS@|qT=xv^civjRbOb6k(K^KQrfJe| z)-TQ8INsA@3%ll%iDISWTHJ@!Z-HoUi+jG&ItnE@MB!nHuWo>BZ_0bxZKc-(3uz#D zP~zX~Ww&+sJ(QC@=(gh8tu($oZnvg*-D%b3l5rju`ZI2=$@=3WqVlL}Z-}F<*_L}r zyrTklZJLJSU3nx%j7`_lo%`^3tOM7xa;Kfssl8P0v}>B`T)Bs#axoFQS_>ZE8Y}io zf7u4nIC>>{22U*^@py10xd)ki8c~OYem5E(Bn$>Er2|5e&*h#v9f0a;@<;CL(<*4= zh34YPYBCF-W2ul>-9+ASOi8xP52$&s@?>($w;#L})11^M@{YUb!0#Q}9yf}=-kbfO zcW6Cje!k7tJMsVC)xCaY?R?4hdsi2Ky;W0q#P$E14()DwF?PUj^iuNNPT#bSHD|f2 z6Mn%}lE>C;roE&`SHI_b8~FI7N8-T>qQFO1ld||{FSRVm{-(G!>DvxA9X8gqb$wOy Zlj_wqcF_*!J34x;_up$S^QKoe{|AZzZr%U@ diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 438cc30645c2b8704bdb2b41f23723d9e0f8639e..b8649077433f826a783cbe577a0325db5bf18927 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oN(0{}CD1h4=A literal 17 VcmZRcbt>7YGeh$`0~oM`0{}BR1Ze;O diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index d410a00e49d0de372bafda195e5518fb2753ed77..207f562ea1d659dac59a60efb4ad2844c0e00e0d 100644 GIT binary patch delta 1230 zcmbPzg=ynerVS<%j6WuuN_^z+onn!^p7lQi7_5@sEGv0{RbbKCyZg6CsQBZW`rpKe#YuC~d$EJoptY>6Diz$XhJ28x$Xc3dnhU%Yi{!?tpb=lr<~ zm!#UogVa={*sE3ij(wET6mcI#~aMhdUtlu!L=%+uSFraHwFS zd)GtZ9i|M?i3rTGw-ICDVN$OCCT zV<4dk#EKv+P=4ZM$)gMJMN9HtJyEc`D}ib%qs!#=Gt26~EofZS7&!g--B-81l^Jrd zK#c_&00M%dKmyHJu9ZjF3^SW~Td#Z&HRe9>9o1OYa!ef@>94u9o}_vAe-%kv>Lz;w zsv`ra(j17{KnVlQl9F$BS*j;PzMq`swRf3a)M|*cG8=#@fq<=Y^6gnJliin!0E0SX z&E)u{Qj-H`rzkXDm}MHcbhk$IH~;TXW=h=w=>n#K84z6@os+N5cA3n%9IWfYWX0uD z3VuGDA5Xh*s(ee|)?yy}JLyn+ps9vGadP$?V5)iW@WAA$bHK@T$L8a6mM}?3xXt;? z>|mh2;iuuEUttE_)F9iGJ$mEq^ z#-p6gw--8apauU^dnY@lskh}V`NVe3p1F4y)L3YOwn3=Xf9L1Rlq|uD% fie=gt`1EvC?EZ46v$D@2Mm&Gg1JS`W52OPCTQRca delta 1106 zcmZXSdoWZ{9LM)XOUy3o)%Mt}nM7rUUXwiU?<4%*9CQC@etdhG*Ohz#cDm`XOH9C9m{!@N4=bp#?e81;+&iU#3X=N{I+O=-f z@378o8VgwPzzVbHF>Ns~Pw^6}jczHEiUozfX7g8sEOHOYF?3nI&`f!4;wUK;YK|;Z zdy_(h{S5f;59Ygi_3v))Jt3!89*>s>bhr?Z$@@A9vD)A^g8UN(BuN74SmW81OG0P% zp0YTVlMvSvF38-$GuoFSGl@P!e6+wk_y&cIHu2`PxF z$AP>PRaF$JupXPMm|<3|Y$+)Xif=eLOLG+BMCJy~8bn&5MvVbY=_&cKgFka0Uu=Dn^&qRrHL!<}m_&hyL}7Iw25$xPX@zgwr|X3s z-X$K1Hs=(}EYKnna-4)T^cZo8w4#v4vrMh9l+f$W_dM;&O9Iiz!%Vc{L4G zYP1=y8-Pb@E}Sq=Orl1(f+oh>&U9R#0iV=d(f4gfg(E?#ftfS!voHxuLAW>jydg7S z97g?r6>#mNy6x4fkR8#&mR|oq5m{u;dX&34XnP>F8s&rQf56#0Tz+h#Q-KD@ez5us s?c3G$_5^Lvph`0bBN_Y&8e_xAog#DgY>+95W7t70F^CMs29<%#zmIr&-~a#s diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index d8f2e6d9eb3e2f3a28b35e1cb7fed20230bfa84c..bcf634e838a6e2b631e6ed8878ffc42ac2b8bb0b 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~qiq001lu1Ec@| literal 17 VcmZR6A~{9FsWEOA0~m1I001l|1HS+O diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 9fc8a6a29543528ae3972493b907cf280e4caf05..e4a9d613d1ebfc94e47f62902e011f7389eb069d 100644 GIT binary patch delta 823 zcmdmfow4yQ;|3E6Mup9$5&}#D6O`t$EHZ4YW&nfi?UQY#9tg~Sn{s(p=u$<9VD9zF zqSAr<>CvykQ!dp*#R@0KN}mu|c6#v#=hrEf5W$?p&9X8Di~<{W7foV)kQxV7x^;4^ ztb)L%!@J#eL<^K5f_c)D=gMXXY`e5=qQL$GN1!UXH~*D&U=%pK;A!l}2d<@1Q|&jK z%1biwo3OEdOuQor)gQmvRq-SRvk8SM{N~ z!{>J|TsxLR#IkSxhXO9A89_JW8gD_>%1%~Qvk>6cxp(LHy5;YoihpkoRr6pHP&Au% zfrnqM10tIHbMjOT1p(z&>$_18_nScVAK$!I!;z6+san~!eki60t+G)8t$A8Jn?dNW)*uYgx{^ieA?`+;%cUN=9 z&11V;w)Q820tn3s*&6RI=HGL8-1kw|zxInCD8A5C_?NAG(>ZnW$EvkQo|;IP$78CP zW}9v4^)@`{R9x_(($y`XR0uU=vUeJ%>F?54od4!7GA~|s*m$i2C^@6qaR1fAX~J6~ Q6hE*{eZMM3@j6Ha02}%MX#fBK delta 117 zcmV-*0E+*C-2u1P0kAX}0SmJ=7!U%JT^iVvO&dv*t{cCzg&b%BlLa0alQkY-v#lOD z0ki!d5dyO&BESQa{v}A0UMAR+W+yMRttUVNlLaamvo$I?0h5L+kCQDdsI%=X2m!MV XE^z|0mN2OT0vQ>xA@~-vK}dEZqdh4{ diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 042fb75100f5aeabb38fc27b2f9581844c0abe5d..a3f3adce9d90220a7bb6693f26cd096897ae90ed 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~oNe001;a1Nr~} literal 17 VcmZSnIL9>cl}9%Z0~oNb1^_hb1fT!_ diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index eb88c3322450370faaa1ff8339aaac6d3f659f03..c680c2892c5089df1dfb9ec3218808ee7fcc1ece 100644 GIT binary patch literal 8 PcmZQzU~KQdym%G>2}T1b literal 8 PcmZQzU~F$sSUCXz2hRej diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e649283..b8ac5fb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -42,8 +42,8 @@ public static class MotorConstants { public static class Swerve { public static final IdleMode angleNeutralMode = IdleMode.kBrake; public static final IdleMode driveNeutralMode = IdleMode.kBrake; - public static final double maxVelocity = 3.8; // m/s - public static final double maxAngularVelocity = 56.0; // rad/s + public static final double maxVelocity = 1; //3.8; // m/s + public static final double maxAngularVelocity = 1; //56.0; // rad/s //velocity PID tuning for overall swerve public static final double velocitykP = 1.0; // 0.0001 public static final double velocitykI = 0.0; diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index 22a1f7f..9ed6671 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -39,12 +39,12 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double xVel = RobotContainer.getLeftJoyX() * Constants.Swerve.maxVelocity * 0.3; - double yVel = RobotContainer.getLeftJoyY() * Constants.Swerve.maxVelocity * 0.3; - double omega = RobotContainer.getRightJoyX() * Constants.Swerve.maxAngularVelocity * 0.3; + double xVel = RobotContainer.getLeftJoyX(); //* Constants.Swerve.maxVelocity; + double yVel = RobotContainer.getLeftJoyY(); //* Constants.Swerve.maxVelocity; + double omega = RobotContainer.getRightJoyX(); //* Constants.Swerve.maxAngularVelocity; - // if (cnt++ % 50 == 0) - // System.out.printf("xVelocity: %f, yVelocity: %f, AngularVel: %f", xVel, yVel, omega); + if (cnt++ % 50 == 0) + System.out.printf("xVelocity: %f, yVelocity: %f, AngularVel: %f", xVel, yVel, omega); m_driveBase.setDriveSpeed(new ChassisSpeeds(xVel, yVel, omega)); } diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 6dac18a..e7fd5e5 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -88,9 +88,9 @@ public void conv(double[][] unicorn) { public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { - double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond / Constants.Swerve.maxVelocity; - double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond / Constants.Swerve.maxVelocity; - double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond / Constants.Swerve.maxAngularVelocity; + double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond; /// Constants.Swerve.maxVelocity; + double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond; /// Constants.Swerve.maxVelocity; + double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond; /// Constants.Swerve.maxAngularVelocity; if (cnt++ % 50 == 0) { System.out.printf("vel: %f, xVel: %f, yVel: %f", targetAngVelRatio, targetXVelRatio, targetYVelRatio); diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 7e70786..c0f6d7b 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -32,7 +32,7 @@ public class Module { private final int moduleNum; // ZERO INDEXED - private static final double kP = 0.4; + private static final double kP = 0.04; private static final double kI = 0.0; private static final double kD = 0.005; @@ -42,6 +42,8 @@ public class Module { private WPI_CANCoder _CANCoder; + int cnt = 0; + public Module(int module, boolean inverted) { this.moduleNum = module; @@ -75,26 +77,6 @@ public Module(int module, boolean inverted) { // } } - public static class ModuleState { - private final double m_vel; - private final double m_dir; - - public ModuleState(double vel, double dir) { - m_vel = vel; - m_dir = dir; - } - - public double getVel() { - return m_vel; - } - - public double getDir() { - return m_dir; - } -} - -int cnt = 0; - public void setSpeedAndAngle(ModuleState targetState) { double x = setAngle(targetState.getDir()); double y = setDriveSpeed(targetState.getVel()); @@ -214,4 +196,22 @@ private void configDriveMotor() { // driveMotor.enableVoltageCompensation(Constants.Swerve.voltageComp); driveMotor.burnFlash(); } + + public static class ModuleState { + private final double m_vel; + private final double m_dir; + + public ModuleState(double vel, double dir) { + m_vel = vel; + m_dir = dir; + } + + public double getVel() { + return m_vel; + } + + public double getDir() { + return m_dir; + } + } } From 2ab60e38141f5ec2630b34320693af7618b9627f Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Mon, 17 Apr 2023 13:37:44 -0500 Subject: [PATCH 07/31] Added some basic Suffleboard stuffgit add .! --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 43697 -> 43747 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 23375 -> 23411 bytes .vscode/settings.json | 3 +++ build.gradle | 2 +- .../java/frc/robot/commands/TeleopDrive.java | 6 +++++- .../java/frc/robot/subsystems/Kinematics.java | 8 +++++++- 11 files changed, 16 insertions(+), 3 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index c6a75a0615a896910c2d7f7ff589bf88573817bb..a2c2493ef2a9bf94add7c34e0c31cb54637bd18e 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~j!$1pqNy1Zw~Q literal 17 VcmZQ>zc8b1qOQ9v0~jzL1pqNq1Y-aI diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 3a1ce65083f6d982bf00d98823b38d24f40d419f..4e3f4b447a05ea3f415a1c6d5e829ea437dfeca3 100644 GIT binary patch delta 15536 zcmc&*dsq}zAKn>SQiHFR6pRQFZ>0eSrg`&rV!@Z)=Z7tGxvUg-=c>5EyTMQAv?7NuVfKQQ-ucc+%$om+dQR{TSKf+p?w~ zRqQIv@yZ19wz)Mv|8G9JjODBPscW>@FZD=DADdQfE;TE ztO^7qS##0%i+9D*;eWlBQ8?tQ-tR7)id=HoxMI&|LDv$?B#D<5nk6Jo!WE7tcwQta zilIr2AVk3#nMRPFEU~C4i@*qHHknOiozG2>(x-}CL-+zcoi~Yj6GzgooEfxfO!l{jps63It_qW z7ThJG$1Cgg(6Da!=q?*p-=&k& zu3kFeWV@qBKE3A78frRnQTP~inEm)hFi`@FJHxU9M+*wea;&Jxl0vXJr%0r1$|>In zw|k4lT26z`F5tU$*?Gp>d~3V@JUaZupApVl#)om%h#y{9vpCCOFa_)f(+l|Mfnh@D zeJQU_o^iv$M)BKG5d~}+{3eLBhQQUjU5I}H%OGXYkI^(}N+?DGof%lwaY_;xMkE*s zEbc1OjARUfc)6!Yc8e3o2IeQ&=jk}dkAq%G>*;Fh&^@>F$FHTnn}S#crY{KiU}%2E z+&!&Z1|6NAz#(TIxS4w{zQa9zZh0?v(1=RJDlQ4{T^QGD2rc9;k1x_X4u9_UC2yKk zmGJdbjgPGAQNHzqdo1UUg7?kDfOhc>XoX3T5XE_F`H zT-N^U%?X>4n1jUZg&kXwF4qQAu(R*=`;yDPnduumb1yx#xkK#i)?niOFur-mV-s-v zc{J6xq*)92eTy33da!fPyw>y)LzEQv4@k^GkZ&x`@xg5sN>2D38Cb#!wR4Am5`5OJ zhTJ$~Q}4(2mGw=lpOR%6)_DQ27K1eu9>P_aSLF4GA@{nE8=mw0fVl%VyVqVZsr?+@ zYM!_Yh5pfE_U?lRDlT!Ai1{veUdYSu7yZ^_{h@@hZ_fRE*3$=iB_meRZ@zGJiwncu z@JLDl?{zF(4_Z@J;fOziN>%ST&XDwqocK5Q|M=Vds2zn;9VQ=DVX?Qb}# ztG}H-;xpHCOLvd?4{}L>EKam@?lmcCE>9>M=&TxH;->h?N$YRjP}Mx`pw)SGZlv}h zq?kG2K8~b@wjE^gs@B&Z@0_({=z;jA^zw$-S+Btaw;gNyyoHIudnbtDRGdJc%=*4p z1M}(2Zo6o6ufBv6!w773iD^6I8>gaaslol|yQkOoTYX1$Y0{NKM$gkVsg{?rbK_j; z$FXYs`DgXqQ>kE9Hw|z6;D+~HHAlzd4JVu_y)9c01!Oo!b#=;;fbjxDVi+coPLYrW zoMuRpRzx1-6?MdEBopS?1_&&-Tmt5IoTGBZE^oUB-``l-)LB@SwI|am91PPCf(swM zp`F*G<+@)9PAgnHYHXm=Pk1Vm#(iK`&U5cDq_ySV(|96}OB znWGpP9QOgCL-Qg>kTQvLf}*aUfp#AR@pit*>nbSW^MVO-?dVhO8aeWl%%0;8oZbKZ z2IILH3KS+qk)R=kCj$xqCPzU4JxMbkeeA{R9fRZTS=GH~NXOTOB_%jOm?RkhLtSG! zCCE;Va+7hwiBU97kc`M-qQG-FhEXax3+)AyqTE5F?sW-wdR-G`ukO&9x9ex*?tF8> z;_~gcY_**o5Z)GHAYo25CP+i!(Lf^LIhy2U+zI?+2;3BiM zzs*Kd)Y>ZaN_{K+xp0!_6-f{oT$Tu&rWuT4X_93zAfxhtqr81Cn$fo5&&)-y>#S|v zDGx21Uq9#ZIej-hG5FU0Lm!Bsv@RY4MEr+P#4{9G3fpq4qh|0HxgTnNTZz!#wi#pNirt^I+!9! zTmZW_Ba;fNC{97$28@Ow90op+J80}kSK<~%g>9t$u7Z@+J# zM+z*fbvQv035PmmUbV9O@|WJ(8Zk8>fMJf|LH`WEhSM=mlHfK4M)3?s5~3`KVA2RA z%i|c>8AXm%hgYKA+b+9Vm1w$NcE_g_NHs;f&b&I%`Ox~`k2M-k;4#1eWf!`#keC6C z6GdFXS()TOtSRBA+%t5c%X^-Jf)s z-G0#M{61yPx4c+ljC>3*fYBFoALkB;sS$A>C+a#*DDRi*hD@hoPt*mOf8_3p!+do` z)5?8U9KKGM$S>C=8h=~vFyA5xA8Rh_B?hn$I3lld9757VXU5BFN+2+X>lALgcAdf~ z0$aYt&>v!cgf0>|%$FF#=lu(~SpajYG2Se2m@gI>(Jc$mFz809-C3mfnrMCX60#K}<5};Zbt9;Y0+qdey@t8<2~5z6KBw!WHd)-g&o zvzXFzPPvrSkm}?x%UdzS(e$Cy{>+dlfxWbB6xp?S!f7% z%us;0L9EJQA;4kQ0SwIn6sHa2Uyx!BbiQVy-Xe<*<6sa`Yx~V4dT2(&P|h%WTy7-G z?J$$vhLFaJ8rII5W(wLcnPb4?Y6xbk*hpu^hS=5Smzmc!Hi{R|j%t3G3E0?DnHqwb zEj1uO5sWbR41xrlk=t~bsZC>wAc#Mh&LW6wApRU?!qL!K#DJyKB5EcAL-b8dsWc7A z%-R`zi2?7WRk)d2Q;W_bHz3=!Z{pMgXOV=mx!}_y!;)o-TC>VO+66v|`(M+cc%;%W z*1w?I9Of0Z{@>d0bql*9v%%=l0n`?C0^Rw7HjwzXX7-gxMQCX7wdSMpo6l>bcYl(L zEK)0~(B#Zwk1c8$YTB(0s`&Iqqz1fDo!o*Zmuch7i^;V8+PKXto<$H94E&oavqhEJ zwlB5cOmB=w78-tHoDjCC8BzP?u6CN$E$xxTI#sJ%V@~?=ue3Ms|GWdT$oOWj$NzEy zY$Ud*JZ{r}wE2%j^N$|j3q0<-9@c+kF60xp>P|e{+7Ve`fHMBXyk2elLJw(80-~y0 z=AfNMYQ~tC&1!wn^mjT$#AcLBEb(|p-tV`?yiA?af3v-+J)&yg`00@`f8*D%X9tMP ztM3j(<^gjfI96K}$vX3WZE!{1=48F{LX49wigG%AdZ!lf9S5({3r`=J8~7Z<7DW-& zmTM7scIrZZu>{Up#tzyR)j}Uz27co!JBitk)oD*_PG;MiW@}GJPLlTbmugQL&Dr*2 zwc69j<}7=8rS>$n8MiN2wWp2EgniX=?df3@5y1+zEkwp6zCv2WETif>Cxr!q5NyLGP? z#oguG;`P{oZVh~t7@U-5LiNaZ+TexrXT~9OK!U*Fz_)8QGvS&3=?X38EwgH0LiA(? z#tsd>+DJ)ijAZRk+W76Kt5WoAg5mz#NVce^yQv=dg8g)ItFQKt9%c1wwEwO9o(Ni` z-J{VbgSp#0u~r*d@k6UUXi)gdjV-DZp4q64Uiw0xIGZDH{*4{qURM3Xwhf7i^X4DR f)jk#;x1S~-eafXapGDKu@n_M5wVVDze;)jQyO#`o delta 3198 zcmZXWdpwl+9>;q;YG&53Bex`EXKmIoLkv14?RFt$QL?QqG;(Q%8RIfYTB2jSoLcus zrHn9jR+2+5t*EHbidH5a)VS2iB~ebbF75d}zh{_d+xg>p{hs-}f8XEt_kF(K-}iUn zg!+XO>Q`K+K94 z%ophLp1OMa6Wi}-<8RKp zB2Ytth7TlKD5JUEA^sDu17)6FY2JR-ofeRgAS2j=4=NVex`k+hHp=LtZ&@WuU0?U% zS%@qSQQuCMN#l4uQGh?n=hJLk5OVyma!MbqaYXwN81cQO* zft1+)x!_rgU0t0-3*p|0JhvLbZ~?nPsAW-;E`u(*K^m79Y+7_%5Q9Jg6KSG_VD6~r zl?xsI=8wYCKbwutinB(bfIsqybehNg29c{} zJKDmv3+sFxsZF-4J;I@ga8wYbmJdT|e zaZ>E+5PPS(b{)HW;1JrCddpI=B{~UGMC?EM(51lE_;(ehGrmFZg2S8}U5e*ofXnzd z$iYr7MKwSWoNJ|NyJmz254UcPknE!d!uS*FJf}v4gJ)(ubt2)gb4HqN_Cc0Yh!O?G z4L6PTSbKpDVnUxo3Dsz2mLwmRy`FWMFxAWC(9q)wPUr1pjuRa}3kJEp*pI+{DJ zMAhfADF+i8LT@Yoc&?+?+8Ob06ZGXzU+w-?;R3D_-hz?BTZetAXZF064sX-XS(Vi9 z+p0v>lu^cdj9|DLwfL>|mZfzDdkc!48b?*;!iCGkk%M8W#wZQsVohI8rtEbdg?G%t zgiRaiCCjAoA|e-m;+P~l=C(Z>{9)=n^9%P`GNBAwi3zGs6iiG(R(a-7l)E(P`izzS zcO!~m-~g-|7-R5;$C%%dBat3?@Q?Eq@w-D$tpnvfui|x@gBdVU8ez8A>T+;-yCpk6 zZ=`1ESW7c3Rsw)#=WseOJ6yF!Blp-K$FBk_re-}~tYqo<0!4(yph8aiS5eD1B>nb& z?XKqzUU2cEIUg?qI}=CG)f?!>m+q(;aXiX&_$MSTLeGq&c>#f8^Vu!0?`)J`Dy~fM zxO<;wmjGH9?hl;|eKZZE8_J08W9mG!`yuhm?DZyxVs$b;o`&ZnO!Z4&z~t$4eA{?- zD9z$_$GY;h8^?+i8G)tNrQ^;FL&Q6kkrSGB}^%}_oO4C;}W z_Sp?>!c8m+@2sA`6t-3ZfDxI)1t2Vkzd4jk^9}ub5)<(oPDk!3Spnz`;C8^q4f726 zLLO23g3LY<)5~=lo#}i?Ewv_2SKk zh4IZ{>(9s&-kMEmzQOEwyRSJ|5CNL`yi6J^TL0~%s+4l|mlb#IguE}7rF`Gy+Pj5qkO+zan#T#E_0PTHt=~(hlYR76x`$|1-Yz)5A%{dq z;x{-Jc7lzD=@CKlc+#+QKwK11d_%WR$d>&AuW|&TiWB4sFgQb}X`fx681SwtKP`Ro z_?qNT699qjVWS1$bFEKHp1Hdv@n_S0lTSpW@iYVq_(MV*Uu!LT-KdUT_i9|*D-Y>D zb%%fi-~S+Sk1V!A#l+QTT9>lYXUk(tS=?BsDkpm_LL%_}D_>*5{(HL@Wp4kUb_CsM z)54PKGG7uM3(hBa_3>YciUGmO8xo&OKZ5z!(5 diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index b8649077433f826a783cbe577a0325db5bf18927..ae90e0cea62e4450be41b6f65f2c36a0e605f3fd 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oL@2LLmQ1i1hJ literal 17 VcmZRcbt>7YGeh$`0~oN(0{}CD1h4=A diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 207f562ea1d659dac59a60efb4ad2844c0e00e0d..6fb743f4d37af027fb94549d2b20623e1d797f3d 100644 GIT binary patch delta 557 zcmdmZmFe+SrVS<%j6XMs}6Pg}|D+?(vl00yfzZeD73TYxchqT-6pjvgjrEE9^^ zUQe!!(*rW3x+kv&GdS2M-vu)`nJ4qdgQU2HCp*UL@k}Vnat51o@!*BY@xiLR?LaAa zun4z$#$?91!jlig3j@__I8R<5DmD3IyaulR$RzPoJ!x1X5SV&_pr}yavYHTZu>VVs4r!Omp}`Y zp~PKF9sv)WV!20doc%$Im@4R}Dc20D`nfrd9)>9c)))rNIAaJJ@X4T@qQXyGF17Ln z?vGL^*pLwL(*=TK_;6W}cl}9%Z0~oOJ001;i1Ofm6 literal 17 VcmZSnIL9>cl}9%Z0~oNe001;a1Nr~} diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index 1df07ab1aed68e755b96a1288f61ee304a2f9243..d7bf9f468462d2364d343033993da26c0ebc81c6 100644 GIT binary patch delta 74 zcmX@Vjq&p~#tkMCjDnj@C2sKZD@?aD4?8Ox&ES WY0bg7a-(8~*k(r$KH Date: Mon, 17 Apr 2023 17:32:19 -0500 Subject: [PATCH 08/31] made small changes --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 43747 -> 43747 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 28289 -> 28527 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/subsystems/Module.java | 8 ++++++-- .../frc/robot/subsystems/RobotContainer.java | 4 ++-- 10 files changed, 8 insertions(+), 4 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index a2c2493ef2a9bf94add7c34e0c31cb54637bd18e..fb402cdb3686314b32b379ddcdf7f37aabacb63d 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~j!01OPE#1aANU literal 17 VcmZQ>zc8b1qOQ9v0~j!$1pqNy1Zw~Q diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 4e3f4b447a05ea3f415a1c6d5e829ea437dfeca3..33dfb19024665545c3ca4b26b1aefb09cdb0bacb 100644 GIT binary patch delta 950 zcmZ49%e%ancZ0u&Qd)9(c0qAUa&}opQc|*Ml2K(vl4)wbX-+Bw1aNLH^x$V?J+<;y z*U!zZo{aJ;1hp_TY@hBpk4a)OPp1Clj~RPe?}0QoZ_eDlIg@dP4?#Ow^|vc#Fiv3J z&by28k(z+j-lPi0lR^%E+fE(IsC=fs{r>~Ty-Wf-;$>PlZ2M*XcjAQmt2UXxo9uW* ze7dh26ZiCYj~R1>{xB7;m-^LwW>-dMUGRxQLB@rX8B;|kH>7fJ?|H%~!zi@6>i7L$ znJTk#-(HgyXvhj@TnSPz{rpqLZvt2RG_0mdEL|Dj$96(@ZpPW^jW&#Z)03Yw27AnV zezWVR!A}LoNg=5@srdyiMVV!(ddWG7#l;K)E1IvRUH4YH${cd{l*A(OD~n!%6z}3= zW)M)kyY!6mRY{JY78lOCKespE{_i>CKStJLAQjUYUoje)Z3COUM38Z!cV=E{ZemGh zaxu(c#f}$A?Qg|7W$g9MPbtlTJ8?(Yq$01#J&k-a`}X8~f9UrPtnQBi zvck35$Zm_XYs0J4Cugm5XT1mZ$4eogKbnH#?mo?zN}2d1_T$zwZcBke*A+rzQ}woL zIr2}-{r=d@S5=m zqxjd)nN|*g|2^JvGk5DGmN)gi2D$s`cJH^0HSPqHis0(afg40ww*RwW;$X%I{>g?( zqSNm_VC0$p--G#!rfm`eQ|lyH=WosQt0@n7{w9iFX!^o4 sj4qq&@81)Z;9txfKimAX@KUx_41XByt-2-vGmgQuiQ6+OnC#C10I-mjRR910 delta 949 zcmZ49%e%ancZ0u&QnHD8d1hHsrg>UwW=cjyc5+HVwt1d$NlGRI1h8x_^x$V?ZCJMZ z|BubBo{aJ;1hud-Y@hBpk4a)OPp1Clj~RPedqA3-H)n3&oXNPtM=3e0B(EYf(_Lh%FJ)c31m6t7QqJR&~b*NusL`n$)BIYQHHvn{>eh6kOB3qDl3 zx+RWr;bg{C(a8;|+}nGeFv>6r`Azg@lj2yI`P0}|OfKDwhjArH!SwS_8NUg{e~T1% zOxhHj6@5tU#*dEU(;ICV`=%#9XAJgOb8+YY9|k`a7$=3K=A`BqxD;iUrRpW;Bo-Gl z2$X!Y%ThfV^8Ms2uf5CcqEUI zGpF0QGfB8G2z13V?F)Q*Ix2R5xzkzM=ev4N&IhWP$I962o1ape19#%5^G~-cURT@X zT^6J8Mz%z50a)EvJ;rI%<2{%}r{4lPCFNH$7vsT(l{VK)l*?vjb+&8)`{Sh$&>yqf zz9}?BP1AOjOw0cpBDoJJ)V|-8%|da{|DT^v%nDj}<2X>Le&bJ_*M^sii+iW`^8K7~ z4k$GBdRz}{0cS+a%lnUxPQNL*z2G(D5k_$dD?9|761?(dl;|F!D_QZ_UKI-NuGVnM(jYD5vMUF)b59 z4?f0q(B#54-N~J)RRBHMCjUiVBuoT5V-QSLpb$f;< z(>4i#My=vs$CdV+z87s+{>Ab*!}Nt`7+p5k-@h*^Ubs2*lvDk*-J7CQqSkpnyzm(m O%WHbKXH+oRp926+G@R=I diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index ae90e0cea62e4450be41b6f65f2c36a0e605f3fd..71bb74439df9c4736e0f549df7d562ddfac092c7 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oMe1OPL%1mgez literal 17 VcmZRcbt>7YGeh$`0~oL@2LLmQ1i1hJ diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 6fb743f4d37af027fb94549d2b20623e1d797f3d..2b91722837b0c3cca945d9fa7822cc6fe2bfdf8b 100644 GIT binary patch delta 485 zcmaESmFe+SrVWoH6&5k2pJd|yv!$)@4(ptce2WJTyfePef3!Hsu-n?udB4#{lSVFUv)zi2^mawomHl=qWN0db#JAs%prGA zNh}h-0(G(y)Fi9Pw`YMoSUhiX{8FjOfwS*`INK&0&XH13?0AvXK6m2tUlzsX+|Qqd zL$sEJL9`0GZO)#vk5|I6u+iTAjcnzU=#7%=%9!_qRXKdQHQ8~Ml-CN^W+S^T&aMrw zPM@5$&Ycx%dmO|F&P*V!XAC4XfmjiQ1jq$clQ Gr2+u;}7_@|C^*H%Qp#6u8K7V3Gi&*7`w(=q2TAjm$#nQ`2P6ee?G=$X+A{1GY7;X zcKOM*Ggl~lThO?uF>w0vyRUA2D>LL^fhvNiVGtCZ>^iGVq2!xgmg>on?nrm45(E&0TD&7Qe;7g*KBM=6sXS4mCQUnyb+481K7Eu5J^ rTF)3rXacb!2n%$@GVKd|dO9k0f4S3H+2;`L&!6;6<_Buuze)uFg*dgc diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 804c94351c89d1b7fd55f483619e2efc16dad3ce..6e59c91ff8fde1b1980eaef2f32d29c52a447166 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~qk^001nc1Uvu$ literal 17 VcmZR6A~{9FsWEOA0~qiG0st&M1JwWk diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index e4a9d613d1ebfc94e47f62902e011f7389eb069d..59b50429969fc6bc0b3f91d66735be95d8a8ad00 100644 GIT binary patch delta 416 zcmZp?%lQ5t;|3E6M)l365(aDnTUhqR)o@?=$N&a;jhkcT7Bcc1ZaFThXK@!InD4v! zt2`$&|2F9*+3tsyK?ONCe^t84#GlZ%=E8w#rBLlhHy>5iWaK|`^G}0jV+vI8`R1o; z7Hk48n{_tSGoR^#iZ0$Ps=Ij>@b0;4+n}auPu^5mN)t~<78%uW6XyYPMTw?_`_7VUg}r#nOzy3b-^bJK|YiN0G=Fp1^@s6 delta 59 zcmV-B0L1_A-T{H#0kAX}0U)zA7%&F2eIA(sqb?u=vqmG(0<$hAAOW*xCocxGtu3Gh RlVvl50TH1g6|+G|Kqh7v6H@>H diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 887033b078b65b8e3c539494753863de7cff0c6d..9b31170f3995d4d7e4e9501cf9d8a6e670710d09 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~oM50RS|E1ULWy literal 17 VcmZSnIL9>cl}9%Z0~oOJ001;i1Ofm6 diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index c680c2892c5089df1dfb9ec3218808ee7fcc1ece..9fccccd814ee491c36aa867a36919f43b046a1e3 100644 GIT binary patch literal 8 PcmZQzU~Hdgtg;IL29N?P literal 8 PcmZQzU~KQdym%G>2}T1b diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index c0f6d7b..3e27608 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Module { @@ -32,7 +33,7 @@ public class Module { private final int moduleNum; // ZERO INDEXED - private static final double kP = 0.04; + private static final double kP = 0.2; private static final double kI = 0.0; private static final double kD = 0.005; @@ -58,7 +59,7 @@ public Module(int module, boolean inverted) { _CANCoder = new WPI_CANCoder(Constants.MotorConstants.CANCoderID[this.moduleNum], "canivore"); // _CANCoder.setPositionToAbsolute(0); - _CANCoder.configAllSettings(returnCANConfig()); + // _CANCoder.configAllSettings(returnCANConfig()); _CANCoder.setPosition(0); /* Drive Motor Config */ @@ -146,6 +147,9 @@ public double setAngle(double angle_in_rad) { pid.setSetpoint(angle_in_rad); + String name = "Mod" + String.valueOf(this.moduleNum); + SmartDashboard.putNumber(name, this.getAngleInRadians()); + if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180))) { this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -0.8, 0.8)); } else { diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index f283664..b3a239f 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -62,14 +62,14 @@ private void configureBindings() { public static double getLeftJoyX() { if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { - return -1 * m_driverController.getLeftX(); + return m_driverController.getLeftX(); } else { return 0; } } public static double getLeftJoyY() { if (Math.abs(m_driverController.getLeftY()) > Constants.OperatorConstants.joystickDeadband) { - return m_driverController.getLeftY(); + return -1 * m_driverController.getLeftY(); } else { return 0; } From 7e97afe4c425c50bb18682af6b28165fa2c5784a Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Fri, 21 Apr 2023 11:43:03 -0500 Subject: [PATCH 09/31] Added decelleration and fixed resetMotors; All is wellgit add .! --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 43747 -> 43747 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 28527 -> 28799 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/commands/TeleopDrive.java | 17 +++++++++++++---- .../frc/robot/subsystems/RobotContainer.java | 4 ++-- 10 files changed, 15 insertions(+), 6 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index fb402cdb3686314b32b379ddcdf7f37aabacb63d..3ce20d98bd713597c50b19b4116024230191ff63 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~j!01pqN)1aklY literal 17 VcmZQ>zc8b1qOQ9v0~j!01OPE#1aANU diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 33dfb19024665545c3ca4b26b1aefb09cdb0bacb..c05ddfe8f6a998e90b77b4c8c6206d2b4ce8aa76 100644 GIT binary patch delta 609 zcmZ49%e%ancZ0u&QiXA4d0B2@nps&|Mp1TEZeDU?MMiFRVslNnP*r=Ney_)WmDqp$I%(*9kpuT8={j7>eJH`*}vO;3K#SfHIJ zSn)X^^D-01g2|b0R|Gq6n6_ZaFN0tF%nSm{-f!;;_w;{!HTl+xv{$@Ow*Pz1_>YnG z0Z7Gm##fBoj43RkbJ=>^cPjASI(zP4xNOUr=^))Ng@C%-`;Gafzcft}ar>~4>DqM% zpwL{!pSLb@H!rq0eDT{k$%!CUJ!~iBj1OO0yS2MEqA%6{5>P1e^)2Jb?WvZtuC3{x z7G3add%m?}E`{8Pqn0$2Pr ztfopVT^ZlUc0zY<#@XqOHjI7Klb9#mUi7+=_+%`-BS{a#IG!Rb+hZI!7e^# z1_8ypOV21@mE`zoapA1{b9?jc|DH4cV`M!BQn8)!6(cue$_m$JBfBlmt_`nFpPaSM zo%J4A_e&w5?xvu)yHE3_QYJo${kZjv+fty=b%oH_RK2ZQj{MUyd4hk`0)?(#Jea9f zarKqZk^8=lw>GK(g$jDEulpC-`Tt=047U>k`&G6VykpM{sQtz!7YXm8au0T}BB(@`2LPJY0)GGi diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 71bb74439df9c4736e0f549df7d562ddfac092c7..a5ef79ee7f225473b0913b47b87bef5df831d6ad 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oN1001)i1SbFh literal 17 VcmZRcbt>7YGeh$`0~oMe1OPL%1mgez diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 2b91722837b0c3cca945d9fa7822cc6fe2bfdf8b..d8886de03a04edcf71d27eb0c810ea58b8dfcfb6 100644 GIT binary patch delta 245 zcmaESmFe+SrVTrzS(bEgCrsWOEhVw6ZPB`x*^{*07c|Vf#$3+Gz`)oJl>9LHeYB54 z#p9}j;xhZ?)9p7ieYwV60FjhuVE}=`lcJNOW8MLcn&mS2f0NW?`6l7XRk6k(fq=~$ zW7k+K9Nc6sp*iP4SVbDA_Zjn@MPU6c5qH4)892^tuAOPesE{XE@i`##G84yw$(e6g z1Uo~WU;t6Y{%iB?S$_NycFD_jclv4H-#p`w+|$D|y1}a2UtgH)xJpWbC3G%ZZ~IOK U-dktS{R@|Eff{jUa`CDH0Adzm3;+NC delta 245 zcmaESmFe+SrVTrzStf>+x=r32EhX{wbEcI;;D3*|+|1oNiRDcU42SSNTKz{D4xu0HWT!xC}Z=PznlaW8e*K@(4y*5z6?~`4P7ct3H zZd9BhyxGx1Prcq4C?g5Pf*>r=ysPB%qArdzE_uJLm#{NE0}9Has+e_Q@h8zsBHvF5 z=<+PgySormMOW(1T8}NaVw%4HKeqDiSCG|cX0%O-jaIu9CDbOqu3oWw(zCUgN_sVv z?rfU0HEdVJXWs8!4U;fc6pQ}+_}WrfBVqlmKf71fIAW?u__MS9Qme5DTTj3Y;hFw# aF;(zq3&}I333Xklf7DPv<@RbXkP-lCgNRH3 delta 64 zcmV-G0KfnL-~sR60kAX}0VlIH7?=mMN(02p}y diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 9b31170f3995d4d7e4e9501cf9d8a6e670710d09..a762766e3618441207a86a3a8cb51d0b659b076d 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~oMP0su7A1c(3t literal 17 VcmZSnIL9>cl}9%Z0~oM50RS|E1ULWy diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 9fccccd814ee491c36aa867a36919f43b046a1e3..6a782b28e812ac361a31e8f190e5422282a9e370 100644 GIT binary patch literal 8 PcmZQzU~FGfEFcH~2Fn4# literal 8 PcmZQzU~Hdgtg;IL29N?P diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index 334b1c8..b0dafda 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -27,21 +27,30 @@ public class TeleopDrive extends CommandBase { private int cnt = 0; + double prev_omega = 0; + double prev_xVel = 0; + double prev_yVel = 0; + public TeleopDrive(DriveBase driveBase) { m_driveBase = driveBase; } @Override public void initialize() { - m_driveBase.resetDrive(); + // m_driveBase.resetDrive(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double xVel = RobotContainer.getLeftJoyX(); //* Constants.Swerve.maxVelocity; - double yVel = RobotContainer.getLeftJoyY(); //* Constants.Swerve.maxVelocity; - double omega = RobotContainer.getRightJoyX(); //* Constants.Swerve.maxAngularVelocity; + double xVel = (RobotContainer.getLeftJoyX() * 0.45) + (prev_xVel * 0.55); //* Constants.Swerve.maxVelocity; + double yVel = (RobotContainer.getLeftJoyY() * 0.45) + (prev_yVel * 0.55); //* Constants.Swerve.maxVelocity; + double omega = (RobotContainer.getRightJoyX() * 0.45) + (prev_omega * 0.55); //* Constants.Swerve.maxAngularVelocity; + + prev_xVel = xVel; + prev_yVel = yVel; + prev_omega = omega; + SmartDashboard.putNumber("X-Vel Input", xVel); SmartDashboard.putNumber("Y-Vel Input", yVel); diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index b3a239f..964ddd0 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -69,14 +69,14 @@ public static double getLeftJoyX() { } public static double getLeftJoyY() { if (Math.abs(m_driverController.getLeftY()) > Constants.OperatorConstants.joystickDeadband) { - return -1 * m_driverController.getLeftY(); + return m_driverController.getLeftY(); } else { return 0; } } public static double getRightJoyX() { if (Math.abs(m_driverController.getRightX()) > Constants.OperatorConstants.joystickDeadband) { - return m_driverController.getRightX(); + return -1 * m_driverController.getRightX(); } else { return 0; } From 40e9f0f4eec343aa76f0b5f41300eafa6425f32e Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Tue, 25 Apr 2023 00:46:04 -0500 Subject: [PATCH 10/31] Added wheel autoresetgit add .! --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 43747 -> 43747 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 28799 -> 29343 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes src/main/java/frc/robot/Constants.java | 3 ++- .../java/frc/robot/commands/TeleopDrive.java | 14 ++++++++++++++ .../java/frc/robot/subsystems/DriveBase.java | 17 ++++++++++------- .../java/frc/robot/subsystems/Module.java | 3 ++- .../frc/robot/subsystems/RobotContainer.java | 3 ++- 13 files changed, 30 insertions(+), 10 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 3ce20d98bd713597c50b19b4116024230191ff63..135d559ce46450f1530f223dd47359125e1584f6 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~j#h1pqN?1bYAg literal 17 VcmZQ>zc8b1qOQ9v0~j!01pqN)1aklY diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index c05ddfe8f6a998e90b77b4c8c6206d2b4ce8aa76..e6238e5d7ee89b6cbab492040ede4ed93281d64e 100644 GIT binary patch delta 1386 zcmah|ZBSHY6y9@TSp!!t7Zw)eLw1RgGzCeqVL@1sg#?7noD_`61(pHiTRvn0HCzT= zBQN<11h{}<6tJ+xWdsB#LdF<%GSNUj%lIfo1SNry?p>RHtUq{vp65B|yyra6sa0}o zm0Wd*zy0F-cjhSC6ep_BRIhKSRp;B~5++W+z-N zgTtbbcLl~L*T!xoXXPF2&xYyTlbTpc6APCE1(~Z9FgQlp%b`e*;$DGqN4!SAnWr2y zxW4X^hnny7_7=1<3i-H6>@%0YSvD!}=9DO&oW5yD!bvNtVGI(05QbESB|qjYbk6i8 zoVd2M3^gP~@yHF`hE(A1unq`?;K5YqN2ZP=s-7~3Tc?zu(h~BbjHokzfHe>f_IIV; z=H}JLLA&cV4huKKHw*#YdI-<)&|dvbbyoGGoH@@VAA{Bl=(q^ZqQev5$ey2i`rQ&b zI{_R0)hc>q=NB8`+`g#zsKg}Sl-Q$DTMoxZrlzv+gabs132{59KkS}Y+&HWL^5Q9) zT#4(k@WsNpV?SKa=#SnSFWfJkX+|$6z>@dVp^m45xgDaGe(FcQSwco%4P6~=o`koo zo|r8Jm-#GSGmK;{8;|oGwUF}#4pfqnZMIe{JoTNns<@t})_aYcWoq0@`rv zBle1}ZY>l%1&;ipu&U23^1g}`?rBVys=}Rq)z(qGfsR{&DXOFkt$Ee>gaMlM09*Ft zLuOY4Qy^eZGUFqdroU5Ob|COdVRe}MfXchq{ZHNl?Hb2Sk^3|_7#3)Y_6VQ^i6j`` zmxVWdn=v6x*`_Ep@XO07ITX@W#SB-$24zs9EIt1JjfZske!(((-=jq?{>3J4(&Hft()^v_eZ?e$YB! zaPaZzBb<*1)!a-vq-coPHzUrp;<@>RyOGj5Iz$+GyeMr9KDfK#Q@i13#XWSWbnxmX zNQr2^VHa#&<2pA?CH)26U}3m_(NoI0Z(dP)Cwp2sBUXRSw3?vyJc1m_s<&b?q6t;= z^r@FSu{CLbXx-EG-Ae3vTo!ZZg0eOJ%qfxuyR7zK2S%FbvRLcZ>&@w z*EI#{-Cd3%{IC{6SAP1d%%$f=86dF)%g1$PCs$+^G|E9;5{ykPN-$R-bfuDN(MI1F>oSL4WxE`99zG`M&Sx`#ufD zHVwr#Es-H^oDZZ~sggwBtVBUZa!zVmd~CKLH90mpmGWenX(S=GK+|44UuV!1gkmlp z&pJV+kSIWfu?TD7%fzhUJ6fOdtXv}`H9}|%a~CD#yc#FnB@)FYANCn2cz4dU%Pk$Z~Hpg*S<{Q$WjsQNBaMfCpVdJnF7%P~h+0h{HZWny= z(UI#CJE%1YWPB^K=q@+`!#3FkOJo%G<&Md6&kTQk0KPN|Q11jR;65LhV!gAx=!37)i(=HAsMR5A(MLsXMPh0ET{u2B9KKZTzz4$2T_@x9_xVly)- zc$!`Iqe$UR%%X-6&NLo*v!$Y@&fsmZrBd+vzsbU5p%LT1#_Jjrr)=(`)k)aO{LS9^ zvAsu?ZYkL7o1q=TvTI%PwI#Ik9_(-ws0!FOe3q&UdrQgtbEjUqY%F`o+w!KPYouTgKqHdF5#0*7n!JQRqf9Xt=_nm_tz68^7bALdSsoJ`Uz!fhb zb3Nd+N|Tq99=0(H!*|;S&+vZ`cY2jcm2J07UMO4-o~#QN>GjW5o_^<{HbA!XhPgm% z>Z90%@*Ke)om+2Y@x=Hf9nEsVnO3bdveN0noi<{8K7y(B>v*x%RblKC!@0_blaHmh z>8&P=b#`d)6g+P^c+Uv+M|U`j{CF5Eh=NNM7h#SY7!%r(C1a16`Kt9sg^C8nhtdUaV+y zwbA|DvwCb>em62Eq-U%rq$uUsvgb~gZwY=-iw)k|x4xxxTh{ek$-mgs)xXfQwaln0 zqF;CBbPZg%boxY83EzKUeJgre$B8ESuT>2;l~%kfULeKOpaqBeQ|HzBpO?SLtC6V& zsZw}gvYH?d&cKJfXU+y9xXI|H#t7``zGB0#G|$Ahs}#zy3zemXpK6v)F0@QtX6wFC!3#y3aT zco>Z|@GuTAHsXb^{+%yd6!8YoNI@vp%EDt*7K=7YGeh$`0~oLt0RS^s1cd+q literal 17 VcmZRcbt>7YGeh$`0~oN1001)i1SbFh diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index d8886de03a04edcf71d27eb0c810ea58b8dfcfb6..e082e39cc5d505ba67e6bca623f6e8f3d8c2da93 100644 GIT binary patch delta 832 zcmaESmFe+SrVaIx3RW@^-(pw3d)hW#9lzID3an`1^b&|7+4{+ikwQg+LKl3mMPGNwe#EdAtZ3n04TvHR`N_9u0lmLCV8i72rBagvXP;1*J3G>6 zSJ&cb>st#QPgp+_g}5x>4nzm%!^xI&)Odhl%>fY*bekMFL)bHR?!3;V|)LD?HUYVz<+k~{74stMNH`@ znfU)~X)C3{HW#^m@YsmZ_FMR02~S=VB@9w=X7b7oJ(m8<=jKelTP`)3KiWuPN7$qyugE=(d@}p?Z+~KR{I5J)L|$P;Iho zjGRKnR#kJ|U`baFe-&=9~v%6=|H_XUumN zflY6TxC3zp$C=5sGjAzytvteJnAyzRdgX(tG53M*U?VO*N`WY1EuUOI%S9niu;Oz- z=4B?11(P%1t_XI9xRcueqKN(1=TnYmy3V|woO)CE;ZSE zjtI}hu#zx{azVGrGncA?0{Xz@!%Kky4RY}DIStIfAiw@#v-biK#>xE4L?AJtz6`A3 z$mEp^L5xQ^n{O}lF_6#%Vnq-Z zU?4M PpvuWYtHdYoU!?*7ye9j( diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index c519ce8f980998f6054dcc572fd014f8d8ba3bdb..dc633c87a19f64d64de0f23c0c831efbde6411af 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~qkB001l$1F8T3 literal 17 VcmZR6A~{9FsWEOA0~qjX001l&1FZl6 diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 1308b43838860135a4bc2973ad7936ef7b1e7b8c..3bc6890db14e04b704be6061c30a25bdf9063a4b 100644 GIT binary patch delta 889 zcmezWfN}m)#tkMCjP{#NB`g>PI`5wiOvsKCWB`N0+|8+y8O#DJ&Y6EIzQ;8IDtdYI zQ5i=@{@L3YmYLVrLk0Uc|CQxn7C6~`ch&++ne|Xn&B=ciG6c>a+vCS4f6)yhSY)|5 zRxy&1zoS3?+o6_7h+ske=Cw)*OadR%-JIK=DN8~1drxLn-NE!4SyuZYe@5x2(!9-3Cl%FB{;C}y zp!etJ0iK0vvmgo!4sW*Akzf+=*zi6sr$eg{D*AM@uigSifv^>8=1z1dVuOk<-27Dk z0;52x-ddJfO-pnkqD9f0*BVwb@$c0v_DFSv8e7OdS=OYBN%rSPMG2wJjvjUz^~ONu zl0Xa!VL<_zqkke7&**ZFw-sU6+SpwM6qG|%!GB9Qv3{V5Cr*ooLF?l^xFH-cjF!^z5 z6{ZTaTCYE>(}V2orQ5o0RK$Y3jb@FC;?*EWF|oz`zOJh@?kVd`E{qW7511ca_>TSi z{`Y5cn-2c|Gg&cMxSnN`Xzy*d<9thPH%Mse>S3Bx>hfZ_cuA-3v%0Wz;#;;RW2#7E zo5fpb-SXr&KRjfK9&=8X^#-ICAY_<8O#MKBDz!VD!{cY-%A6t{p|EOio z{;;u8jt`*(rlL4Va5ro_dS{rQNgqFw3E%l&b}YWEi&o2_!btrt^;VCGlD VHw>~xmNhfmKXjktVqgY_901?Z_w)b& delta 102 zcmV-s0Ga=vq>N3&%u5CXG}FPH(dJu%1uvmG;W0+XgSfdU+Pu_2ff Ivq4BcD76PA0{{R3 diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index a762766e3618441207a86a3a8cb51d0b659b076d..e961a6fb83d30feb0b7ec4ad28d7c43f7291e8db 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~oM#0su5j1ONa4 literal 17 VcmZSnIL9>cl}9%Z0~oMP0su7A1c(3t diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 6a782b28e812ac361a31e8f190e5422282a9e370..ed2574f4578c4ffea9d752d86aa167207f0de282 100644 GIT binary patch literal 8 PcmZQzU~JzSx9=$c2>=5> literal 8 PcmZQzU~FGfEFcH~2Fn4# diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b8ac5fb..2dfdd8b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,10 +33,11 @@ public static class MotorConstants { public static final Integer[] driveMotorIDS = {11, 21, 31, 40}; public static final Integer[] angleMotorIDS = {10, 20, 30, 41}; public static final Integer[] CANCoderID = {9, 12, 8, 13}; + public static final Double[] motorDegrees = {92.109, 179.736, 344.268, 214.893}; public static final Double[] angleOffsets = {0.0, 0.0, 0.0, 0.0}; public static final Double[][] angleMotorPID = { {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}}; - public static final Double degTolerance = 2.5; + public static final Double degTolerance = 1.0; } public static class Swerve { diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index b0dafda..5c234af 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -4,12 +4,14 @@ package frc.robot.commands; +import frc.robot.Constants; import frc.robot.subsystems.DriveBase; import frc.robot.subsystems.RobotContainer; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Module; /** An example command that uses an example subsystem. */ public class TeleopDrive extends CommandBase { @@ -30,6 +32,8 @@ public class TeleopDrive extends CommandBase { double prev_omega = 0; double prev_xVel = 0; double prev_yVel = 0; + + int counter = 0; public TeleopDrive(DriveBase driveBase) { m_driveBase = driveBase; @@ -38,11 +42,21 @@ public TeleopDrive(DriveBase driveBase) { @Override public void initialize() { // m_driveBase.resetDrive(); + // Module.ModuleState[] temp = new Module.ModuleState[4]; + + // for (int i = 0; i < 4; i++) + // temp[i] = new Module.ModuleState(0, Constants.MotorConstants.motorDegrees[i] * (Math.PI/180)); + + // m_driveBase.setHardStates(temp); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + // if (counter++ <= 60) + // return; + double xVel = (RobotContainer.getLeftJoyX() * 0.45) + (prev_xVel * 0.55); //* Constants.Swerve.maxVelocity; double yVel = (RobotContainer.getLeftJoyY() * 0.45) + (prev_yVel * 0.55); //* Constants.Swerve.maxVelocity; double omega = (RobotContainer.getRightJoyX() * 0.45) + (prev_omega * 0.55); //* Constants.Swerve.maxAngularVelocity; diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 6aa8eaf..87524e9 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -7,7 +7,7 @@ public class DriveBase extends SubsystemBase { - private static ChassisSpeeds m_chassisSpeeds; + private static Module.ModuleState targetModuleStates[]; private final Kinematics m_kinematics; public static Module[] moduleGroup; @@ -20,7 +20,10 @@ public DriveBase(Kinematics kinematics) { for (int i = 0; i < 4; i++) moduleGroup[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i]); - m_chassisSpeeds = new ChassisSpeeds(0, 0, 0); + targetModuleStates = new Module.ModuleState[4]; + + for (int i = 0; i < 4; i++) + targetModuleStates[i] = new Module.ModuleState(0, Constants.MotorConstants.motorDegrees[i] * (Math.PI/180)); } public Pose2d getCurrentPose() { @@ -32,8 +35,12 @@ public void resetOdometry(Pose2d pose) { //TODO: FIX THIS } + public void setHardStates(Module.ModuleState[] targetState) { + targetModuleStates = targetState; + } + public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { - m_chassisSpeeds = chassisSpeeds; + targetModuleStates = m_kinematics.getComputedModuleStates(chassisSpeeds); } public void resetDrive() { @@ -44,12 +51,8 @@ public void resetDrive() { @Override public void periodic() { - - Module.ModuleState[] targetModuleStates = m_kinematics.getComputedModuleStates(m_chassisSpeeds); - for (int i = 0; i < 4; i++) moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 3e27608..c9ea9f5 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -71,6 +71,7 @@ public Module(int module, boolean inverted) { pid.enableContinuousInput(0, Math.PI * 2); + pid.setTolerance(0.0); this.invertDriveSpeed = inverted; // if(_CANCoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED) { @@ -112,7 +113,7 @@ public double setDriveSpeed(double speed) { } public double getAngleInRadians() { - return _CANCoder.getPosition() * (Math.PI/180.0); + return (_CANCoder.getAbsolutePosition() - Constants.MotorConstants.motorDegrees[this.moduleNum]) * (Math.PI/180.0); } public double getAngle() { diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index 964ddd0..ed5ba11 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -62,7 +62,7 @@ private void configureBindings() { public static double getLeftJoyX() { if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { - return m_driverController.getLeftX(); + return -1 * m_driverController.getLeftX(); } else { return 0; } @@ -92,5 +92,6 @@ public Command getAutonomousCommand() { } public void runTeleopCommand() { teleopDrive.schedule(); + } } From 78bdbb8cde428d9102a29511f38fa4c79d196326 Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Tue, 25 Apr 2023 10:58:05 -0500 Subject: [PATCH 11/31] Auton worksgit add .! --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 43747 -> 43747 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 29343 -> 29513 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/Autons/MoveForward.java | 81 ++++++++---------- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/commands/AutonCommands.java | 11 ++- .../java/frc/robot/commands/AutonLoader.java | 9 +- .../frc/robot/subsystems/RobotContainer.java | 4 +- 13 files changed, 55 insertions(+), 52 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 135d559ce46450f1530f223dd47359125e1584f6..def3f730b6db02fce190df6908e77c69e99b1868 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~jzr1pqN~1cLwo literal 17 VcmZQ>zc8b1qOQ9v0~j#h1pqN?1bYAg diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index e6238e5d7ee89b6cbab492040ede4ed93281d64e..6f9a6c6b04683bd1ae3a3e7556f153b83a73e639 100644 GIT binary patch delta 1166 zcmah|YfMvT818#GTu0$p)=?@j%Rn{^QUNJLb%iz_BM7pQNf`pdx@5>o8I00SoyKim0NrVq?FWDOem!~eJ$d7oDy zXDZ}OWlFqPZct#>XPMiB10~|1yialjnYqO!ImeD?h>tSDthWRO#k+E_!Kzcks^18V%aA1Q8DH zG+O32+CnQS>h(mmOTeXsl3U%5-8 zjYimtH=4O*BM9u27Ol)4RdK-uwHkr2>$6aw+n~o$;EA+G@WqGjD2Kl?K@eVdhZYJ< z@U}xcZMMw+-V2&gWE{qMgF$q6l^XU*0%grMv6UyA4(#b1r|%*Yc;icEG++XM{Qi$L zdSn83aaFHx)T?a;vKFdXdqXM?U9r46!=f_?aP#jke_wmWR)0&Y-4ohNQ=JoEsB?|4 zh52`>zTI%H>?uv1bMX@vw|H01hxRy?O}jbL6iZj$#D0N|2s?JJP6f?3PzArk5O{8| zE4V$8JT*1mlcja7Rcv3-n!m0u$AQ5RBv8!{z%R}xW^mnZyLzeiD%m);W0D+H?;{J& z*r;x)lYq6hD{v82=)l9;QYFNTg;Eq8j+L?2Rsey){YlV#DRiT_I*R~-^Et#SZi~IOaHBbGfIgDYbq-NyT&0r2IA#MxuSG; zb_g%P_26^VHVQGSidh|dQ@!Nyvh=dLOYC{Ec`+@;Nq<^K$2Hb++sYhR%osIVf>mRb J^GU36* z?dBFwCV3TtI@lSuPj{ThBr%yMQ-AWujJ+%^_j=bhZ_3=hDU)%TkE+?1*@TQ>tKP1d z!8n0=JMS*W1Qnr$T#6<2+agx`zEGI2^6S7Q#zoT`te9lC&$-7a$S4%<^3c2cSSs6} zMWj~$T)p*p`L#6t9ObZ}_nR1}P2XtCBsx9z1)~tlnI*kz zrx(9qwB`b_ejChSXPiGdakt#`YarPhAlWxB7}X?pxZ7Lo%h+GKnO!_(g4ZzopKB)H0$<0ZN{SbO{e7jWR@6z zzBgH}Lvy=>J>zV~0IslKc0mj`#OBSqT=lB=z0-zeAjdqfWnpAsFlS_%^!>K6%8IJ> z4`OS9=4cdbn!7w+P>B7|wap5aZKr@j3j%qXk5}9_KlADAA@x(HSHMQEeak5BuIjvu zV3HQRzd3M&C<~MArO5}k1WhjJXPbOrOX_q*H^%bm{?<$tLg;CSaXCC0b51{C&BQwW zzco`CMyi=?m?S!B2kZ9vHcW2J0_d?k-O-*&Y;wUwk?ryhOqyIm=t0i73Y4z5=X)`o zk`R!Pc4Fdn68T=7GvRWCGRxiR3(qjRY_7k5PgJ2s*6!OB-pVc8)8zt`OD4zcIRy&4 z6H|^egF?@F^Yr>^rU?qdhd37GPyV;ZDr#fKc3aCScc%UO_rqZ2B8V!NT3IH*S>P7$n diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 6938fb2ac6f2d14d0745f31020ef240055533987..0e4e424d4d4000ba6c06c58d89ec3227cc8a88c7 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oNj0su2=1egE- literal 17 VcmZRcbt>7YGeh$`0~oLt0RS^s1cd+q diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index e082e39cc5d505ba67e6bca623f6e8f3d8c2da93..a42af1e20ce5635bfdfe3fc78a940c7a25e8ee44 100644 GIT binary patch delta 648 zcmaESmFe+SrVaIx5>7>TcgHUGTd5JT;r-T4z6Tf>7~6L$*6~c99(jair{an~ljEbL zB!tWRMGelSzL)#Hd~cNHv;7cND@7(xjM4+DT4^(RWryD6k5MT=&hi(NBcl^ocJinl znY=ezNchwUHFtAUUd_GWZvTTf;!o;Ga-_2{btWJ?z zJnLD6KOaQlp$A}fg=f!Aj*fW;H2-SB2z6D7;*`$okrgudTs{7&Jq{CUIxzK&)i{G1+^ji^Ro6uS;5fPh-q*eVzWptF;-b z$YJuvGO5i6XYw%uy`VZ{^5t1R3fJyDOja-1GN1R^$*SXUx?DK9 zE5VFaIh$`U^y3JU2->a0xiNW8(v_%1-(pw3d)hW#D!o3-j z_eM)8aE1M{3u3q-HgDGDs#m@5ogjLH4H&?{?)l{NfohXwW8@TSWbM99;jP@VJzXw9 zxnyz-L}988SY6>MkIB(7?||m#{h0i}NoululknuKSYx0-KFj8fu@6*1?r_;WwRxJT z!l8nR?p+Usca%!(^E$-$$`oW$|HVfs5G&c8CVS6xQ7AugvgFZ)_o5|vubwE_-IYL9 z#OShl{Y(W$kT*0YU!LVN*?p-9(1w5wljE04O%9xW2gJEE*>H{?571r?h{=L(lLKc6 zPp+FI404Rmk~b&t*VG4{lC1 zImE~$SGZAef#7CGk2tM*W1y5I5DS8^z=X$&CJk=Nc6_bR-)Y(0`wSG6LscPO6fYwg zdaC!n`93bCw|h2Vs*s)hAg=DOS74S+rE0Zf=p#%Oo1+&xie6%v-|l70x=@_=jNfF& ZKw;r+4>yMSG+&(Zqx6+?L)IUVHURtCQhERY delta 53 zcmV-50LuT#<^iAO0kAX}0Y9@f7(olOjw7H5vqmgO1GA+sIRUfwFxLaKcl}9%Z0~oN&0{}E)1Qq}Q literal 17 VcmZSnIL9>cl}9%Z0~oM#0su5j1ONa4 diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index ed2574f4578c4ffea9d752d86aa167207f0de282..f36495b629f21f36ae6d6ec744736af66f2712aa 100644 GIT binary patch literal 8 PcmZQzU~J#X!@eB=27Uqz literal 8 PcmZQzU~JzSx9=$c2>=5> diff --git a/src/main/java/frc/robot/Autons/MoveForward.java b/src/main/java/frc/robot/Autons/MoveForward.java index d0136f4..237a431 100644 --- a/src/main/java/frc/robot/Autons/MoveForward.java +++ b/src/main/java/frc/robot/Autons/MoveForward.java @@ -1,43 +1,38 @@ -// package frc.robot.Autons; - -// import edu.wpi.first.wpilibj2.command.CommandBase; - -// import frc.robot.ModuleUtilities.ModuleGroup; -// import frc.robot.subsystems.DriveBase; -// import frc.robot.subsystems.Systems; - -// import edu.wpi.first.math.kinematics.ChassisSpeeds; - -// public class MoveForward extends CommandBase { -// private final ModuleGroup m_moduleGroup; -// private final DriveBase m_driveBase; - -// public MoveForward() { -// m_moduleGroup = Systems.moduleGroup; -// m_driveBase = Systems.driveBase; -// } - -// // Called when the command is initially scheduled. -// @Override -// public void initialize() { -// m_moduleGroup.resetDrive(); -// } - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { -// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0)); -// } - -// // Called once the command ends or is interrupted. -// @Override -// public void end(boolean interrupted) { -// m_moduleGroup.resetDrive(); -// } - -// // Returns true when the command should end. -// @Override -// public boolean isFinished() { -// return false; -// } -// } +package frc.robot.Autons; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import frc.robot.subsystems.DriveBase; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class MoveForward extends CommandBase { + private final DriveBase m_driveBase; + + public MoveForward(DriveBase drive) { + m_driveBase = drive; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_driveBase.resetDrive(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2dfdd8b..94c8d66 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,7 +33,7 @@ public static class MotorConstants { public static final Integer[] driveMotorIDS = {11, 21, 31, 40}; public static final Integer[] angleMotorIDS = {10, 20, 30, 41}; public static final Integer[] CANCoderID = {9, 12, 8, 13}; - public static final Double[] motorDegrees = {92.109, 179.736, 344.268, 214.893}; + public static final Double[] motorDegrees = {92.109, 179.736, 344.268, 268.594}; public static final Double[] angleOffsets = {0.0, 0.0, 0.0, 0.0}; public static final Double[][] angleMotorPID = { {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}}; diff --git a/src/main/java/frc/robot/commands/AutonCommands.java b/src/main/java/frc/robot/commands/AutonCommands.java index bce2812..e2769fb 100644 --- a/src/main/java/frc/robot/commands/AutonCommands.java +++ b/src/main/java/frc/robot/commands/AutonCommands.java @@ -1,12 +1,17 @@ package frc.robot.commands; +import frc.robot.subsystems.DriveBase; +import frc.robot.Autons.MoveForward; + // This file will be used to store auton commands (activating intake, etc.) public class AutonCommands { //public static final RotateInPlace ROTATE_IN_PLACE = new RotateInPlace(); - //public static final MoveForward MOVE_FORWARD = new MoveForward(); - - public AutonCommands() { + public final DriveBase m_drivebase; + public final MoveForward MOVE_FORWARD; + public AutonCommands(DriveBase dBase) { + m_drivebase = dBase; + MOVE_FORWARD = new MoveForward(m_drivebase); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutonLoader.java b/src/main/java/frc/robot/commands/AutonLoader.java index c9abb06..e377271 100644 --- a/src/main/java/frc/robot/commands/AutonLoader.java +++ b/src/main/java/frc/robot/commands/AutonLoader.java @@ -18,12 +18,14 @@ public class AutonLoader { private static HashMap eventMap; private SwerveAutoBuilder autoBuilder; private final DriveBase m_driveBase; + private final AutonCommands m_autoncommands; private static SendableChooser chooser; private static List pathGroup = PathPlanner.loadPathGroup("trajectory", Constants.Swerve.AUTON_CONSTRAINTS); public AutonLoader(DriveBase driveBase) { m_driveBase = driveBase; + m_autoncommands = new AutonCommands(driveBase); chooser = new SendableChooser<>(); eventMap = new HashMap<>(); @@ -39,9 +41,10 @@ public AutonLoader(DriveBase driveBase) { eventMap, m_driveBase); - for (String path : Constants.Auton.paths) { - chooser.addOption(path, getAutonFromPath(path)); - } + // for (String path : Constants.Auton.paths) { + // chooser.addOption(path, getAutonFromPath(path)); + // } + chooser.addOption("forward", m_autoncommands.MOVE_FORWARD); SmartDashboard.putData(chooser); } diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index ed5ba11..48a8b45 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -62,7 +62,7 @@ private void configureBindings() { public static double getLeftJoyX() { if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { - return -1 * m_driverController.getLeftX(); + return m_driverController.getLeftX(); } else { return 0; } @@ -76,7 +76,7 @@ public static double getLeftJoyY() { } public static double getRightJoyX() { if (Math.abs(m_driverController.getRightX()) > Constants.OperatorConstants.joystickDeadband) { - return -1 * m_driverController.getRightX(); + return m_driverController.getRightX(); } else { return 0; } From a3afac5d7bd906348053508e07ca9c9c80f26c91 Mon Sep 17 00:00:00 2001 From: InvisibleTiger Date: Wed, 26 Apr 2023 10:02:25 -0500 Subject: [PATCH 12/31] Tune starting angles and work on the angle stuff. --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27797 -> 27797 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 36911 -> 36911 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 43747 -> 70315 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 29513 -> 29921 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 23411 -> 23861 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes src/main/java/frc/robot/Constants.java | 4 ++-- 12 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index def3f730b6db02fce190df6908e77c69e99b1868..81cfc3779fb1c6f699569bb191f3d7f55b97e5f8 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oM~0st_j1GE4D literal 17 VcmZQ>zc8b1qOQ9v0~jzr1pqN~1cLwo diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index 3da1217fea652f4ca0f1cf0389e6a42c8619a9be..7b905192a7ab396637f556659c5020058e8ba9ba 100644 GIT binary patch delta 230 zcmbPwlX2=z#tjc*nRN|LPmcDI+Wa?`lZjc^HgIyUmmbSvU2UGp`(ccaV20L($@3HR znL#oaLA2KY$?e`CrCRqV?}RY^Pk!w!HMucyHP9g8$@)n^Rklu(6T!66f14DFKTNi_@1BuQJ5eD;|CRh7PO+F9jMJEGIFgU%rH~A(jv#zf7=FH4?pd+n+ gOrDwrvKh!Y4rbUEPX3vtCjfM|9s?No3IZuT0PeO;{{R30 delta 230 zcmbPwlX2=z#tjc*nKjMBCP#ZoZT=g}$;7N_J7;pQmmbSvO{43R_d^)hCVzx5R!^Rv zpwFynbanDYDBbQ2R=RcaP6*@5tapYd0^4GA;Mr@&*W-fsmbS|ylIo8lYu6f2W{?6zRAk0X_C7+GqWA& lNSkMqr)Gg{ws}7JIGAC(cJj|GJ%PoV#)b@F;428E^Z*7wQO^JX diff --git a/.gradle/7.5.1/checksums/sha1-checksums.bin b/.gradle/7.5.1/checksums/sha1-checksums.bin index aa7be5df19b88de6849c1f6c75eaff2a28757cdb..4ed1836d68632f907a24622e7db9b5393e86b818 100644 GIT binary patch delta 248 zcmZ3#fNA{#rVSMt%(@1rH&rSDR;YVTd7XE10F}4HDA2FgYH~ z0LmTAHD=b;`ak(JMCSfvMjs;{ppyR#U|@N0^1{62&FAu*7=dOAPgV>A8)fS>ncr7x za&mzrP}FF0eF4xATQ0EJd9YX@n9W)Ul6RV{SO}7Ln!FLHr#oMIb5EfQE6A;r&-zPE z?vD}Q%u?PW$gHbtJy|eNYI1F-#OA#nFM%qoe@tH42h<9r?}KRD!pVv~U{C2y_U?ZI E0HE?x^Z)<= delta 248 zcmZ3#fNA{#rVSMt%$nw5o2xUjS(r6#=S<$~WyG>r)9Cu-!Vp8&Rv_!zWKD0d(CW$Y z5XRNX2Xl>?HI1%LJ`EArI+@YOhzF?l3IiBe9-O=|FM0F1JSRq=nGKT_!@x$_PM*x~ zD>XT}KoTh0F}c0~XozhUSnNDlY#x}+S_qQwnXFg{lAkttBT!FwzVzmvLKjw`TZ1N_ z^_QC5A0xh*rMyLuS<@tUvS6UpNo4m3QsMY5AELGWOxa??k;Fj1re2qsL3jlsGSoXmPLlJ4|W%P5+Agw zX+k5tm77$1l#|oM#0IrCAtxHtoYeQLQOrY8@x@CGM5{T+*!IpWAiIt@vg7GLat?6! z?%dye^WE>ezdOJ0eo!9pL3zNIX&LmWwMDl1mV!j5jVoa1C)x_l3pv(eC{9Y`%zfNH zfQm9cruV~y(M-0}>~Jy#W^)P4OthJbEoQ6B$xN{2nwd8S=+g%#Y+E(?=kQ3)gH1v0 z`I+Q*7JW#nH6z{D2!H2t*1S%Gf{t%okaBkYif{i`vpDEjx_L`?gGQHq%GG?z!FyBb zA#3x@)`g>ziVWPyyyARo5^F26mK0eRk1Q#6yXT3BnuvT@?WYQFpgdiKb)^)I(>{lw z5N3g$E97ztGzG*>j6r8e)Fx>S41VDd!~%o=Wun6H=FdT`6vaS8&$%HitSjFeB4WD$ zG513V!!P!y)XFoWW}M5V0)rW=%Vcwyy&<+7c3Ux(3dP6n3xQTN5CoG`3}NmO0inVI z@ya%_`y<`?L4z3+YcV@m5=13thqn=FUkDIY{UHP`Kj4p(t;8L5Vpy~}w^&OYs?FmZ zPM0>HEo8{A7alrufOr<^4hCUeEkP3X%5YC4f@w9o__-qgyzzEl;AhIV1VV9r0CBVE zOktyua!*yUi}N-KFVClVj14wOP+k;!GNe2l9LlhE2UqA~=X+Z)_AviMyVt6*oumBm zI6J7}CZ4;LicBWY!JEwp9${ShoFmuHI$VpGJco&oHpXT*TTLY$|J=Yy+EH4aCW%26 z8$(d7QH@_&N*M+?Ig7o}%;>a9dIm>CLkzUzqoq`|j>%)4F0*5yrI0CLO|O!)X81_x z{GiEYU2G0#&1DQ)y+NDA;H^cVg?OCAi+chN30eWZ_h9ul?NXN}J!p%GG zg(_wYSN>PbqkQ@W;`$TNpSUjK>)?LflJA#we8iF7D6AVusgzg50Q4rCMQbN{XD?)} zR@Oq@q_s zQCpT;c}#401aFba(M13odQgtE-V>NlOlVULRJuoDI zypiy@*;K584=n+0+8$6TPl=)7ianqX;xFuiAZwgXt0z&kE07O+^vuaHe47V2F9Hx7 zO&-Q&vzBm9jzpGG_|WB_lCBjK)t)3+c*=J$7+3Eld3{X`EWiT{sg4HrXYOL)q5{Dn z)zM+twvS+l<|1D00+NhYwq&7(WKp+C){%+ATUGAmDh@PooKHf1K%SH2oUP>CZvn1B$wKNN2`LSoE! z5~|z;q4Mxg=_X>3eEbt9K0A`Ar)>!g7Z3M`pq@~fSf?XDycs7DJ=Lh{E$bk5X~M&k zPB=QmsCq*@ShWmdePEtQG_Uy)5YJFFq(#+x@`>}Ac+{w{RW{yH<8{Lbi}jv&LeS1D za?y^)N5(wE z;9sAC^y%kihrM+MCTCrg4VLVeEUYz`*!%8Tn35%*y?23A&cUQd0$fjk3%-%f6%)v` zxmfb~i9G(w8v8?>?wztIR8il6>OT6K*Or?|NNt2RtEk=D62q;EtMj8p8} zLHe9cFs9!g(z>mC#~;=sc6&HJ*#rX>hxl)=G(mLetz9<`>qpE^IcUG~dhHXRto%~A z`lEhG`MW6mVlyZeJNau(&7f6$#kbZp!?TKmeCt><#PmHPT)*-cxIG2J73G!W{t+z@ zsaV6eGFl)}(aKx5tOa5fpYfwMwSZ3XCf_>S0<>Z+-|}w-P2V;F2|bPHX^9dwvlWyx zZwpNus;&TCRI+4oHPs_J1%@Z&4)7cD zSIX)yg!mLu5cduN;P(V-1g|B#zLVVslCe}8-7XKHDwUce7uIyL>JIc%davR*EtLvJ_KHCIhCDV$tbqtry(5)wmIp5} zjmo5dlm!d5ei4CxN~6ZmQZ{uD4jxohn@rcsVj6y~5)AYid9XI7Q%Q2!^~w~8keAFoA)J%BS=z^C(0lm3l+?H z>%bt3f{X?qj1zyLUKTL_@E1~n^r?XPG~Q*T;>Crmhmex}@!QiWtxS4y2r{mV_f?*| zb7YiT3-gwxgDW>cbZ=#8z8E?L4LPQHNNrkQ1;$?K($ll4@jmF%bL20E4nuLr4ZYW- zL-CvMk)ZXpB05^OVkmOf#rmL0kC&?qiu}IOiRYm-LAff9lu`%13<;g$u!pLQjSMx` zoeUGK!A`q$g)H3RMyeo%_ zlP=O2?%5pOBRPn#wGvtNaXFTp33^ zxT5QEl1Sc)t`Fyb{(t4zI|}|7ZVfqOssf?uK5j}Va8cLCF2OI$FIZs9O~_w Qw-!@nQ1j{{D*5pL0IF|qmjD0& delta 1541 zcma)6X;71A5O!Za5d*}85KYrI42LHbVxrtJMCEXbZMBYrpqxQ&6&3MnJ#a)jWl&$n zwbd$Grg)%D8=7E|1}Y#Ph+1@})(V1P5-PTiR-`!gO&mLDf9Ut)n`fWheRg-=*{vvc zttfWY&58C2+n23WWUNb{HdV1gtIS!F6_}pADnD!K>NIWcu;(8~^P*2cE3L0mIcn2V z^3(n4qS-R1KaUli`$xV7^xJ0-%El>PuQPI(5j6*hgt}z<)4wuear9a;-T%3w{hye* zbjdQmtd+}_4ioIxwv+hBfe48sD5;=jC0hAJsNb}F z1)+z`CiHV-`FWN~)Hbl7dBZ^mtvFW*0odn7T&PFy96Z&?s$LsH*7bgj(e6Zq(8y-` z=(xldtKB1JO3ByD@%&j3?(R8>pEZNX$jautGJc%?X1kB0Q9T7ZL4U~H$?fhj(OnOe zSmsT(7^Tx-j055KU~&k2NGBLX_`@v-re0}MDt{=W9;p)gr9q5y+94FI_8?p?hLO9g z`Dkee)zCn@19crBz#SbB_hjy1bf;i))E(oLETVHZXFy{^^pbvM2LLADw z!5v3*g9`3FC$4p~N2u(EFnkyS@u=#78PH%Ga+XjqIPb{SZERu6IVi{6>BJjL z%;3f=x08EEz(JZ^Bg1oy+czf+(*PWmYTQ$T3PguasNoOaEM&f2E8}o(*1l(y` zp*{}pMD`8Am=sT%y{~Xva6KJwof#Mt6?7)H{65hsIjx^UwdE1c}frYR;r$T?~xCM-jvgn<9t?$H%}*Top-_RNPq2O2*N|J? kcTLg7xzgnLcf7YGeh$`0~oMw1^_dd1jYaW literal 17 VcmZRcbt>7YGeh$`0~oNj0su2=1egE- diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index a42af1e20ce5635bfdfe3fc78a940c7a25e8ee44..6dd189c5f7f818fadabad63e1021721cd2dc2b7e 100644 GIT binary patch delta 2145 zcma)-dr%Wc9LMjHfS`f9BcdeABdvr$OaSqTh6)L1;iyWRWj_xs!5=Jt2# zyu~X&EI2mKfzhw#5vWmV1mPtl2+|HFIn>H?Eja+1?Loc{49q#RR{&GWs${^9B>R*j{BB-k@p6+$L_2UkaX;CJz)> zU`|y-g{zJdkG+>4G<_CkcIm9EtdgYm)mOY8_FT*bcmucGTH8Y~z9xUVrO0sLrH@~>&b7m4MeLfNV@Mpby8JV&RI-td)W$=oD>dbMiAmU zw#<7rO)q_wv10y{6K~BrqP(2#^7rOZ@%^@&tD_n*)Kjd@>|~M(o*(++b45C*G(w7| zkxDRXziUo;gxS$fc2Zyr$@5}Z|2Y~pU+L8Z11ppkc_m797eh zCe%D=hp-Kve5E=76rW*r0BE_<1laP=sE!*AhgI`UMijId4@B&b`FmXWN`;CIDRn3L zN|g+7n$K&utEPrk zHP)fyVUC!IHC7W)KO~22-X}!k>OaqR*Uxus;9CZAjx6jL{{4R)nOmn%35;%Q^IU&} z2v4})YI4-jlP8)S0)n7(;CYG&*5--?DNQF9!j@c>8tnnin40ZYbW?F9PLTcLw{6t@ zji=3XL}nlE9meDSP66uO4bqpR4PK8&Z}S>v%^67>w#Spo{c&k5;_>>a}DcxsdbwfOgtT+B4a~ zRoQo^2}33XLVTrEM#Ie&5}Tj7n9&gDn?O%Ao|!s1bZji=-oak*oyMo)n|HUyR&rkp zrW1PA9aO6N(79nrud$FSWUu<6T8orLoGe$KVy4d|Wx6U?5^v`}@ zX~;=Bw(Ec@`~LD)PFl}e%tHSETQF|cI-~Sh>8IFj7c^Lq>F+Hh@mz!S(x|L|AKuib z3rgBEE7O0yR$Tv>KX8OQIN9cqgyja*4>@Y(J;}MJ_=!~w!5uYGvr}zN4)(Ud^pa7t zYe2Ulj_;k_l>vjjhm8BmtkIk?3L5S_noD&U;1*YZ%A- zSV|W{*>0H-&ECbp${E6Tir|aAB0&w~GzYftRUx?(Dj!qh?}U(InI8i4FyJji{gBP| z)xNmu?YTtVl+?YU#^NN(64epo6o4HT9$+XCfvUi~z()`rA%V;67w5Q?#Odv>$ zMDX=dbAS;*9hHE8q%GB z@$WRoh@|c3;#cBbI`gr^?+vtsxmx6>H0LIRbg9I}jqK9QU7otC?7_B)BCCQVEO)~t tntaI`bY-UHH+#T9j%1wu&C)b=`vKi=`qLTq*EDfhz1a%~CrmG1^*8aipAY~5 delta 330 zcmZ3zl;!bNrVT0*f)YAR3_!qN&cI;6!@$7!bF!(#fyt_p6E~le*?1sZa!+Dz$o!Y^6zc_C!O0tRxvQte*X^znW5! zVBDPR%*nW!>r)2f)HF4nZ2 zJ=rh|%rd$>`M$3n%VJHV)sqGNz>F=E1KaeOHEpL&*3O2go;*1g%Icon4`sDaJ_u!1 xP0sfRX|$a;c_D<+uz7v^Yz1abV}s4UD>NCIHBAL3*T+gte%>UqdHG5kMgXv;fcF3Z diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 7929b2c7c55311795e0e9aaba7ee9d327c3ccf9d..f0a57c2f97406474582bf2f43382ccd343bf1c3e 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~qi#0{|=a1Bw6u literal 17 VcmZR6A~{9FsWEOA0~qi{001mV1Kt1t diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index b00c2df63ccedcb1619abc44010c730c928a298d..8b58293ec9685cc0912700041197a625e138156c 100644 GIT binary patch delta 416 zcmX^4jPc=1#tkMCi~*ZXB?1@)x-M`>J(y(lodFDrJvY}%zF-uXBcX8dB~R0NsOZJb zvC=2m`18}ZgvCXKLIg{yH_Iw%GV;%sH`CcZ{Vh~w_GVjUpi0I5`C9fyoKThKoBwJ` zunIVr)SMFCE7}1S72V9MU&zR>nZs(Uczy;{aR1~|1519k%vnupiUv?KKTlq2Fhd}u zDD#JNLgEjo;QGy`h7+0i1MW_@n6m)tq~hMqdyRpvFRStl;Icnb3N>@?1-5qk^SbPh0C9K7HC;7eflEe@cma>hVH5{LysSm4Vu o%i=>!>g!IbzEL^o+GvWY!sY(21^53iuNUH68tT34?Lm+V02ls&h5!Hn delta 84 zcmV-a0IUDu=>f^+0kAX}0YtMk7*GMTof+T(vy~gc2D5G=AOW+cBfti;ekuG~WS}tv1+`oi`!^9&xcDpb@h{NC+!*S|7gv diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index b869a1dcc100219f3d28528eb8b649a867e6b4f5..1aa430c2010fd989e60f3d295d3f53a43192a5b1 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~oNU0RS|q1YH0C literal 17 VcmZSnIL9>cl}9%Z0~oN&0{}E)1Qq}Q diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index d7bf9f468462d2364d343033993da26c0ebc81c6..0f01e9a93ca8b661a03f87d15cb79a4d3531a0cd 100644 GIT binary patch delta 582 zcmeyojdANP#tkMCj9QaTB|P~p^&fkFTs4^i45C*~E|u6I(9t6&Y~zy30TGPZzFAiC zB_n_4#_*Y{lIBps$(#R5c{1_$>I&TWxPlWZs6N?NHb7v#@UcTOtGOX!3^A6QTV)q8 z3LFTTlA2|9_cKH|diQ2oc|k^gNts9O?$*u_!C2+ZstS>e{36Q@+CA!4LJf_bT&k!b zaJ%)uhLo_7T~O_BT0vFb-h5Q)1*5>P{rn5IuX#`bRa!iGsmcQZ zX36)<+CrGSpi1*6*Qy2xu+NH2PFuR)8Y<{L`Kam-0m1dpiq?jG&VmXyZr-Yvz$Bn^ z^9(z0mtG!J^yFk-%@YE~&6n&JX>s0!3NlYVtGPkI^4eP2a~w*KAcE1;H)Nj48Wc~X1P^ew^VRpf21_l|%jfw&yn;kujgeS-Qbx&6Cj{@Sm0P85e AxBvhE delta 139 zcmdnGi}CX|#tkMCjDnL*B|IljlUTkvRPrI?W-8Zw!&0yTTRi2Y^^EHJ4 z#>sX{;+u1oEEzZNQ@+nQd8_J;$xGEdCbO!4*vzXD!8Ey6>%int?G>9vbs88auho^D rd`~xia<1Ni&93?uER&}iOHMv!?9THa3K$q9S8i0y5ZmnNVI&LyiZwUn diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index f36495b629f21f36ae6d6ec744736af66f2712aa..ea791e097ec703e11b991b115a87e2e5f1899070 100644 GIT binary patch literal 8 PcmZQzU~J#V(3}DQ2NMD= literal 8 PcmZQzU~J#X!@eB=27Uqz diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 94c8d66..1108a38 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,11 +33,11 @@ public static class MotorConstants { public static final Integer[] driveMotorIDS = {11, 21, 31, 40}; public static final Integer[] angleMotorIDS = {10, 20, 30, 41}; public static final Integer[] CANCoderID = {9, 12, 8, 13}; - public static final Double[] motorDegrees = {92.109, 179.736, 344.268, 268.594}; + public static final Double[] motorDegrees = {91.494, 182.373, 347.783, 315.615}; public static final Double[] angleOffsets = {0.0, 0.0, 0.0, 0.0}; public static final Double[][] angleMotorPID = { {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}}; - public static final Double degTolerance = 1.0; + public static final Double degTolerance = 0.75; } public static class Swerve { From b8264aa45bb8f51c697fbf55dca132c49441380c Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Thu, 27 Apr 2023 23:07:36 -0500 Subject: [PATCH 13/31] Got swerveautonbuilder working and auton but need to implement odom --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 70315 -> 70315 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 29921 -> 30023 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 23861 -> 24401 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes src/main/deploy/pathplanner/trajectory.path | 74 ++++++++++++++++++ .../java/frc/robot/commands/AutonLoader.java | 1 + 11 files changed, 75 insertions(+) create mode 100644 src/main/deploy/pathplanner/trajectory.path diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 81cfc3779fb1c6f699569bb191f3d7f55b97e5f8..c4c076ea0851300734349bb6be9cd52517910643 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oNV0st_z1H=FT literal 17 VcmZQ>zc8b1qOQ9v0~oM~0st_j1GE4D diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index b183bff26047820ca46a4b9acdec67babf9d2b83..d60e0e3183c853ac0306353e8d10e2560ffca39d 100644 GIT binary patch delta 1973 zcmZWpYgCli73Ey!1LW1=G3kUX0TBcrfdN6}G2&PWYDs*=HbE)M@CYH!z%U?)r9!lt z7T%Dei5o%@moFepdGN9BN zJP~?`7c~wB(3Z=Ic-5Y`{K)vxeOI>NnQh=gT{=$U0+j_Q=;6D+!!s&ygC8gigiu*; z5gz=EEjuks@l|K~tal?OIuSSKQ_#Kt*n)SQo{PohtstVC`x5Zat>A{l1{PECRV|vF ziHoi{JBQG!!Bz}(Wq!?29j3N1(FOtP;X1B&fhG9Rhd5HN;aBlOJKOceQl#|#CJDZJ2cp1a z5n;9jW|bM7ap(@npBGvRSl+uZoMG;M4msZ+2HLUsQ^%t;E&M z-L+=-%VF2252@Dt#>EqkVX2%*aNQud+V45Tbu}V3P1Q0o7Y}pN{1;9hw}ncY-0X>e zW1R3F7h=yKgxQT)%4hAVc%Pr-Qc8_1X3T!LT8V>6%BA*QyohfM!OQkDx?6T|60a+W z7k)ehu6C9j*}hp=;=!i#-4KX{a!d3NLoi3UQOVsB6ZiG)8fp6|`qFOPu= z@O-!X+r1g@H=cjZukqqY<9_96J>PD!NCZ|^;j8h|FuO7zKLBZ*Z%0LrTB*uN$x`KK zrtQc{%TLc(tW>9Ds+GBK&pa+c=XC4qB_b026}{B3fo31J(HR@!0jnZOD9(8W@@a~` z01Z)ODRnb!|G)F5!Ac3g9FFv&VJF(i$rAd`1r659NicP~xC_@WBQbPWb1DsLegQ=> zB$6!(@lytHA`XgZeam@tRFDvQ@&6Fzr{wA4R{0owWIj?WkGF zvfpaUM`<0ZiJIF>~zobJIs!(;IR+XO`mZ=OWNLOm})L|KgDrI__W@gNt{kZG| zNzltr5b~zS{_EMRdLr6NW*1*s7(AW1yY9n-r_8cqBLXIj5kKK+HCyBEG2$!S=s&<< z-53dQ7`?}!?8em%fJduXPcTm01!L2GR`(bu0Yb}h4&%mIz{oA`(r|XPm%|g|BuMC- zW{~cBEE8JHoV1-Ffx@0)4kIT>s4z0Y>)vC5!U2x(^aOcXD1XRd*8~X`Y94XuJV`Dl)U*D&_*LHxt>c+ml?|)!>cNDdH`dn|Yh^Dc0N5b`CSA z$ULEIgu|m#WP$Ky3%9mS@j;gGK|bVK!x0W&nr3Vby$7aeE7lGrRCPPj6zQ_nmZR7oinI&EniPq$;FI-y4g%;nxa51a)}ELew{i^n$j@aMR8hVpY0m ztz2hFW@2;-x(peOR;gAbHvAZY1H=Qw62Ab3&G^KQEr_(j(-$IH54|d08>eI>;s7aw zT|B@OZs4OS$V0-!$n^%LE-5~anIl)csn@6(ZgDkS_(jVN@>wc%G$RvBW#Rw^Z%+U* z)N{`EEji;G6TqtLBbN{zNTG^z@omZFVS8NT4Q;U*RGwOXkYll*2PA777A&1y1FB%T+Jdy-j> zoQV41u|JZPSjvaX%B!txnem~oHwBQ2`!npe3AA4c)9u;X30A^meC{r^TMC0tDb~hn zlxkHHSv4wSM*_wD$?TC#sG%r%Cv7mcBBMLa2jlhx3|U3Zk?=HEZl}<`f$VX_ezNzR z<-@`m`@zbFF2hFvWdD@fSq|>>3}KMH4Vqbky3Md5nS2{z^9=QtgU02gn@prXx-_0Le$S zJTb|SBvPY!mXK+}+eO9=%p3gR1kFRKMU0-LoxUM`*jJ*wj7b#}tyDj(#Xn6!wpiPR zByWRLbYmx6{t?*^druS@J);|Y)=mOj9N`1Tdq@$D{TGrTZ1n*ydB?LUiB@>uLi8He z`hr!vnJT@61h+j*<-o2k>rkSyXNQ!x0N6TNvGQVjyx|BLQ~L=kYYW(S3|FOWAsy;C zuCmAXDIo8xwS}u{7eGJ$Vj7<>@cosNU7#LWzD`+A!;jEvGRq!&rVg&YzZT3 z3teH+V(@AQU-e~YR{b4x#X41s+Rj$q?Pe+~Hm;_Fh{_h{xDwK9J5kxOdw=gJo^r&q zePJKex9c_J-(v5f5N z=SO0^3JWU1!>KHjdgcl{nF-HW1Ur5nvFyz1L5nA*dVJUuvhemH$OccV1WUnzZ;2B& zRKnoy`)N_KwhQF4@;%P@G`(HI-M(50fnC4nZhzXH6)tzjW2-cM!M}<1D0KDxk|Pdo+&1WhNv14M&0A8&RLQX`#bm*EDAmK zd6%-nQ>P*a?IBewTuWFBW@pLm`S(+^%0KZVRcRF#E-B0Q&GoYhF5auY>OrbXU$R~I zXIL7SL$kOL+mz%m(%oin?lxjzyd}arDdBQ+BI21ztY2toWX!Y*GIES3w%) z_Fk(3k@>A1@J}+_5Tl1X@z7v98=9uQ`kz(@-f}pfjNIWQu5ZC$E?Y0d^0jA(he-&JeGw=Dd zMb(%Le;7g5_chuDlU+2SWVvw5z+{*0)nJbUN0Jp${Y*`rr7pLg$%X926O7Yoa_?Rl zO0=?wQD_MN`O$QZ+b10P@ne{%agQ|VNNl3K-4i=zlT$43A=@O`U_=Kf2Asq_GY4cH zA{(%dhrIa*)a(raQR7TdxJU3$R$BxEB5wD>e(T9=0bWV%8B~M74imQ{b9~xy_@W{q z`{2j`<7ub4?qHj>0X*8OJ5Y_>iFG>W_l)9$G><&yv`r^>V40u1+o3&7`8#Ctpou7; z?ZSgSws;-+wp)D|+%XdQEA0>+hINoIK88Q}Ow$VTyulG)n~Z#05)ACHdJ6J1rbA=F zfGXQT51pP6wTJSk1w_yeYOEGG<46W^0^j&G8@-HDcF;8BAw0(;@u%TGry*Z_Bn7>W z7fnar!lOJ!ZdkPlF?hfXa-VNQ3TD@q_Q2A`Xf%7RR3bdd0}+hRE=D79(M;rRJQK>E zc&2#bT*9lrn$DlkmG`Fa&l|leey+T`Jp+e!tn%It66F+3eg=K6+;d-$wChFlbLIZ= zE9SpJx#wdJH`Cbk|E9b@c!7SLlutfR5&>5m{^%o9T$)|Tb-m#>aUFrh?x_wNKz#mzqIRm}~&Be}mavgFPZ2F#DTD>ZOHJb@u4#&sNy44vy~ckc*I08ZTj7cUQ?$=fBNx`CAojC-`st3F6VRX zPB-`$mY+rrW^&rw56_zSYQ@3#R74*Ei>@S6nv> zD+o*)<~B8(Gg|RgcUYTs6WKYL2^BpuZ1JDP`|pjmd@pS0KRxR?ivyY60B7bQYkaE? zSqpX?FD7%=qaJw)R_&$5f7Fvny?5^rDJp1fZYpUUr`IEY!SxSFkzM>>iQsx3t-M>0 z#DbbMTJ*hx0t9?%J3b^uwy<7oUX(>jlFB2#kd60V SCLf4R`U6?!C*4Q>qyGmH{sR60 diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index f56f9ec3568430d18fb398cbc6b71b8667224df0..e922f509f5611ebf7085d4327f11e1ea7c9e2dfa 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oL#0{}Ct1la%p literal 17 VcmZRcbt>7YGeh$`0~oMw1^_dd1jYaW diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 6dd189c5f7f818fadabad63e1021721cd2dc2b7e..43d1de5da4d40a57e8d6770f05f1aaea93c31cc8 100644 GIT binary patch delta 966 zcmYLIZAep57~XV-=5*Jll(&>gejw6hiPgZh85r{;!KpQKxj8K*j2uc@S5mZK4-9BFxW46 z`d;hRFv*r3f*GwrFv&KxL0nFnBzd%a=jx8j~RB# zQwZ&XhncL-^iJl;)zKaTw=KR7uIC}0O}znw1ROKN3IWFhL8Qm26J|)%ONH*<&K)A3 zbIpQ2Gs3R{rQBr3H>w!;onn~KCn3IS8+;5{C=UmhhK6hd+H!A)3a*e@*-z-gp!c~J z=E%&^S@6t94Qw$bxE#M?eF;R`2rtw?u7aX`Oum-@77T0|l`u_!{1uqa(Xii$s@oEV zAl8U^z1xtFflVQV;Orx~hN1*gvuXHDi0Uy=B2_>ER)wZPbO9lMf?NVlCcu3HWaThU zfHDiV3Zx3RExC{E_02O1eWh{e6^%w19#qpzsd`QBt`0Fz#mN3%t|iB9%NRmQ9xq}I zsMbIzEfk}cN(O#p(%_dV#!FLCju(YXp`0T|kRu$!wN*h9%|_N;z4hY63bRKuQB;4E z`^gF6q#V!t6r@@rh5y~QHx*pB#L#RJbB-IiMuW>z$pwmbDF?1{zzRJGxOK1u^X+Mp z#i#}KxA_mr_I)a0RCC!X*Q$A)YeoG6Q;irj9m`13q6L|iNUiJ&h1}ca&zZT79rY*X zW2zie)^tya34`hws_w~R{-b14SCbQ|zKtXwxy54+{hkHn|Z$R5P^ zV|w>~G{Qg+uJ$OO1ScFGfKODw3Y%r1YZMT)du?RrF86zOhcg%zDvOSSd< zm^?x^k9Btgk8P_Z(u^QV%4{hFkq)Ay13M|OqVJn&;G22oeV_MvpKs>skJ1PDMz9btza(mxMN;N`1}V% zTI9kO^ydo5w?wl}+xArKlB1Irpos$8(~TICBkC-uOKF2Y6I70=elrZt6dy(#gs`oY zlFbg(QD7GnF(3m?EvhyPm|jNI?VN-KWt3O>4y6>>xpBB!0#@IodEiAX0^ z`f}>&`)nkaQ?kyFA_a`&tPGeY^vOWK8(+$eE1mXP-{iga_UOpdm`ZNRWQla?9K|K4 z8y;j%4x8VQc&6g(P5QZ=_PJ`3RH=popP|3Vqq2f0t@J*8$`dd(kEhHl7;?* zJe+l8rlPE?(7jhcf89RTdE~tVE9%LB9WVHW-2{-~w!%d~P!oR%q55noP0K`Uf@1qx zMM~#D)lnX~4I{);yAvsmge!KmN~1bPkeUdYdQFmUXc?OD#QE0E3c;n{`ztunKTL`(2cl)i($v+6)gldJ9;e8kuwI$NCL4S2n%#Ae%t&~w0_ey l7ni5^H*>D0kAX}0Z_9w7y%5kP9%o~v+XHp0kdr@n+ySwu_1sEvq4CpEMfi+ AsQ>@~ diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 1aa430c2010fd989e60f3d295d3f53a43192a5b1..4736631fa28160d2aa06cd51c00f286d9d089f19 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~oNc0RS}b1fu`| literal 17 VcmZSnIL9>cl}9%Z0~oNU0RS|q1YH0C diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index 0f01e9a93ca8b661a03f87d15cb79a4d3531a0cd..e8c5cda85c85bddd665164469b8bec49eaaebda0 100644 GIT binary patch delta 679 zcmdnGi}B(<#tkMCj2@d!B{Y~cCN%z;E4OyeD+Vx#OaBiA6QiG8e1B}pE~t3rn^{63Vz@GS9SrT!2StcOJjAJUPDCV?I%x_R}eU` z-iKW=Jnkh_>C?$;2z*?b(Z;x6#|x@(^X9Y45sU&q&R@0jm*cw&6?LC1t9nAAyLzFAeXfKfoNbWK3O zWUUa0%6RF`y;>8Q_~&`5l&?+qh1!%oSyWd+z&ZEU{lh0WIYZ5Ko~*0;kpJS2xhtj@ zZh?x0PG;3p5b)Vp(5`SqY&ul8>1J6y4Mu@r>mZx66BR!|MUQT_)%Rc$NWJtv{kECt zDyZo1$+d%w9x`~R8R53{^epo(8^RyERK)F{30=^{`$4Q5?G zOm%tP-0$VldD2kT3n#l8TL@G%WX$=TA!7{@j5C~E3KXo`9Q{c+FaWAD?(*cN#$8Nu U7dI+82yS-t_#(3Tevk|k0IpN&%m4rY delta 142 zcmV;90CE4(z5%tm0kAX}0V=aK7$^dhjT%X_?i#lOlf4}nlLa1-v#lPO0h3)I7?X`4 zNwe-CM+1{hBzcqGBp0&{B~SsAbtb^GO(*CAlT9jNv&JfC0kcIci2{@6E*Fy(FWHj@ wFc-5mFdzZ5Z81Oslf^Trvz;^`0h8@DFq1VlFq3^Yf&v@?u^|u?vq4Do6usCpkN^Mx diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index ea791e097ec703e11b991b115a87e2e5f1899070..573566269ff8cf95f7488830f56f3c5d9a527adf 100644 GIT binary patch literal 8 PcmZQzU~E6KsJROO2{!`# literal 8 PcmZQzU~J#V(3}DQ2NMD= diff --git a/src/main/deploy/pathplanner/trajectory.path b/src/main/deploy/pathplanner/trajectory.path new file mode 100644 index 0000000..07e0f93 --- /dev/null +++ b/src/main/deploy/pathplanner/trajectory.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.228385239319803, + "y": 2.1047175059541456 + }, + "prevControl": { + "x": 8.763408508601643, + "y": 2.0492488249029064 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutonLoader.java b/src/main/java/frc/robot/commands/AutonLoader.java index e377271..ba1e788 100644 --- a/src/main/java/frc/robot/commands/AutonLoader.java +++ b/src/main/java/frc/robot/commands/AutonLoader.java @@ -45,6 +45,7 @@ public AutonLoader(DriveBase driveBase) { // chooser.addOption(path, getAutonFromPath(path)); // } chooser.addOption("forward", m_autoncommands.MOVE_FORWARD); + chooser.addOption("trajectory", autoBuilder.fullAuto(pathGroup)); SmartDashboard.putData(chooser); } From 8cb8435f678a78e31adef5516fec579f63a8fad7 Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Thu, 27 Apr 2023 23:14:25 -0500 Subject: [PATCH 14/31] Gradle files I don't know why --- gradlew | 0 gradlew.bat | 0 2 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 gradlew mode change 100644 => 100755 gradlew.bat diff --git a/gradlew b/gradlew old mode 100755 new mode 100644 diff --git a/gradlew.bat b/gradlew.bat old mode 100644 new mode 100755 From 4e796e3a26fe984f79eda5a6f286eb231f0de115 Mon Sep 17 00:00:00 2001 From: roboticsDev1584 Date: Fri, 28 Apr 2023 11:11:53 -0500 Subject: [PATCH 15/31] Added odometry --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27797 -> 27797 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 36911 -> 36911 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 70315 -> 75965 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 30023 -> 30295 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 24401 -> 26057 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/Autons/MoveForward.java | 13 +++++-- src/main/java/frc/robot/Constants.java | 12 +++++- .../frc/robot/commands/AutonCommands.java | 2 +- .../java/frc/robot/commands/TeleopDrive.java | 5 ++- .../java/frc/robot/subsystems/DriveBase.java | 35 +++++++++++++++--- .../java/frc/robot/subsystems/Kinematics.java | 33 +++++++++++++++++ .../java/frc/robot/subsystems/Module.java | 24 ++++++++++-- 18 files changed, 109 insertions(+), 15 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index c4c076ea0851300734349bb6be9cd52517910643..9552e07af098a3097db329d1d3dcd5550311b9ba 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oM`0st`O1M~m@ literal 17 VcmZQ>zc8b1qOQ9v0~oNV0st_z1H=FT diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index 7b905192a7ab396637f556659c5020058e8ba9ba..d4c8abf22ae3377bbedc7d0130290972bcdc39a6 100644 GIT binary patch delta 241 zcmbPwlX2=z#trvlS!SFz_M05-CAIl)EC&;iXY+7!ua_Q>p|3Z2KaBAa!eE*_KLISI zJNY7v(e4cv;hww`!qA!g+FNRJW8x~1S;r>(`+%fvZcc85FwOxPQj^d6h=4>7Ob!gu zWo-qro`8kth6sas7bjQyN=-fw<=veeoeVO>*mHAl@(orX%V6i`%*-~BYpnw&Pt5{J cS_e)(4rbW=nEW$KPXOqCKL#-H6$Dax05pMO!T-cpkr6IX%E5}xeu1Cq9Nn%oFs7y%hllh68yfJC__ z2Zrdfwt_7M3C|4?2J@UISNlp$J`d+bCxZ+zIK8 diff --git a/.gradle/7.5.1/checksums/sha1-checksums.bin b/.gradle/7.5.1/checksums/sha1-checksums.bin index 4ed1836d68632f907a24622e7db9b5393e86b818..de6b5188e5bd8ea63f5cbfef06acc4f9805c6825 100644 GIT binary patch delta 262 zcmZ3#fNA{#rVZs8EHh3U`)#hy$YKGqY#vVD>tzIF=<7``3^8PF1+z50!9q-v;~@;) z$p>@6BD#}LLqxbIGx`|u02S*nfPv+~$qVz6H=oOMWCWRdY_eh)*f5)$llgt6CV%#k z012N1azN(T8~}^P`+`NEfH_kkoQsn;LX_MEGNdMV=Sy$yDRgE9YBKhmeAZuTa(|5Y zW|s11L7<4i&dGv-Qj=>tB{uKvcmYyl9WZ%iAIJvlz{&T)44WU56??#L_nYk9{{#TO C@@b#| delta 262 zcmZ3#fNA{#rVZs8EQ@swPH(Qx$YKGqYy&6n^)do7w0R~Mh8VK8f?1m0AR(;_ljFe* zpxnV+u*m<(ry)}JCo}pO@c`BQX8;4sgOeBLC2u~L=g0^$Rd}*u7}zjdr^)=jQj8+lv;I<(`(wm6 zvy?Xr0!4JKCkqBjO|I>f*u1yn1xStckI5_hKsEpw_rVO?!pVv~AQ5f7$=>}>03sw? A`~Uy| diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index d60e0e3183c853ac0306353e8d10e2560ffca39d..f7694af8aba69f856298d384762c1b8056a96cc5 100644 GIT binary patch delta 25657 zcmeHPd0Z36+jp~~AfVw!IRv>B0XKJ|pmK;>5K+`#LApscfC51XXj>FK>Q!siR7P75 zsFqr_T7O!tR>iB{7VAN^Uf@v=)QZ~LdenDz5-=niy9Dg}PkVfPc6XldJoC)+%=64V z6Tke(@ym}Kze#)?nDN@J!RO0k3-Yy^Y%ug5)VAB?B(*`Tpxb?~M zZ9!7)KZ@qMWQ%_WHV=35t>rY84iFPLfgpMt=H?+EpPQMLsTdPZ_b-PM3L=CNd^uQ8 za=K%3&~GrOGg!Y3bKx>#==^H*5ZR1S9$1mCcA*qMI62Z^rGA> zN=`%wd2-+%jD=wJplTu~n2@W zp&V=&hecqI5VNNxW>BAkx!5+cMCV~V>?UFfv&7QSAL`uDmu~*>Q0{XkjnaPU3ND<& z?4j!XPq=q5@=C`Mv;)AmL4a_E6MUNqfTz@CN900>%*`Tma;Urr=-=|>7te{AiYD^} z^?B(;2%P2QksLoTbRV?D3rx5=@|KIceE5J-O#yfj0=aH|3N6>T(Vn_{DK6aOOzi%& zSW0ak6_J~vp>i@OKr3nGdTz}bVIP)DPbpK=4h1&*$o2`11C23nCR5=iZaR*Kx$ zIi6t1ez^9WW5NRS_hT+@v>7%6!1a%a;6X#$<%(3@wYoESLIYf80O%eJhoaTw6lUgU zLgPwC4~^O>Y+OxsJFDTqz|%K4zTle!5bQ1!m}3(#ol}*oz5E3RICqv_eBkSDKywhz z5u-bTd0GtmV-8t3AsjlA(L57yn!pAp{Q}kl3&&2#4H@J_k|A_8-r&6n^ zoQAjHr?)unAh$hqUIUZLmBc1e|Dr3%jZ(#0VNKZ`vjiXe7UPVv|>w03y zd57yJ7P^4lMVus%8-}^GI8x0!3YFWENYz<4x29DCm8UMM)&k%^2P++U$P<+P2*=d& zfK|E%^K6}1HE|6TsP$r%CwNK&GR~ScvFhIYSSYh|G$U5E2Xl*BEmj#^$}~>7>N;I_ zvNl%f$fB_3hbmv)QMr>f;R-kvW8GU2uL5-M+0uW;bLPqjvv-^=t??rZLJ5EYIZ_!i{mD z((%e&OUH36gsJEtPB24=O5TVCq1T4!R1Z3ZaB_<~-f!lXPQTAH?C%T@`FTotHkp|N z^G~?Df&p2cr%`CMac*d$WUw!sHRsqNw%o8lTggH{LL=UE7fv6g6VDZ1w)d+YppvjHP z%Pgb}L~Yp zv#IhMSDp;P7OQn_r+Ub*zCsyvIa3MwwO7~ym~UO8fCmcj6em;JFw<_f z;~YD6Jo>}1+0WAVNM>Yp)zIFW->H3cMQQ6YQa%{+yINCp_wH4iVM|+V0SZxx8d3$%SWxF} zxC@R{<$NHxetAlO!|L@}ZfC!}MmK)sk1a4>%?*u1e@Y9knkQS=&Hc#r@9Tb8z6&*8 z+6js>+i|kJw06zF2|I0-p*Mf2e1MgL4N)97_cq*NZw#8YH1YdGPs)x&tB0OCdIEBR z{2N%$E)^^G#CBU27U)X7yIHb%!%X_iQ{t!{wp>Sk?+aP$L5Fr17GLjovo3no2Y-GR zh*)pmz=Az5kI%UNUeSnP=g9-Se7hYL9!IRJJLtGGg5nQ^rtR*rG(LdrbE!)*InD7H z`na|&1{$M7pg!SgP{S$Vz+8z$b!+tpyCd!$zt|OZwD2Yt;`-I%1Dyjev|IZiG=1IB zm`)V>a{MOd;TaLVbJ4@Os6 zn`^ft_ioz~N!Y1K`P!KN$DX38T<(p{H(JaLx?UunZ#&8fE9|HFN|C47H@@t z^|vtZnErA1+nqRwS95gpmi9?<@9+)!y37Xq$gutxP;P!xe>Ev?52wqp;uDjOwE4Lb zwOb*Cvom=h#|?C=!<=nD`ju9@&I`N~0Z*Ss)M4BakI-1oJHwg1U;lHIeAuziuYi+x zCXLQ=fvg}_0e$R{@)$#2(_=@k^t;fngkDi<4S+p&?cu)a+<&>%0pyc%s zOq^Pg9X0B+D4p-j9>kwkcuE-Pd>2b5Zd<`RhG=f?hg=g}s>gcyK2vn$Kr;~$&JIld4M^7G#(z{YR2b> z^Y|jZ?%AB*o4x_jaaf(ns~@Eto^=JA1Y`!rOAMXUV;H^8G&cc4ig}$8P@i*R*zCsC zzs`!e>O#E)+^_?|u=$OB7`7MBtm!J?pSzNS`Mo(VKBmhrw!%mNu)8! zZNYaoV0k3R;Z=%a7WichfEJ=(}0PU>BjAF)m3e^Ck|R; z6EE-b-5XedE6z2I#n?K!Q?RTrr!%p)^?P;d$MG`@?;yG#C-hbHHVj5&XPj%AG|>IQ za`=2#Vt?z8(0?K35BoIDMRdsv!igu=I8xlp9Emxx!MUc1jZU+p8?mp|N2{yb;qPpk zDq*X2wL5wc%{_ZSZW6_6u=?~3=bENsVAn#-i}=kNEx+7Zq@@_2xYzo)>ti{>S0V4{ z1kC>xtd&L^trw>QhA?89^T+<`E9{$R~?u+{|J zS)^FE#<1?9e_wdzV8|d&6muDl{Z*l!gE`^M7%Z;}#j~m`vb-hK1$ci3i-C-_N>a3Ih)T0F@+_zrEZKKGM9HI5~TTkFPiWAo2B5v>2xoOnD1-09lR=Qu5$n1RVO0MNS zFmG`8O-`7lx~m7sd5aUGcL(FTD}6yIvVqC$Zctx_qAR=?6lq*^^##dt4!S2Xe8GzD z`Wr!!shnW$d6UpeUtRqO{GG}P13ymWi~^HJb9!*A7#anF%t;(M5U0WKObb#!-J%N) zfLF|UO?USK!9B$Q(GQ#}=EQ>o?{IopcO=a>%QMVQ@ffeYu#dO@U3hAGZQ1CC?td4a z0-F(YTld0K3xY1XIKmup|GO}SF7CJJe(!%5re56aeFYb$m?BLeyXgD>cH!x%4f@H- z78qH%n4Z@EKm-QR2``=-1E$*O9UUR35a+Gggo7<59D(($E@1Zi9KyOf4yc{X@$l<- z^qa5p3rfhKEjx}5?meKm0{zV9`KJePx*`=cl@5qntK_zRNgof~aL2 zg;T;M(^O(yTinM_DiaZ;Oe_(INCA%$sAQx}$XBcQghVD3sw8T#t}UJ%tjfy5xeHTM zy{A6@_R|icH|*{`_&gD70ip{;w4OaB0OuY$9$D03^&dh%^?Yq-qEs!RKTW6}9 zau?j!S0t8*6+8(^3Ke{UP^tM68J{PSN)=*}NX7@FZSZ%XAZHw2ljhEg+4=XivDNQk zj>9pl!K@Y|H8@wLz`3U{JXTKlzIwQ5_XFEM$k^AdmL$~$;+-+o39=V~fFucmknx0k zg^Z`9D2Ys|=JC`@0VS5g|3J7cJ}4vt8I1din%{gpDrbCkCG%S3{ZX7^%QYiTI7om=Pa+GXEyTHylsMX2>D8$2rf7??C@bwHP??^j{j|a>e{U?SH?|^f5OA8M>F5avlS#sub7}w zUzbag?w^@Zy=0qj#5R@nvLw-ZsWJ5|R^kAAe3*VAE=WGOGwq$fA}el0hAcjoQjb{= zNUy_IbB}=2bazUbM}S?>$jvK%J~(XPcRtq3GQ!+|kzV+;UGPYWSgleMN{Lh~BvcZy zSRs{C624jn20P%iD^=%8(rPo$+dON(!uH42r_Y!hFKaaG1u;0%;W2B{T)(LN!YOId zt##t#U96V{`H|T{G24+OfV+-(d#IcRqtE_y&~DT8$Su1MZv6ATxmXJ!&5wH7i?pmG z5~)wW(f%o2w4#;!PFxx^E}^m|BGHiMlZKW*o=`2|iDUvLpGT;8FriT@)bLl+3ci3K zq!NJyeA5X})VtNGBky@gLhwLGc zU?`|>&^{b|b+Eg-i zn%76?oWmH~($X+}D20M-?^HJN$)ak%4@;j--*~ue6C*;Y`I#_u{#oJhx9vhPp*NMP zd{9-bwJjUpUemVhXv#{=>}RSpOP3ccjNcm|(Im%r-81HiqPA%J1K(BeUvX~pa!AN> zgF#a>!60|s368Y%=X>{h?@e%+xl`FSEjs&c%yMh4#ZMje*#YHgH@!4@V~%(DDQ)Kw zw<;}Tb)=&|vng!%z-d$HSnoBK=P>lvu(2T{f9@I=Yrz!g)?AA~c6zyDMqRW$8CUt% zs;e`yL-;{!Eo0fyaxqQ8%!qXO6XeEeefwm=oAVOS%s%k`Wc6(H22Crd;L#-L@ekPdP1@8!f25zsI|Mt@yuTu$w^2#!7 zDMq{1x3&yquh&{;_pXmWDxNk_H(yjbF+6^(Wh{GIYu6dA?5P*dg>uzy>Uv|#^BP}~ zo8#0QRnA_R<x57O7>=fAqy zA3L4*KY#qr$Nl9;EsT+FZ9!k~lR;mR8S_4S=Z}h8!#XdLPPsh#m}M-aFJ^-{?~4p# zpUaqImA%&Q^ir-^6Tj~^$AUr7t+`BD>r61OD{coBzqNMJC)M|tx6hm%<{x2KcY!f0 z)wi|`)K0JXkA?mveV0^SyE^Idp8>%`pIOFgXW1aW_QxIdtMvF43pajpQ#-U`%8sW* z=Qrvs%p=V}FV)Ou0l(7rHRj}^vWl5Meen6FB6-&0e!VSYaV@MCUI7gO?EhIwf@ZKB(BM~k6}z>iPsb&*uL{#Pfi<=Ze|5HuSy7t;&zV7Tl3;ZNatSCxdH06s8=Wra4x)!wn4S ze*U|sma&j)Gv~WfnOZFoNTq5aUn&+6@I+0eBqe+Wk0<7l3YiR4g`oYL?aVup(!EtV z1H#*9F7;4USjcx-!CBY~u;DfA;HC|kyu@yFmTXD@mUe3acBBQ;FmK^dp36ocqXazr zliJ*Xk%_leTRTtLP<|`SLY~VC_2mgTD~J~f_-h-qn-A6e?Xxx2ujAvP%i~)t7_@S; z;~uE7uD(1GEt*(OP!a`?6bsZ6p+v|dBnlNzq9(wsP;_>rTb{G$+4MuZ0xl>0=yY{O zp80tVTQKvZBbJ-K$Dw>RjH)siqD9I+8*aahB-QO-&Kv45_q zp8nRs#0#%Qc$Jt(GPZd1nrjw-Su)e7MkC!$N6h+dL2~Z1n&a7@$1bv3k`Y|yDuO}+ z|GpQA1fDMPWg@kLqSQk8FToQ?#8SRgpaNtse5m1=I{D(>*RK?Op<1)ceP>QY_4gJk z39KN|F?EYXLQ&!7r*_?T)L(gX%f?SDE}q?1)-sVGq*>YpC(Fl=&5%;`u{dV_)Takt zD_I?s@$h2q?IJt#)QmkMZ9UctGz!{JRM>?i+rDRxY)w4Ad9wSZp)HVRl=(FrG!mtV zl96hKOes_-1p*-jPaS14C11@W2!TMR=7A#wK2oonC+_Wesjr>wRsG)G(`|*NQJ7h| zl|3S@8|&HMfV#PGuH7fYe|ODu@Eu_D^>-Uaw?LXV%(wdVNV=*NYK4*~Qj3D4y-g! zs@TJsC$5NB%#-lclz^{LOC^+8NGbSyHOUu96#@~=y?96zH}JgxkAdn#5B+7X@IVzl%m8+frt>o>N1}slmrD=WtmtdkxBUq61*iuGv(9n z5pi?8?NaZmsqZ~}{q}D2jGZ+avmkzyK*1LZg|Or;At*i%mb)o}pu{SvR3(8mC<$;C z;cr95+z#095WZ7b<+Jkc-Z81!Qf5im!C9~ws18yfQj4StkqlPec>)-mL?o=g!+Ntw zCRPKV=dgUg<8O|y;cf$(3N~CSV+N8UNr6JZBh)gfSRqpJpxg2(sZ^#?2-PG>EiA!P z7y98%I$^<+VcdsUDdfwfG6^MBLUZTw1u)+y6heY8BGe)kAqM>Zot_CfzW=H_5xXl+X0leYV$OSd7F8~22=SGPeUUq+>EFGBW zV*LGoFFwrzwXS%u@$*7A<7ZT!2!HV%J$D3L4`aNU0G|FE^@pDAzK=H!A+3pvtdB^Hf;pYY@2is4{C#q zk>PVYFvzbd6MYB@Fn_a`O=O}^B!Q+|(?;-RgNo=gLU19V@jHCpWZZL1P-9axn~zMM z;dxk-;nC+~ASa{|57{7_1Cu63%Z4El9PHlwZW4gDN2AfQ7A^qW0m(EZ?ZTADgXM!?j&B4mKO60^=T7Z)-*yak1HVG!=`*_F%kG z2Y&yUjvo`@O~%0@wN3@KV%*!f7dDyl`^F4x65UeblvP)xZ%gr>HfMi?e@_nZmf?Xm z_YpZj1{czYh|H70)&4I;E|bA!<-ifT?S2{F&E_~F@5o@>)03XP@gRp^=(j{k*&GZ5 zU^fqU2Wh==XPf5;qOdp4vzbywL#*u$1JW!+Ug(X7+ss5{r#^Tun>mP#?Sn_ye1ynC znw*WuulhiNKS1Q!K6se@G@2wzuG+xAA*o!5yMdrc+}Uo1{_PwaFfI~z0V$EVn@!~o zy4&nX*zJBqE{lYce~8FKk#LmT5&3&09A!BgnokrQ<+q58i-K+Sq$UcEa<5^OA7j`j z0CR?YY>a}VM1u#1qwt<~*YquFq5-cj?h2gyLJ{f^R#;y+ntO;$>kCKo7a~8V$&#bA zjFo+Hp8ZssBuduuu*D$S9S)4!4njgb3NyT#)zPyf<&23xZ27Xt-5BQ?s{?pfDgyntc=8(#!=M z<*Jy9xn!1FsJM%{Yo_&cEptIrOI&hKNq_gfVTNJ&ns18w`HyGbJNJIhIrrS{oby;Q z-FwA!@3p-iMGr2@OifK2nUl&hnaLU18T=TYrc*M~awKx3oAqBCOU9#v;Csu?bsZA2 zQqp2GrBsQ_l7Hg>K8)Nn-#q>x#w78ka%FMA(=KM6_cYLbW9k{uH zl!`=;ojWS;etGHicjrqsJzliADbv2lJkpSp@{TE)GK-QaoyjqaSz{QpDKnX}qI$EX z5y>E5gN2^AfXk#M{Jam;^~BsQ+x8BEhZ|r%!1mk>lRPm$i_iYWuw@$XK}3RO=7k|} zWg74UH6%Xq2+1$065rNX_o-whG8C-X@>^FbKVXe`@r}ez}+(F z(m?p}4&Vnd51_Z`4rjq%8)7~sc^R3QrTFR&*v1$2GV+(tp|~3{_0xj2;0WyN0}Sv+ zB<5uayVe_S+Jmr|D)BdEMrqT(7(x^kms`?Ghz5=-cYCYDes)Pmm)wS);lmiZe3;K*|zQYN%=t6!B) zU`7C_KWk${7g~yEGZQb3=Y}VIU z^VJ8MRz&aYI*zu03Bm9!7My``7eTWc-JTBuo|f?44XPB5>a%Fj$Q$(@?6~<&+W4K1 zE5T9=kOE`|ORi{F8p^HUnoFP&l4Il0kV$JdJZkqxLH@bVzG7}z5_ShF?<`RhtHnnx zf_x`Op7~|RTYk;X74I#IKJvkIP^|QxCwYP!u%I1y(=uj{4*vKvNC4NB-*Cre1ojau_(}gEaFE~&<0o(CQGxtmB#vkFjs~{rx z%i#Hms}B@RKNGj}*N8!uVYimZ4NBd?tc0BGbaStCPLwj+2&r6FmYeQm^JCqA{on6< zeC8YP)jU7z?4@Fv`9TDjH2_Wg^o%ZgE+aa^j>_`wsfO-wc;#qqnfp zH4y23bP=NHYaq%!AECm*h|2xAXkf3+Q$HEh?uX)p{p~IXoKnW=f*&YIOfgGoTy~0( zWiwjDl&k#Yj|;9mGG>kPymI-C1&ZbOMuK_0)5V-rDcbs9|7#COLV& z$18dNnKz`*rKPg03$~|>{HgIz|FsZhYiV{zHc((YKx7_WX4JYhE;$T?z2Bb z0{5x_tp^-als)5%hCBAfnC)Y`9{-}=`Lg53`xv*NM_<2?v}I0%Y2SLy7~F?X&$){p zl>oXAvuMwNWdC=Q)>)R*=#l$_ua;%632KwOY2~Wry(nM_ozkK-z9T3C`tfH^e~>(X z{hC*suHRg-eky-=tt`gl_vLMc>5(Z&NjcdWIoVxOG9@89J@Z|+{TTGQ1%|`E*Fm4s zMz??-lrFdlUhdko)Bd7NGoO|$q^FGpZo<3K-E1_|4k*VXcW|A$YVpDl1_< z*r)@hwd~mVj?Q@o$z1Y^=wlhNX{4|7)BuYuBe(fmj%@O@cyG@2b)FYswfq{k>WIB) z$=xEtTOBcfw3s>14tTU1Xso1(lYBT>>oY1?s{0 zZb$`Z$3=R|3wWO7(V*JTVOTHlp3d2IElR8WlU7K7h9r@PmHS5`)d6VuU_xv({ zpGJ1jPRA{S&%RzdZ45R6l#X~88xY;}tr%`;VS%9bTSK-b#2&M)RVY~QwChnQwD-$keUL4OB{>y zMv5@hP!%pN#)8A22Q~*8q_M1`<$C7Qilir>g{O5t(|qgPN#Gxc)nug~x2fixD~gj1hUX5$7e7k>p5{;Z1s$GBUV9FyJ-38;vH~^^^aK z7s%}tH^>>BIRAS6lf!R@b=c5FmwJ!85LGnD$=?UD(v9!Pv0?nYsf@f&j z#Br3VhJV1d-WvaS70ZG-TS(886n`k$nDl1Q=B52U=~pbai<+_CVN%4n4z6ZRvV;F< zp9X&cNzI04MY4_e=&>P<7RTRvq2kIBzb*^tDd2D6Rinn)wTnnX%+8dGk)y4_pIlE>7hY#|pBbf_}(e)|{VI4tSTA2pP98DEXEj`-CJ zYYgi$m>LJRf+-GMak@7ja5C`Iu~T0@w_Z5^dJPA58C;D6+rj1IpmIp@4yPn^9y1uw zp@e2cbTUJ$Izfv(!w4iJqQxD8`yjlB3ZF^Okj%n6L#yT|+UE@#-OpW@hcr#O)*z~1 zz`@Ugw+1S)E?0mJe+l@Ui|Tb(k0QsA(1}hN!0wd;+Op$48uV=0ZhOD3eP;nzap}K; z`wx!YFfKzraahOhWaDjP=6L-yYW=`f(Gzya0VbMOQ9a49?YYV*e`i&n}MMtLlNWvQx5!bd%9U;;3|6CzmD$W!r;P2Yf9zJX1k7`UYT|By6?n?FJJixzr5^|H(gyS81*VT z0$+A<9G@HEu-v%!lD|f`E@8EvZghT1-{UFc7atw;E%--(T%2fsy4M7Tq@>$N1Dm#h ze`lM=Sl>d%qt+mY%_0rPDTY~Oak{8f*>cq<@gVd80zYoQZL>P~tY z#X{FM(X76fw0`%?+fx=)3?)kL=mVR( zEWIiqhkI0*UJ`kn;~5Iaagovsq{I^jhN286fx|fy?E4r*s=2nI0G3(GM)O;vo3-?t z%okqUv1Qh^0De`>_mQr`skx}x;i@m+s#JOQ=d6c49&0w$=lg?q zmpR<8>i`1FO1H#2Vs)5NRsRVpcG9Z;6@NPWMyvdeT&wso zuScdF)h#&b5Y|+M*TyA0hbt;ap#Tmk3WCJajD)WHD4=8D1eT;EiePyY+%y%7>Z+o5 zE+aD~HJck|8=#Z}EsIk6_dguj^zGA6PhH&XcrR83s%D64g4#1Y;i>@CjFAmM8KjQ} z5Bad@KsOIQ}iHE_c%y+$2b>3ahyry1%{AB zk}wz;oHiOLqY+1GR1U=`6J}!J&ldc>nb=E;;DK)}nZ07hTQgd18_}(OT;hNlA+5iU zfrP$fw|KZo5;#4>(k7N;Nt`qABqOl~K|(2Eo(1oEVKG6{q*u9rr02@0Pd?tT ztH#t&0j%a4Zqv^J*x`1}Q534Cz-f+QDN>Ml0Zkg7GI9itc1D4vVA3os>e+5LZ59@) zxZUk>sp5i+Zz@0Stbb$E-J9zjZ{T%+f!xksvfv$#gcAh9L>MKCBN$1-3DJZyAR|*w}(aKlkttM;2m%itvqZ4hasL144d#*sEx5$+3#+@`yUcLP|JTp5=|ALC_00 zCD4Rqk{Hp%&?G8#G18Jq;3jy^@@JM0nO$Z1#y8A|gQnN-`et&=-23f5%yzVV9bh1# z|4uzlClynpMm|(cshA)3LIRz05kvKT z|1VS)sJYcRRu<@-3kw|4T~0u&L3d2G>zp(Fj%IfmU5(l8>2IBL>fQ0IsU{!pLNduA zA6_dR?rO~mu&VRaI_EUCtIE27>X66PIp>e(&{BoE{Cd z#zJ+@N!U7zx!dxkzO~Y&uI4tosP_wdZd2!++H_GbHAmzsU#yvR)Hx>|9j?}@W`UaP zM-{SjE>P#33Utw9TqqQA%#!JxvvUroMpY_5_vkC9&3tNQ&0HMPXWt4?Ek$6kJ?o@% zPH)v2j+`H|%{%8T)IT>=j(HoMbH2z`?|ilqa| zxgAVeSD|6~uIU_tc9_g{5XY4foI}Nq;jFp|?8+$4ab3rQ;@^*s$_UN@*t!j6$_UQU zQU?T80Y)`-2DSj4V{B9B9NKgoA{Bu@H75%ju0!CjR>0BWWKjoEI@zMmfk1osrcOg? zGLm!D&Ow(thhf)b0DImpd}m{6UO1@48G4e9%f;rtp3EfYj#ap%Jo_J!qrd{hR#u4tnE?hAX+PAHrr@7UN@l|e-O?h;}J`cq}R!-YMItSq% z?va`2&i+UPW+3|@=Bn?V+?;}BVtMGP3Yquev_&5SWk{=-ZCyKtvE(S$H(OIVeHgBVi@urg zyD|WlLYYDXpe-qusaX$OYIhx#shtn~bDiU>yN(?|f3HKO?XO&rnN`@nJoL!YLx{DO z=@Xx2`$bn}el@ngwgTy5xBZHB@~@35Y=5Uz=2UC@kJiiFR@)Do-NNUqB}JF^ZT9vq zn0>RK{EhG1rwr8KQ7Q2LQ!E4ye~SK$#kMC{C~WmU769|wXc~smD0Qr7M3JIU4G${o zA#jvAHW*C=O|z)B3O0F)iB|Tqg_w6ZO{4r8PY5Dw5IKUNS;C02JCw+h2AbA${G2?^ zkU8O+Dg?DnlI;i$!{oFbVbYVdUP75W&On(s^pULrmry=f&r7f)qZtZ61P&1D+JIkR z#QRtny!8Y_4LCij9z#$xL-BeX>4J(cP@xRUupw*{tYkDp;BCwXqQ1=YKzbMooa$)fulV%VGcv1=M6L~8T7o7M8?vK zI4bz!DFRh@PzH(R;Ob>q4;Wd`)i%B0k$MjLGz@zlPTZ$hFZk{YF5#dyNw7tI8&3Ww z$O|M?mV%Q6`WVVYF{H@z1cx$uM!j6YLef0MS7W{5vHF^Ds}0n*nb5%1HhrPN)25GF zoB{Bfr%fMaE~ANQk~mUCA8(_&1oXMK$e|B22?8ZxX`bTXcdL;N=6bo>rWfqwZIewc zP9nVFt%1W`#Sj7vO>b8^_TA%`)ptM125tfetU;g){de<~_BZdBXRD9AGCv z;zfs_d9eINTQ7>BGlDfOs|A4Qrzh<5l8xoox7d5|?ZcQi^a`={VtdvAejVb#_W}G* z69<+F5D&GzRe;Wf+BDT2$Hv0KFxy-7a~w;8aR&7+VpSy%6`k|Ly zfUNjm}sgWS7C30xHfIBMCqI#lb zjY+Gj&7@YYw&@Fvt?k1hFDNQ-2EgmB9qi^E&Wc8f7DXNni9nhp6D<)uPvL}MAWb4` zWFQx-NrLlZY{JPqvdTt@ZCq~DzcALOk!qk!tLG?(7=kcysLqZ>#ix3XGm0iOBog|R zUzE^pVdUYsd)RAmIBvsG+va~@6k)TPyxfPHBvi&nu{gny0*ySFBqYMf=}{#pDN;Dh zBQ#ldfA$VH;5@1gVp&ueB;bUR zW2_VWLvO>89XgcP8z@vkCg@EfvY3IEP>r8}PQB>R%a{oG@na;~2Flh%85If_4CqLt zmv~f3${J}@)T_shD4s`~7>1?{5d47+g6Xuak+MTR?<9Vhw&|l_s5yzl3kDkmYNP6$ z{GNl!RGg9v!6m4>1qQ-1Mola2_@iI%c>ujwO|*7{Lei|}W$kf=gVjW7 z&+L_s>UJ=N}!pTY%gHLbOqzn96wZ8b64RSNmEttM9clS1m-Y1(M7E2K##zg5V2 z?U3wulpYSW)3o+HX@$6?1%)inVSCXDdf-&NZ~ayi4>j*z-$P7 z5ymGSkZPM1lWgvwX;puZ)g0m^Wvu>7#hdru}y6tZb&r1U|Bd_yKrD&&Wqkz`X9f7&3Ea~1Mp zXEgE?cE~+6=z_>Xg(SKlCs?D9gS%*&YPT!o2VKwi0{994j*%R>+lIQNKSa7YGeh$`0~oNL1pqU$1mFMw literal 17 VcmZRcbt>7YGeh$`0~oL#0{}Ct1la%p diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 43d1de5da4d40a57e8d6770f05f1aaea93c31cc8..32c23a417296852aaed361257c78dca02869ad63 100644 GIT binary patch delta 4587 zcmZXX30M=?7RM6=6ci?q03i{Quq1H-ii&J1B3Px0m|EOXQ7MZHD6R!1h{z^OJY{Q* ziilMZC8mlSqVNYiTt7@^_G2y!h2s+GwFs?+D zP|(H;O)jL8d%sXe_IbXj_Ms#LahnlP#+8cm)HRZt1~ z#D-5)ZWPR?Gfu2pUoVM?{S#3k9j-VCFw5e*VNn%(5wlf-+ChQ!RtT?Er&%E8Z~{^t zs~FWioYLytanBHBsldg_i*csi*uZVfj(7x}1tZ95hr3Qi^DmZe z_x+Fj_%Z}VF8YzXc)C}W9AvwKa#;11Kbf>^=cMg_A#Qy|&uh2s8~RhOkw4;=YKU<5 z5?^|%+t>6!P6gYw>Vq$vY0Nw3&Jm(u;aJFb7GR#%HLq(Yx+CVW4Jw_ZvFKR;C$d~> zU>r1HCY&w4!fkO$`PefQ1v;7OO|Q0w)bh zEWEe3o0>#+(Mqjw0UhK2-Bo`jI21WpMS_QT0CT8ap-#CSffz4W={y3TKpWUGZ|AL` zrmIc1GJB*29doHum{tUCLy;^K$R8EOczXMMPeoA@#B9G# zR&C|o=FifTa>Th7O6SpdO*1RKCcN0b20`%x*y!?}(W}Zk%)K7D2SNJ*h;Y?lza9zf zX0BX~7$*THx=FAEZs+R%*7+f34Up<4!0HT|j}+Tc(^9k)=!6A#`^>t7B!FQP&TAoq~mIAFno!2F-k3Jo}dBu z7*e^7g7#onywQyg7PCk(6ePO{G2t;sULH78P_@Cl0wY-zA)Pf{>tG~+s$ZwckZExsBXW;k|zKAY;P`;S?s%xGuO#b&8FA5~`WsoU3Pk-j2xW#OzUA6_3-(NvH<8NkwBHViDy z%A?q3NY)6j%~h#yw`Q9n#y$(h8ot>2dIvlAQ<)ipf>U}^n+WqptrM>Rao!tHXuL&F z&)2zrmfuE>&Oyvp3l`dBZ2$ObL;uW~h*>p5hE{?dD%iTTd+Zp*xPO33JDV{wrT48} z!iBR4T8ArbJMs0{Eu|`Jf6zc2qV_8wS{IG^)dfa8PP9c#paZp!7to*QF~)anc&*EL zN8KfyO2~LF5wXoH4W5-WpUD|fR_#PwSO}`;QigBKo^!|S3qDZ6VGz`% zz=DlMhwpmn5VH+|zOF{`1>K2ITmcN9yv0D*i#_RI~1i!U}jj3OcxF7{I-A&l#kPpBzZu?UiT6_{TXmlw`?gRZfP6<@qC`Dm2|ccunG)*5|T3#)9NkCZbOBJS)BL9Yaujdoho z6oofp4z|khSNMWHwQtLoG5SE}eTaT7!A7YgPsOKE_b(Fz#p`6O_s=ahk9Scs)oC46 z2E1oP-`Kv;YFBg;aw|ozwu(k-`eXt?2fSjLB2C6aJX@ArPY$U3Tma| zjRMV^0ZdkYCptk=O*u+H_1Al+q_!o?nD?WnBWU-#zI**Ijm8U72pSpH@?msyV;GuZ z&TiojKJNMO;potLf2gG^67Y#jPix$KrW#BtTn!sT1TC=cAcuYL-Hp(vj}|@R>n0qM z*Pp>3u2=D#ycOZR1#t{ySpWXO(Xixb2OG9zbn10HFF ze=HNBZzB`zk1-854XQi{xjS7AFVf8W;qp!npUsU*+?O_T#P+N}2)@>Fgv`y)8i(iM zhiMB4)-rjgl9Z3^Z*?TQmj{ZfmMkM+!OJb_t=Z83{h!M`u7~)bE#y^ z=5#w0!+TxmW=0nh-Tn>qL!82RGdhd>SD0l+C(HS}u*QtefjOBh!^Q&L0$7!a4{?ER zJ;;&R05_1>3iK?ZN!tKpvs_Jaag*$14XA(xNm;C6Uy;Ek8VC+B;baz*NO%ZMWTq|& zJ|eLLTy~S)WWZ7+(m{sAZm35h8{Y1A#XWVz$kS*BEi~}YWWtne!c&(D3$jUvosfk@ z8q|=YtrlJ(aTNqPL`+*2%*i1tZo5EQy(cT)pF^CnwgjrlAnhsW(hF`bd7||YjKo7o zLP84)BwoO?TyhN1F7F6^1uSsOBOM1|S{~7cHanUtgy*vW$hOWwP?1M?cN&8l2@}9% zBpC+&NQ{JdBz&MuMvOq`1hQ-+9~zW|GofF`B;3(67e=0o;~t{t&LHsJLwbe48YIGC zKN1n3LLwRl0U5IzJdlWmNFeK7M|NWh>oQrO1k!N>G^3!+@EM5&aQl_a-wxqOqyQkX z3$7rMiJu$?Eb>`Iox4d_6Hw%_ARwP~mBFHX!bA59WFxT+YDv-71bs-ffnxy?)24yY z0`lUahb(V$s7ne6qqaA2mJE7^!3$Do2ZLQ9k)}NWA*9d=AsGp8P$J<2+Csunhu$Sv zLGc<74%ht<^YF1 zDa98OmmZIrlN$W?=h(!TsIh--p}?HZN!rtVV{PhK-P0Y8!^%&rwt7Ido+1Z`#(0PS zezPWxR4oD0`@_-V23se!@v%1}Baeqn!SGik)GJ;V zQCS-P_08$K`d=*iE2kv2$)4Av(vChxWjuzX>BLp-*MfR82g()^*Sh~D)GXp8<(SmO zy)=}1j=nQt@v$kB@g0Kbr0)~i2hAKW>Dx|<#>Rn^xqE3nRhBT2foy_t5OYDc#?je|dM zl?d^Q-8>Z1)Vhsub-GO_+hdb|N3SeJ}>8)Pka8jw$6Nd95!|G0%Z8+FquH} zvnp)2z_*1>M$VxiSj?Gg-cofmexo$pxw@t$$Y*L23Lik>>ss;u-*%80CjaBdm&wh` Zy<+dpEw{s8PKYrWKrekX17Z(!{U6??zcl~= delta 546 zcmajaPe_vi7zS|OG5a%0sBrr}mom-f{-K}^d0%*@X4H?y)lZxP zZD*ITnQ&YT;j>u4Qnd~>dIhKSAr$pq^z96BRZTDP_1ZLH%phad@EWnYLA2Hf@x0!{ z+FhN5iwzaj*l2>@*vqHpwcOEz&T74_ew_J-b$K!-;h{aP_Ye>(by{XZgU7yKC6NkFVS^ ztp2dt**9_6Nn3I4Qyjx{US#Hi=uXD}Hz)M(6vW&KXmSs@w0DxY@WntdFZLkUEMaCb z!lV0Ns-VdSG1eM^WqE*y4(uazef1!B@B>D^$vk`L24TL~gYNHn9zSAMq54Mz?HwyB Q(fhP+TKnSm7OLFv8$*@v>;M1& diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 21bc8cdaf6c9e7fb5d3dfb4bc653e11d5b7c2755..63f40c90fea129f70b809a9b4c9bfc12dda7d4cd 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~iRf001la1B?Iw literal 17 VcmZR6A~{9FsWEOA0~qjE0st&)1ONa4 diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index cb7f9bf017214b01eee39f90e065758b49c30137..d6d41cf25ee3a3ae79f73d431ce1e6c7522c5d30 100644 GIT binary patch delta 442 zcmX^9it+jz#tkMCjM1A-C1$V)Ec+~_5U^o|HUk)xnQlHRlgPwhDY%7Q!!Z;hSlYc= zR^Ee2;KZa-!4S3J^-xjw&9RCHS^3Lt*GxImr3n>WyIE9y1&4sseL3xrWopx*q8~SJ z)em46h|bT{<5Aur0Tn&J`L9tSBY(JIdBI)Ybf{qF=2VjoMu9^4LvIqUdznB)%Y-*S zHC19_Qkc0>v4VfIqX(O=gE3G_5{LysSb#H3MD)+9&lSR5H4H7YYn*|Ca;PedC7HdM zB7;^O_iSImuyXe9$%>_F^{7fpt~tnQu8vLjk#_ey+8BQu(~viv*~?~4P8B=SmAdf7 zFL==-;&=ORw*!4GE`;d8i4#*Q|Ze7-xzOc~p R;cE>0kAX}0aLRz7?=dJcl}9%Z0~l~{0RS{l1Ofm6 literal 17 VcmZSnIL9>cl}9%Z0~oNc0RS}b1fu`| diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index e8c5cda85c85bddd665164469b8bec49eaaebda0..4d87dec7473749f17f8f1838d0a1622b9c687eb2 100644 GIT binary patch delta 2021 zcmYk+dss|q90zbiVYT(1w4u>VGhNJ#QlZjNTiKZuy3Ep|v@>&NhQ=1!7F!BaPfJTK z`_r>3tVNG&+htI+T?kt-OH`|Dn@AxcjA|L}<$cdSd;XbcKF|04{?5GT%-dMSs=vl6 z_L{^x3f_~BQ{Pl0*1$V{+lYTJ#T;m!q($CQRST+0TCl0dfVW*Fl~w=TU+HJsg;Dtn zq}l0^lFEv|O<7`7EdrxmoN%YrK^tX!pN?SGFu8Wl;SO`rR5o_`DTre8(G-5~yRNG< zNa+zsU~5raP0ifgWy2(w3kJ3fxi_u0q!+PBXCc^59;9-I&C3KM$4(?@1ao^|VgB0K z!`pn`24SS7%=G+6D|D)_`k!D|hU&fTMJ%*6Fd)_TaSdt6hbWFOb*zj%;_j6f9i*WI zGC5j7nR3tIh4OT4iepUc>8sQDebtgl#LvS+PP$e4@SA6{Buw&3h~>_<+9kL=uhmFW ziIC0Z35$Nte==rUH3y?|4^vt6r)zYlp*0`7MgRp|G4hu5TsJh3EtRXFnaiW}Po(wU zE88P+Q0)qR+!ACq@cQP_q8x1W5ac+>(DslUs$xGaHhCwsJH!bqC4tXgtlH8`rmSJ6 zSG7``=Wmw>lJGk4=k22|Nw{vo`#l$8r1@ar@zJ>Bp-M!%kODD03Ng}J$oxCmbvW8`RC6jK zooB(|Xck0L=2u!9nr=I9C1?vAVJ=F^HbB7D9oW<-pin4AVwugo zmx?B0-$T2QM_KjBbfXmM$rx1+z$lCpqKZYKriG>pBpJc%q_u_JgR52_r(&mjAWF24 zny_}g>T-%Ylr(7Ip{ST*e_rJ2m?!uMqv9H5Pvr^iZ4S8k-h5n6YCD)(j_BJRT^)JH zN$pH1m|84!^1VQ_E#sGwh8!mBl#^hzt3KU`T^R#mt~|=6U@-LZqO~)xTnn?{f69BJDA+~D4;jPk8WLojm`$!Y4zyGS8X7_PX67>SeAWz{%rDhmj6&qXy-*7~+n=ZG~zyL+54_`}M%jaOcsB#SGU9}|3N z$9JV`Vq1wT!QUeneVupdzEu7@Dd2#?LyVqY(tKL8!2%n-1WX>0R9Ihaa7cCL0*s1@ zAob)?;T40g2hNo{V$_V%$9ht%rHg{Zit!S9DOP1GGk@`l>hot;W^Tr=?*Gp#6C#$h zoLq;UZf5+mEOF+w8Eoe1^@3)49kuk`(-Xy6%X&z;3%Jaosuu4o)iM!12-{o2be}?IY%ZU_p*or(k*AC)1PP zwvfgk=AOPxo%Pd#i2dZP))YX3H;+oHbygP3Mda9(%OKOcj!HgCC+Ebt#oz|{R*?F{ z2{%5CAG$L)X^4!^U~1Dm$G`4riYF(6IuiVrb4iQuqsI;;(ghMSR_ bpg7|HpJ)F0uJ>GgkOjNLck8!=vRHosAAs7v delta 331 zcmWNMtxp1R06=$cY+9TK<#!;*mws1b0Bt(hn>5W>nC^xR zA+b3}Fz6^mZO$YOmGsIB?>)aLuTRQ-QYZ!~QRhb-a-!<=)H4noMZO#=n$x7_G#EO+ z87)Yw;PNWev={?zEW6M6b4zs3JeQt0?;blg?;TyQLC{yE>8nuit6cfp*3sgW!filj z6c9v%2A4s}+nA}4= 50) { + m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); + doFinish = true; + } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); m_driveBase.resetDrive(); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return doFinish; } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1108a38..08cb1ea 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; @@ -58,11 +59,20 @@ public static class Swerve { public static class DriveTrainConstants { public static final double DRIVETRAIN_WIDTH = 0.635; // in meters - + public static final double TRACK_WIDTH = 0.470; // in meters + public static final double WHEEL_BASE = 0.470; // in meters + public static final PIDConstants ROTATION_PID = new PIDConstants(0, 0, 0); public static final PIDConstants TRANSLATION_PID = new PIDConstants(0, 0, 0); public static final Integer wheels = 4; public static final boolean[] invertedMotors = {true, true, false, false}; + + public static final Translation2d[] wheelLocations = { + new Translation2d((TRACK_WIDTH / 2), -(WHEEL_BASE / 2)), + new Translation2d(-(TRACK_WIDTH / 2), -(WHEEL_BASE / 2)), + new Translation2d((TRACK_WIDTH / 2), (WHEEL_BASE / 2)), + new Translation2d(-(TRACK_WIDTH / 2), (WHEEL_BASE / 2)), + }; //TODO: tune pid constants } diff --git a/src/main/java/frc/robot/commands/AutonCommands.java b/src/main/java/frc/robot/commands/AutonCommands.java index e2769fb..f8234c1 100644 --- a/src/main/java/frc/robot/commands/AutonCommands.java +++ b/src/main/java/frc/robot/commands/AutonCommands.java @@ -1,7 +1,7 @@ package frc.robot.commands; import frc.robot.subsystems.DriveBase; -import frc.robot.Autons.MoveForward; +import frc.robot.autons.MoveForward; // This file will be used to store auton commands (activating intake, etc.) public class AutonCommands { diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index 5c234af..f175678 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -70,8 +70,9 @@ public void execute() { SmartDashboard.putNumber("Y-Vel Input", yVel); SmartDashboard.putNumber("Omega Vel Input", omega); - if (cnt++ % 50 == 0) - System.out.printf("xVelocity: %f, yVelocity: %f, AngularVel: %f", xVel, yVel, omega); + if (cnt++ % 50 == 0) { + System.out.println(m_driveBase.getCurrentPose()); + } m_driveBase.setDriveSpeed(new ChassisSpeeds(xVel, yVel, omega)); } diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 87524e9..8767921 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -1,6 +1,9 @@ package frc.robot.subsystems; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -9,6 +12,7 @@ public class DriveBase extends SubsystemBase { private static Module.ModuleState targetModuleStates[]; private final Kinematics m_kinematics; + private Pose2d currentLocation = new Pose2d(); public static Module[] moduleGroup; @@ -18,7 +22,10 @@ public DriveBase(Kinematics kinematics) { moduleGroup = new Module[4]; for (int i = 0; i < 4; i++) - moduleGroup[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i]); + moduleGroup[i] = new Module(i, + Constants.DriveTrainConstants.invertedMotors[i], + Constants.DriveTrainConstants.wheelLocations[i] + ); targetModuleStates = new Module.ModuleState[4]; @@ -27,12 +34,30 @@ public DriveBase(Kinematics kinematics) { } public Pose2d getCurrentPose() { - return new Pose2d(); - //TODO: FIX THIS + return currentLocation; + } + + private void updatePose() { // updates current position of the robot + ChassisSpeeds currentChassisSpeed = m_kinematics.toChassisSpeeds(moduleGroup); + double xVel = currentChassisSpeed.vxMetersPerSecond; + double yVel = currentChassisSpeed.vyMetersPerSecond; + double omega = currentChassisSpeed.omegaRadiansPerSecond; + + // TODO: MAKE SURE TO CONFIRM THAT AUTON WORKS WITH DEGREES + + currentLocation = new Pose2d( + new Translation2d( + currentLocation.getX() + (xVel * 0.02), + currentLocation.getY() + (yVel * 0.02) + ), + new Rotation2d( + currentLocation.getRotation().getDegrees() + (omega * (Math.PI / 180.0) * 0.02) + ) + ); } public void resetOdometry(Pose2d pose) { - //TODO: FIX THIS + currentLocation = pose; } public void setHardStates(Module.ModuleState[] targetState) { @@ -53,6 +78,6 @@ public void resetDrive() { public void periodic() { for (int i = 0; i < 4; i++) moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); + updatePose(); } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index f6f687e..7df258c 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -6,8 +6,12 @@ import frc.robot.Constants; +import org.ejml.simple.SimpleMatrix; + import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Kinematics { @@ -87,6 +91,35 @@ public void conv(double[][] unicorn) { } } + public ChassisSpeeds toChassisSpeeds(Module[] moduleGroup) { // converts an array of module states to the ChassisSpeed of the robot + //gets the independent x and y velocities of each module based on encoder values + SimpleMatrix xyVels = new SimpleMatrix(8, 1); + for (int i = 0; i < moduleGroup.length; i++) { + double vel = moduleGroup[i].getDriveVelocity(); + double dir = moduleGroup[i].getAngleInRadians(); + + xyVels.set((i * 2), 0, vel * Math.cos(dir)); + xyVels.set((i * 2) + 1, 0, vel * Math.sin(dir)); + } + + //multiply the current x, y velocity matrix by the inverse of the kinematics matrix + SimpleMatrix kinematicsMatrix = new SimpleMatrix(8, 3); + for (int i = 0; i < moduleGroup.length; i++) { + Translation2d distFromCenter = moduleGroup[i].getDistanceFromCenter(); + + xyVels.setRow((i * 2), 0, 1, 0, -distFromCenter.getY()); + xyVels.setRow((i * 2) + 1, 0, 0, 1, distFromCenter.getX()); + } + + SimpleMatrix finalChassisSpeeds = xyVels.mult(kinematicsMatrix.invert()); + + double xVel = finalChassisSpeeds.get(0, 0); + double yVel = finalChassisSpeeds.get(1, 0); + double omega = finalChassisSpeeds.get(2, 0); + + return new ChassisSpeeds(xVel, yVel, omega); + } + public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond; /// Constants.Swerve.maxVelocity; diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index c9ea9f5..3f8ab0a 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -43,9 +44,19 @@ public class Module { private WPI_CANCoder _CANCoder; + private final Translation2d m_distFromCenter; + int cnt = 0; - public Module(int module, boolean inverted) { + /* + * + * @param + * @param + * @param the x and y distance of the module from the robot's center of rotation + * + */ + + public Module(int module, boolean inverted, Translation2d distFromCenter) { this.moduleNum = module; @@ -77,6 +88,7 @@ public Module(int module, boolean inverted) { // if(_CANCoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED) { // throw new RuntimeException("CANCoder on Module #" + Integer.valueOf(this.moduleNum).toString() + " is not green!"); // } + m_distFromCenter = distFromCenter; } public void setSpeedAndAngle(ModuleState targetState) { @@ -120,11 +132,11 @@ public double getAngle() { return _CANCoder.getPosition(); } - public double getDriveVelocity() { + public double getDriveVelocity() { //may need to multiply by the gear ratio to get an accurate wheel linear velocity return integratedDriveEncoder.getVelocity(); } - public double getAngularVelocity() { + public double getAngularVelocity() { //may need to multiply by the gear ratio to get an accurate wheel angular velocity return integratedAngleEncoder.getVelocity(); } @@ -202,6 +214,10 @@ private void configDriveMotor() { driveMotor.burnFlash(); } + public Translation2d getDistanceFromCenter() { + return m_distFromCenter; + } + public static class ModuleState { private final double m_vel; private final double m_dir; @@ -218,5 +234,7 @@ public double getVel() { public double getDir() { return m_dir; } + + } } From f73e644a92fb0874b20f9b89b509f191f5ef7af8 Mon Sep 17 00:00:00 2001 From: Krishna Shah Date: Mon, 1 May 2023 19:11:34 -0500 Subject: [PATCH 16/31] Bunch of changes with custom pose exponentiation --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 75965 -> 75965 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 30295 -> 31247 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes gradlew | 0 src/main/deploy/pathplanner/trajectory.path | 8 +- .../java/frc/robot/Autons/MoveForward.java | 2 +- src/main/java/frc/robot/Constants.java | 10 +- .../frc/robot/commands/AutonCommands.java | 2 +- .../java/frc/robot/subsystems/DriveBase.java | 119 ++++++++++++++---- .../java/frc/robot/subsystems/Kinematics.java | 37 ++++-- .../java/frc/robot/subsystems/Module.java | 13 +- 16 files changed, 142 insertions(+), 49 deletions(-) mode change 100644 => 100755 gradlew diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 9552e07af098a3097db329d1d3dcd5550311b9ba..0ed30ff3bea3ef6956a6b58f37ab142df9a3edae 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oL*0st`a1ONa4 literal 17 VcmZQ>zc8b1qOQ9v0~oM`0st`O1M~m@ diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index f7694af8aba69f856298d384762c1b8056a96cc5..e1d88b9cba04f27225a2e1f5d7584e491bfcea48 100644 GIT binary patch delta 17828 zcmc&*c|a6L)}Lw^;sH8<93tToqGFJqdnO8s7v4rRhY;8Fboan0%pl+q^Q~yq#0v#l zDWj-hyt6T6QNaVnRij2t*aWYQ@xG!EuZYAWe$_q9&@^l8F%vg`n5L(yey`qp_3G8{ zRj<$SSfArjGIT(AkL7fRIVEM{xGeQJLOs=Jn4-*_s!pA(woIKMvHmAzCJguje8=qF z65~eEhKb41%zFcaTFhF!?As^!N1$}hz{f$aDMXATDT8qHqCq&~wwF7Go5-99*{R%x$K!Z6>gKNQCzJvP;*N zg{FX97cA&J1FkBVkn^5UHVAWND!xvF$68^Y!1mq@hquPO8P7vSFft8zqK7zU(SQu|S(1Su#1-@y@{Km&gX z!`zwRn?vFD0|@&#?`7r9k#Ime%p3g2{*o&2LE{i7*1&HL0?+)bo8i1ez`G2Arv5i2 zjD{8xNl|;w+pVmp99#sY#n&&o=3m{zsp#@N92Z+(+889;d{!I_g zz}QRRwI*Yn_Xn++ko~P1W_RIMZdm<|3ALWH3&>xlxDEho7(n~54VY}oK10a%f?r++ zZM|G%k4_CcyF+mxa&kd#NYd;ta+tXNe%xBqJLt^C2)$e{Z_vY76N>;)x*Fk9SuL710-h4PO=S=M08=*-|@;|r^ zWv(DUZeo_jJj6m!bVfHamB`GL$WC^u9&qD_?+0%3>gC=kH}h=$E;Ksy`x$f|RC*TIXSIPFKK}P!zbl}ikHwssMVPZP!?{6i!=?Pl&})YTbYDDwQ&Ru^ z?Wm=d>=CQC&R89@y~o3uh3pUz+yD{&XTMOK`h92LA&E1M-}XJVV%1`9^i8c8*(VVJ2P4Ps z?XseON22?=fH#R*9=Y6)|8NBm-j2h35wt#MbgPbKi>@!tR(^RYT)xBF{X`2;1dDoO z!2uH4j(5K3+`|0B(*AevziIH@ujR1uPg=qm0l?co+@q>C<55_6)e-n$?@z-9SbyE+ z2JggxAZgWuynj7`nwua@TF+_@jzN#vtv|*-ADj2#JH>0SXGXOd9lU`v#t*N8yul_j zoknDt$V{8^U>8%u`!ThCVeR8R9=WLoS8)b7{yO+{sD+xCN{3tjH!_RJqHTR&1D;wOds=ez9p&s$lvdyR}AsA`_dUtzbyxE^^1Uq+c80Nx|H?m36_JP2dhl$ z-cBq`Q*{_tvtfT%_sYGjz}u#|0*ORBuxwc^h>VZuedxJcQrm~6BT9T+u^#MBl)$7f zax1OeG6cwk%Y_Nt77j!EgUJk8HpNT0Ho$rq8V|hl%jY&*35?u;Vi^~=UV&|6Fe$UA zd=e~(!8FYH?GwC&o(_tb2^H;_@3yyQJa%Mz3EdoQW`BlVdt&XG?41<6-4knv0*}zy zVO2cv<$gr1rpkm4&;Gm~4voWNnA%+#%^aGsRIF&`!|>m|v93(~?(s6A|H8^8pab*M z?gj9~K#&YK_Qg6u*TLXJX6L?1GNI$5khsE2=%HX88~oTv*_x3Zkb=id+&&Uk4gs&g zUh!yxLhnRcU_{>*``$v|WvOZ8JXHaPq)hTARF^wkEGADg7nNkvI))Z^TPcU{R zHX6pP#0;P-BmX=Yj?Ke7q6nQ*t-)0WxdGQ{^$ImO_W9z%{@mq{E?YLWS~Z=CPNC5nrTWKn>^9fEz{y68yc-x zzx?ru(DPqayG4^bJ_3INUhXLTb(Bg4v2$(nI;B+L6t2?Xq*A4kE3{~uq#UOSJ#Nsc zWR%!nb46!EubiuD;6o)A*Ms$f=h;`*CKgKi{}ECF|=aiKiz&EbdY> z{>erRG|FOrNeZya@}EW>;K=S;9r9=$ZlH}i1))%A4MwG2rPJaXQiC_~Ze&fgsi(Y% z7qI;}H<?B_1yAdWi@N<}A{ZL3934T0T6{f5)1H&k~BM?j09yUR$HB$KBs=(z z_GRz_NMbfLE0WFLQ>F#BE$Vf@Ma{MEy!#fa=YT(jkBe5b6`v1AQYg4#QmqqTStE$K6BsxK@Qu_f!XnZ5!GkiTE%&$|FcvGXMBu^ z*DI5_@`*@(7&Kyl(a(GtI2{C>b#RTxKg&^~ z8_$sqAuNqOcUTv}@z6rm&UqD`#+=OV8n}K&l*6#f7l3t8S%bwxn9`EdGc2TidL7>% z9zCnqqCS=8GS{gs7ZQ%zN&lkIR~)q7F!4Z{S*9#9#bi#l&+^izcfz+{bRYZWq7w@~ zIQ^P0a1{NG7e{w6FgUQ$8dC|g**?b&*YZ!+dd;fud~H5{b?s+~j)n@Y_$l&%uR3VQ z=Q143FZVmNbCGR{+pJW#JwIo}annadRY~81zXZs^h4zPgjbEV2Vz&mipwKX>!fl$@ z_8vQy)O~W!(RKC5NGJ_%`A1E^el{R+i`54 zcVJ-<857AyQ{2NQL|uBVRQugpvy_yWOa8)z!3TCU#5~*LJ7;5o0k-|TWVa-^1={==iCx@jM1;jf? z1uJRFfD;C-3deCur6d)!L9WrNR2m~m;Diy5cm%@49NQ3qWu~*x`gW1!uQ{60;;lV9 z^KbeXK9Bl7%uzTo2Q@ofRGw|!J##Q0jyc9t2G895N#U>6ug&xP{+nOd^3K;z zfWR^>b}lN|fNM#kO0QSQbsAcyS1WaLT&qE6N`k^EQUQC;#X5>%Ly$HmW|>lL3_)!6 z*kv=%O$&AJnF@Ai$5u45U!oAjF#1gp8)-&S?dM$B0a;D znnfhr3}iYSx!g2zY4R9aGxlLkUB$8j}OEx=wA zW1@t9Zx1mU`ev9Wp-T>I6np&7_45*rd|9w;=ArKUBu{%aZi`|d#hhvckUYoJpb`P0 z*Qf|uu0-?GD&$IoL2FcKwR$~4P)b<801JF}D7%w``N367uvfTvw$^1zXlm(l^N7Uj zSBi@dZQ(ncvw%?Ti?=1}bd=Uet8_-aj=<$QRO>+V!D&*irxd7;X@uoDSnwZ;AIeGo z_(G@Zou7LZkQ;|?{{EX)mHge%2^dJ|i}vh1*q~C<8k(l%Dw-lSq=KMy6t30F)k?Vm zmGWS(MVK%AtsZlKw&2?rVu76CmP6a`PBqWDC5hjyp3*x!2{<1I3D4)bIZ|duR3H^$Y*DuxrKm_#Ux?M>V0e z{yYW}`m$Z)Ax4@clv=&os3-IaoX{8)T3W9mX;cz6C{XbXV!2q@vxSb$#a`uvCf)OV z;qQmPel*ByQT&$Koq%&OnhLEn;(8;lqbOQWAv##CQppWy_tw%Xqt0kl8sI@Bv>3vn z&!~icV@I0O-ZUlC7W2TNZ*fs)!L<%aNz|rwrPEhEUAeK( zDI={u{q*_iJX)R^csuk#}i%v~ba@+{dF@L1_z{L%k_x;6u%s;1Pzr>Wt?7HrA zv-p~K0tOQL&)mo9En;ajxsTKAI*#4$f7K0{cA1uD7i9h?cUNS>t1E1+oN>L{fh&=} ztV=Z3v|J{3VspT;ad!`BG!lf zB7sbJiJ|el{{lA)#GGpQHw$FKiv@gihYQeR(D|iynXuB&SG&XLV$^Ogf6IiWcYdxZ zrXKDSyIbWx^t?(>Qlqz0|8&xzy3xW?%7sVXtk?n&MBw(FOWwHoDwv>k; zieMCT&%j2&@wrWzklN&%NCfdGCbO{N@`%4?gd;DrZ~{xmYE(!B+UXmoQfVxbkhSB@ z#Rg?&FpyII)&dn zgq=nasTOnpcezeNM?Tbp>oxtMCn(@S@w*7)RSDN4x6mDZKD~D#)@~##j4*7j;Tp}^ z)~bna-Zqh0mW+{aT4fQ2sV!WeLrYo$$bRF;X-4>sU-_MUq`nQi`hYwnZWE4G(u`y+ zy2>`rEw1$CJkQQJNt#hkTV}E5KXv$AYtHewmI)1?V@R4&M2B+N*3)M%wK_|~Ig6h` zOPZz7Th^jJO!j?TI%f_0wB>#`>8S$t>Dqmj^!jS{DWtBObSd{VvMx$GeHr_1cAZ>$ zW(E7SvrZxXl^f-59WK4Sl6}|dL6r39Qub-|gV$ZTW^*4Xq}A)#SCtRCNjGrs?xA;Q zirIIO_1&aL3)!bJ^|*A`XY5mceWdgZyH1$B_3g5SbnUKttP-nc9(3bWgOWA$O`>g0 z3i;Gs57@>_7tePA3z2iZ1K|i)?{iz=8!VNlf$=N4Xdq zh!P`M_%xzM!9rA2#7Z=Xf}J7(OJWqvd*{x=j%&%Hd7kf|@I36!%=w*j&pr2)JLm5A zD^0#%X|i|h$Zq{N#L1#0G2RN9I7SfVEsGV+5ep<<2|nIpkq+|*sJM~4x|M*yLHsES zQM`g5BN8PF_(8IW1c^whRPY03LJ@zIAW;D08i0kDP!ygJ#gCVT%i_f{UW`H})!{5V zcqAhI^!?wjPFq}ZEv>$2XU3_-ULIcFJRhDHAMOzly#OB$4Iymc zp2NT#usEr%RScsVimSYS{8qLzE9+mRA)5VS-A&`jT1;_&uMe&-9 z{ztLSSw_H$T6|U?`byD+uc;+;dsFXdK`bBrYe=DPokbI;JkD{UMW|Gbd}q#`A4 zdGw_z<@jA}zOh6Xc-w?9W!v5DO*IgIf>Iny%b~wD1xe4-f-+^m4ct>th2oT2TJr_+ zcyX*!5T(g8q?De*@eStizvBr*IPMr)zu)1^WWvQ4jZKfl$4kdoMvLQxazVUuEhhfQtm$5=Dk;jVo9y}jUK6L5AYwKOJwA32)s4GQpcI6Qr9!?J&(n+N!-x5CfCv7-%+~K? zhE0=zxj`F?e?0I6HJHWieJrix;lWj7QE3xh!Cz`S($Ql9E3X10WH6`8mK|BKV=|d` zUG%g&^e{t9Sg&UDjF3rqauhc5Sb;G&wziB=&(8870=qh%tgcn?!0!6>14E?G!NQ z(yr9ThH{Pv&l5SrF$-)s$8sC^ie|gh#%A3EDXWx86U7QKa*I%W(>CkGw!B3ZmLt_+ z^EAQ%?mdk}G^jWOFu71D0@XAHdLv2qEm7j3&Ih55OTdky?;w4v!}j)uc_l%T{KW?n#Ae{Awk&jr?>lJ^(nqQO5k?_0xWS`5qb zACvbR=L1)^3%4Wh>%&#aNP&MQ@2ebNGv`}SogbQhY~H5?$!_gW`wrB3zUjxLerUP~ z#dmvizY8_1=ATRblzpvfdozFUkIee!RBYWRC4Eb{(HWJE_9lHRs;K_ca=smmx`8~m zy*VFdMHAlaV9_q$AO98YGvAZ%xp0e|P^E5m(x*JLEYjnUao(Bo{ks!A-jVJ@_kr&C zu`1Iy!OlGiInn6}^lbovB1L=zUm_4o(WVJ);Zc3UZ!b!|AORIj1%Iq8Q8YpppCpJE z@@NFfF`R0_ZQl8Gj`@mYouwO+1Ho3r)R%tcQTAn+`f4&g!$Ud#%~0@_B8rr2xG%s`RY?dUnotl*qDpg(&OM-B)8Nu%rue zBYIJ+-zS5TF7W{OKa7D3?*K2A>cihBNRbe(n2WZ{5(PgfUYsZjK<_pxz4Cx}=A&;d z##OYWY<;plQWtEafQUyrcgjx@DfYPC=vlK;~&T<@bI$sLl^oO8JpTi(I_n@wh zvx9lJD5)Oz*Ijojikm&2>ytVhS#?j-j<1VXC(N;Z^;cemJ z+1TfivgC$_do)9VE?9%3YK%-EL~j^)a2?K?b7j(H-Vg7jx)|@=6U)8v<9!5%2M}Y8@Y7Tn4d~sWuEho9Dnuv zWz4U91YEo1WS)?Fe=OWf+XGeQ17x5^HAPhF#Sj}Pc9Vn~;2&~~2Z zUzqW1Ro1c+2gmwL_ucYt;&DowrZ5Of*1%J8VXL3i;0x2wMV zyntb)&3K8n*nu=wsHhB{(Lc>#swqeuD*L_EF1!439bTPUxVskE2Mh~((&6$MvYen+ zZyG$#vg2M%+^h#yszO60q-Jl^y>US&h%OTrU0!%j=a(Xk%l1L*k`hdCp>rKDGg$e6 zUO&nf&f=jLuE}+PGx-lZnVV*4NuMVv*y7ex+@_1obtT}pAxc_Km%#P_dzI`qZ zoZ4x65PIZg;oN)Mi}Mu=t&euQUl?*&V8Bz{vj_)!jD^#(7nN5i_u zzzllUqY%3M8}KX0dJJ5GiJNFl*7)W@c4Ky7zc5ej@UD^PkBM0Z$kY4XZFvSmm6>T-G4$JHR0ko=A0+80mq$-3h znAhd+dHFDH5Mgew8N(o(B@rGUL|B2xADEu1a;Hb+3gsEjU2{+mZt*3IRkFKlB^!3~BSM&L|DJ_c zS%%I=R%V*H2xL-mM=hwbLq^0I#%k~Oo>n1q9|0E&lB1coz;0R$+&qM^Ax>%|$8;#s z^{wOv{f82++VM|7nZ}IdXy!m#f+ZzSvmj1sqlMEt-~_)LMhpe7@Nx!y$mH+Np(IB$ zhf=bm&P1^m!&7xfoy;`TEZ9j^AMMR-VM6@0(uYarBu6tX!(;1!E%@#4g9Z*vN4us* zEr5Fh2_LN_hK&YJ+Qi5^_hNX~BuBGkpe<7m;uvGtkPlggB~-JxkQ~k80@H>QJzJf> zrG&8g+d$A}L!fwVmI=mPCFpbieJnWR+% zEa;ur?%`DS1L648^o6+Yb_LTia_jWeVJhMRP$288+ znTZX>ztp?ky|XX=xg!(bDJa*s_s9gAOeNetvU@h@Q;ti9wYzB+@JWXxW7r|0UDqqC zH?V^ZpCkLcr}bq8rC$&|8TVzkzUcD~-&Ur#jyi_Cqwj@|IegiDF9#UPCvZb~;+pJU z%mIh*45A0P*>=dY0KT3cr5O%BVDko{NZ4 zR70f)xY`Eq3o8~8BjD*-2+q95zx@{Q)+-XLX|H{e`S+m3qP+*~{~ok3XOOG|_FnC- zG`Q{k?lSZ5@l5+p=KnpO(dI~Abv&a^z82b*od36jmS$M-n&5tqek}#?74y3(R7U2s z-L}<_NcAJSa-pt2^Cw+T`+VlXYi<~k3F=^+Dd`S}>5&W3 zE8KPCQ3K_x0}5Z?pItr=m`r4oS=aNe0bFK1a#GIeXC-yu3)ZlkP;Ad=vE<#*~ z)%xUGYaZ6nGP~+Gd#j=^1r~0ucWMUR01Q=+rnUmH)J02rl?ZS2IWVB{Oys4KYu+lL zE)P**l|H!OutW^LL=#TZ5Sjesx;h&A)lwX_@)zmv+(F#`# zSWhce$xJxfkd&hdKRkD5#BY0Mq{p!0Tx0RA=I}Dq)bChP@IiYPT7h)Qi`DE@&Ple#W(d@^s@~v?7ViJO=exsX7oNAlRML5 zd(n2&aT&F{`(NzBM$mfSYLguU8%#)jG{Bm$8<)-)?qBX(aQw`^7xPx3_p#RT8O$3G zHh02IgNKGWP3ofTx2^c{?U8Y#i{8SZflWCiRJp{}bMx$?&6+~S-3+0?5trL4Dt3uh zAYRQ%eZt%L#{zR*S3#iHHoYe*Amb7o4!H%n#N2L@`)B(X7G|0Q!70V&K>*SOc#NUu zE1!kj3$cvsl&3Si!_Sf=fC*4;N^V2E^^^8~eCXEXO>L=M_|Bhx}yWkDJt%a+sh??Ktreh(%3lYjzNKdMP^;A z)C-t;%iu~-(HA@JeJSbb)jf}mpk*^tcZ@#I-n zRZYp51yWh;R~xL7s5JSH{d=0jBL9ez1s8|pw%ULu(`%3cS9KvfAtMAI$w`~uEVaEl zdSrZNw1EiQpP{zlj2Lr>dpW*bxoBxHmDVqPf%}NrYA6bBwCqhTNNlq$$w6YP_d7l+ z_^ZmHAJ=5bqcSsFpr*E6nJ{8gjGh`QbBj(@BxTzh)Ng#HhMKf`QwEgUV{H%o^?++s zdM*C{oXE%j9mYSeQh}y2+M+9!w!YD;H_ZAyr*?u(#=z7&VWn!Qk=n}erUN!tv3X2s zQQtjgb=`sS#sB&^ zEgrJ5?EbxlFJHLy7~jH1sExMEi8NsXCMkQ}zO^;V@j0nSn>}o%)v1g@18~$}8>2hg z#xYkiGIPGVv~27CWPa@Wp(woDsC1lWKwB5mkP%zyu{&zt#lF0(Ep>-#R-7%t!O8%z z*jBhP5?fm^xYNcJ_k3pVhq(sp9+;`I#`FN&8HthGeAWz8b`4Cu{K9Yg1K%ll@a?TS z?m%*yzTRA9Yni-f@}asRU%U8zixE4pW{!zsp-9%|-Bm@`imUQ+4=f5jeZjE*B2>C) zeT>;44L0O%Bqy=jg?qpM!o|JQ1DsOka4AhT+9$EA=k>tOb8X1)#hYr<+hXZ|lV#H3 z?)u_QR^d4)8@D0oqNcu;AQxIXHf5t>SnPjNUBJxRHQ*czEkX66l~ZgRBNa38FIihH zcbPJ#UUjYj1w50q5Bwm&qZYp0jvx6tj=Hu0Ddp>Pk!4T@dK zIHVl4MS7xTdC4)CJL4{x-ph_h1s(mcoguTXrHp_7{I#zBK9i*V<98PytUKBLF&jaL zSrx-P*Nu#0EG50-H2Xzv`SLH$jHw*Ovt5h=0*j{+O!;rURU&1kaIf*c{%uXDyrt@* zWNTms3$3Jgaqe&>kCa(4Mhd-k^0T`M+l9HuEDNQ)@}KZ_jzQ?s2sQ&YPR)0Q&PSW> zeqONeyPR7$4rji>fDq`^m&`_<9FVr8`Shs8JG)0eyCttrHUyYJe`GQ#%+IZIoWYRm z=MIgzcp$~{_IM1)r00)!!W14`ZQ|P>#Z;5_pn{2r8Jr%bfenyVrsWclULBJWjI7;1L7m zdXdQp>T^0|q__U}v4!JaiA#(Zm9WWlB)G#eFLExLxWBd0YDI#H&@gvI&sE<9qXO7Q zMO6!rzTBIn6%Fk>F5ckI^=qkuP3u;H6RDjZE^lxTTP%q=+jny3x_A6Bb5kN^Mx diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 53af6a96b6126545eeabf2628bb9b18160fbd24e..4562e346f4f8e80e88c83065c8bdd32bf53e4d76 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oM{001*P1ZV&N literal 17 VcmZRcbt>7YGeh$`0~oNL1pqU$1mFMw diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 32c23a417296852aaed361257c78dca02869ad63..a6ece5318092f88b3e69e0f3257cc511f3a0387c 100644 GIT binary patch delta 1461 zcmZWp4NOy46z+q9Ahfi+BISX!XhD%tGaNZ;;!jK6Fw52Uj83>v>h-^Z~kYxTa zgY1wLgz{rxL6~AiCkO}~Xk6H4Q8P=BNgQryWCJE5;&Au9*CmU)$$R&E=YHoq=iGbG z>lbdIh|FV9!+kq6q)LUeJXu&#_{9&0 zgAPooG|&E~NQ1XqX_8+Ylokd+pxoEc?vL@9qc=1hP|FGaFXuK{mwTkK^0KU-q*h}C zt*zlfrJN9oSeEIJm)3|Z!-fU(cwDq?(TCQwj{wuL0H#%1WP$NQ0^AgvgwOA$NGA7* zW<;L3zSkWFb7>E?j`r}DL%Jdb1y{d-J_R9MXvq0`kGyVwsCV4c)}bI*oBjaJ-nQBB zfQtpdYNfC6@k5U@qj3dT)+5eBxLF{inL4x;XdJw?4;0EjknRa4ygovJ;0O+f(?UoC z2TW;0OsmW#1wK`Y5#TpMI|W>0VVnXi2DquoC3Y;lrxv?frQN-h?ugj}>I8q*QM5$w zqszK%z>GKysBy#>D4`>XfLS#Ueef*ctf5rygLyXrNg8C8c6Gsi%DN&E8b~0WNrVx~ z;uHb)=_s1aE|iRh6;4_WenJd~_;ga7{R%!xCmlM4P(uQl_aqEcz)c4SlL_$5AmeiG z!^RAf*V7B>B#?3W;FT2YfU@RhLkvpr0Y&7;!#3o_%_Q?$Y=yu~irfnd3b;GLNeVdL zgnrs`31&0J606LsirS8A7ddDahoDy=di=L!x z+%TfEqwJK2#3~JWO6f72-beEr8RRk#q&uozfTO%9xKIQ*5h!h z24%24Z9SL{5n!&-3gfxG{hRgdrM1p|#RX5Ud`^drETFyk?S>H%h6|Qn>{*t#nKE5@ zKW8ll-La91ZKPdLcv@;Oi7=iky3)23R~TXJGL9tEqTQtOX6hNx6pceOO>6G# zss9oqcj+609~&c}q}c-wH?9_b_t{JRlbYpSwYSUPO-WX=$V}0}y+w&InCHRNgbPZ; z>^96-KP9k^4yV{Se^W7W`>e^=pblNe>C$?F_zs&YcdstKc2ot02}7WY&Pr#EWDU(I`6 YV|H&~p?)C$#Oio(Ya=@Q+m`kJ0BMX3rT_o{ delta 1220 zcmZWo3v5$W817|N7%RNu?Okod11}~HpZ#sPp_rvsm6meywL!4K zq?j$0AEwT$ea`gjDgU;>z;MO}&T!4D;SKn|NP|l4ptbUceA5sYp}{~)2Y23_jue$cnt3R#F&HhNadb+ zi+4fjkZ{2*X&cLytieEmj5}^RfuMZ?(c4*+j0A)?$tc)Py}ifm*v-9Zf1-DykntJ(TVy;|%<^WQ zL~1eP^-jRUKrl;(Ckm{O(5kaakwXjoo)aCXkxf;G5~eb5L~;oye+wrED>HGJgS2v- z=OBAC?v&WF)ZqF)-u$_bMf$d-(?>GB^Knvs;W`>*WhA10W0k{)U_ zi8mlN8{zY^hOz_rJzqvlzQjtii$iC;s+qJt*dH>e7v!OV{f!G&s!C=8*K6jirtgO` z*7`j~^Up4|OqT75&m*t>!ZQB1%yV-Pu9DE$DruP$gL`CzTfLjEtPVM^UOL>bJXQZ! zf6DBaW~$~d{eK?Amm}v~O^)1K(UYl$Cr4kOsd)4SRJ5%nyJH2TPE~Miy52>&-d0E# zyYVJg9Q%Pl7qKe{|Ct31Zqa6!UQU(!bG*;a(fcpFzY(} z-EO1c>Dfr>V8B`d*Ay7R5e{te2zS_Y^r(xgXI17RoFgH&lkKQ&9TGd4aLKFK(#dIm zaGJJCgC2*3K2EbP9Qw5LkU&dn@{zb$`h!;YEx^8RlcnzB+|57j^nZ2XUHck?So6>r zqK=Z!V}vB83y|9}RCl-AOQNPfYCUl$((Xndm^j|UhVY(o=z33J4xL-z%2dPA3gWsY VG9e0dQ

e^!@VC`TqM={{pLdwh;gT diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 63f40c90fea129f70b809a9b4c9bfc12dda7d4cd..2643e4012171a5cc49655f862ac626ab4083ca2c 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~iQy0RSwj1U&!% literal 17 VcmZR6A~{9FsWEOA0~iRf001la1B?Iw diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index d6d41cf25ee3a3ae79f73d431ce1e6c7522c5d30..71e3859ae9a0b10b527bb0aded9660d5210d0594 100644 GIT binary patch delta 1476 zcmccqhOz$(;|3E6#`4Lg5-S9nSFBhjHP!D60~nOMPQEG;AkgunC`Ny>Kovx=ta7rf zWP?EWHnW34)^pZF1S{W8o+`OPps%ZQ=Us<@l@P%Sv7w@)gBh?a#<=GC##kb1{dwQ_yL8;D?~BFwEatTp%K z&Rwg9iaSrX)!87RShCS%n&pid5Wxz;$xn4G1XLdEQ%X_bse!6&-z=&dz^vi?^+8{|nW= zdNZqe1fxLp>9xl^t!iVSqVFe%S`;xUTW(am!L!-XLrhN&6upu_EC|8^i(fvxnI!Fd zK3`W_XLz&@rn8YREegoJRnn({pR)N>F3uS56RrK z1XD#$zVc*_i_;EY{BF61TcTDOQ$@qZa-pS%o0ATi#B7@II8zg@!WzhexK7UO91pL4 za_+jy>&s_9eftou1g_#x(w5U*fA?iIMEPh+^sn!lywFRSpReubhjuRhCmSzYXKTK& zoxCtdxPHo#Rq5Z(x@X?2i49%wP!H22l?Uc;Ewn=RInLSd%$2LT5mUt)eSf)STb?CF z$Un97zT#<%siJ$~glT+53>ot;sQK&6(YlIIQ4jNyLsqiuB~Ec}AJcVvYG1jCAe6vV z1oMVD+*gm&JY#AQ#sAy-Af^hA{=YZnp4eo3W%zaE$`r0-OcjL>zdtYe`LgRc>r}>< zTdwOd0PRPM!WFTTL&C(@bi59=ahyM&l^xTN_h*B5K6+u-`hNSLr?U+cPGhR*@|J3v zEO2o30qdFWvikp4V5(qUv||24#l4s7^%q}xbUDoii+^Vv+B^N6Y?zzc+oMzU49o;D z4Y_xlmrWto=40f)xpU8*pZX3{#rs#~t!FixHNG87-oeN5Mh-(oq(C9J^IflsKPOpL zciS$Dx_ksfiNM466O?|uDts&0IxYEK1&ar!ir+W zlkFg3vwk6e1d|mesI%23Z~~KEDM^#QDe$w#Dz^fYl`Sxn^)0BAwJtET{VqrYqjfR^ qlZ`b=lkPPyvo$vO0kcIoIRUelIZpwT{W@v_Aepftcn`BdNER-VPdFw3 diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 4cbf5590d8ac580ba318a6aac73b45dec4ffe141..c81e79d5643034bd6ed7a02045d3660d225cd987 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~l~D2LLqe1fKu^ literal 17 VcmZSnIL9>cl}9%Z0~l~{0RS{l1Ofm6 diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 54ecb8152759d13ac78e030f24f6dfba9f4825f2..d26e3b15142f0e0b683e1c70b5b758704926410a 100644 GIT binary patch literal 8 PcmZQzU~Ipce?}Dm2_FK& literal 8 PcmZQzU~E4z?cQ_%3FZS_ diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/deploy/pathplanner/trajectory.path b/src/main/deploy/pathplanner/trajectory.path index 07e0f93..1009766 100644 --- a/src/main/deploy/pathplanner/trajectory.path +++ b/src/main/deploy/pathplanner/trajectory.path @@ -49,12 +49,12 @@ }, { "anchorPoint": { - "x": 7.228385239319803, - "y": 2.1047175059541456 + "x": 5.6640442500566275, + "y": 3.0248652771945976 }, "prevControl": { - "x": 8.763408508601643, - "y": 2.0492488249029064 + "x": 4.742234572926548, + "y": 3.0248652771945976 }, "nextControl": null, "holonomicAngle": 0, diff --git a/src/main/java/frc/robot/Autons/MoveForward.java b/src/main/java/frc/robot/Autons/MoveForward.java index e9e7f8b..be6792d 100644 --- a/src/main/java/frc/robot/Autons/MoveForward.java +++ b/src/main/java/frc/robot/Autons/MoveForward.java @@ -1,4 +1,4 @@ -package frc.robot.autons; +package frc.robot.Autons; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 08cb1ea..0e365bf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -29,15 +29,17 @@ public static class OperatorConstants { public static final double joystickDeadband = 0.2; } + public static final double wheelDia_m = 0.1; + public static class MotorConstants { // 0 indexing public static final Integer[] driveMotorIDS = {11, 21, 31, 40}; public static final Integer[] angleMotorIDS = {10, 20, 30, 41}; public static final Integer[] CANCoderID = {9, 12, 8, 13}; - public static final Double[] motorDegrees = {91.494, 182.373, 347.783, 315.615}; + public static final Double[] motorDegrees = {91.494, 182.373, 347.783, 102.129}; public static final Double[] angleOffsets = {0.0, 0.0, 0.0, 0.0}; public static final Double[][] angleMotorPID = { - {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}}; + {0.1, 0.0, 0.005}, {0.1, 0.0, 0.005}, {0.1, 0.0, 0.005}, {0.1, 0.0, 0.005}}; public static final Double degTolerance = 0.75; } @@ -62,8 +64,8 @@ public static class DriveTrainConstants { public static final double TRACK_WIDTH = 0.470; // in meters public static final double WHEEL_BASE = 0.470; // in meters - public static final PIDConstants ROTATION_PID = new PIDConstants(0, 0, 0); - public static final PIDConstants TRANSLATION_PID = new PIDConstants(0, 0, 0); + public static final PIDConstants ROTATION_PID = new PIDConstants(0.1, 0, 0); + public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.1, 0, 0); public static final Integer wheels = 4; public static final boolean[] invertedMotors = {true, true, false, false}; diff --git a/src/main/java/frc/robot/commands/AutonCommands.java b/src/main/java/frc/robot/commands/AutonCommands.java index f8234c1..e2769fb 100644 --- a/src/main/java/frc/robot/commands/AutonCommands.java +++ b/src/main/java/frc/robot/commands/AutonCommands.java @@ -1,7 +1,7 @@ package frc.robot.commands; import frc.robot.subsystems.DriveBase; -import frc.robot.autons.MoveForward; +import frc.robot.Autons.MoveForward; // This file will be used to store auton commands (activating intake, etc.) public class AutonCommands { diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 8767921..2c159a3 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -2,22 +2,45 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.Robot; public class DriveBase extends SubsystemBase { private static Module.ModuleState targetModuleStates[]; private final Kinematics m_kinematics; - private Pose2d currentLocation = new Pose2d(); + // private Pose2d currentLocation = new Pose2d(); public static Module[] moduleGroup; + + // // Locations for the swerve drive modules relative to the robot center. + // Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381); + // Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381); + // Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); + // Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); + + // // Creating my kinematics object using the module locations + // SwerveDriveKinematics m_sdskinematics = new SwerveDriveKinematics( + // m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation + // ); + + // SwerveDriveOdometry m_odometry; + + Pose2d m_pose = new Pose2d(); + + double prevAHRS = 0; public DriveBase(Kinematics kinematics) { + m_kinematics = kinematics; moduleGroup = new Module[4]; @@ -27,6 +50,15 @@ public DriveBase(Kinematics kinematics) { Constants.DriveTrainConstants.wheelLocations[i] ); + // m_odometry = new SwerveDriveOdometry( + // m_sdskinematics, RobotContainer.ahrs.getRotation2d(), + // new SwerveModulePosition[] { + // new SwerveModulePosition(0, new Rotation2d(Constants.MotorConstants.motorDegrees[0])), + // new SwerveModulePosition(0, new Rotation2d(Constants.MotorConstants.motorDegrees[1])), + // new SwerveModulePosition(0, new Rotation2d(Constants.MotorConstants.motorDegrees[2])), + // new SwerveModulePosition(0, new Rotation2d(Constants.MotorConstants.motorDegrees[3])) + // }, new Pose2d(5.0, 13.5, new Rotation2d())); + targetModuleStates = new Module.ModuleState[4]; for (int i = 0; i < 4; i++) @@ -34,34 +66,53 @@ public DriveBase(Kinematics kinematics) { } public Pose2d getCurrentPose() { - return currentLocation; + return m_pose; } - private void updatePose() { // updates current position of the robot - ChassisSpeeds currentChassisSpeed = m_kinematics.toChassisSpeeds(moduleGroup); - double xVel = currentChassisSpeed.vxMetersPerSecond; - double yVel = currentChassisSpeed.vyMetersPerSecond; - double omega = currentChassisSpeed.omegaRadiansPerSecond; - - // TODO: MAKE SURE TO CONFIRM THAT AUTON WORKS WITH DEGREES - - currentLocation = new Pose2d( - new Translation2d( - currentLocation.getX() + (xVel * 0.02), - currentLocation.getY() + (yVel * 0.02) - ), - new Rotation2d( - currentLocation.getRotation().getDegrees() + (omega * (Math.PI / 180.0) * 0.02) - ) - ); - } + // private void updatePoseManual() { // updates current position of the robot + // ChassisSpeeds currentChassisSpeed = m_kinematics.toChassisSpeeds(moduleGroup); + // double xVel = currentChassisSpeed.vxMetersPerSecond; + // double yVel = currentChassisSpeed.vyMetersPerSecond; + // double omega = currentChassisSpeed.omegaRadiansPerSecond; + + // // TODO: MAKE SURE TO CONFIRM THAT AUTON WORKS WITH DEGREES + + // currentLocation = new Pose2d( + // new Translation2d( + // currentLocation.getX() + (xVel * 0.02), + // currentLocation.getY() + (yVel * 0.02) + // ), + // new Rotation2d( + // currentLocation.getRotation().getDegrees() + (omega * (Math.PI / 180.0) * 0.02) + // ) + // ); + // } + + // public void resetOdometry(Pose2d pose) { + // SwerveModulePosition[] ModulePoseOdom = {new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)),}; + // for (int i = 0; i < 4; i++) { + // ModulePoseOdom[i] = new SwerveModulePosition(targetModuleStates[i].getVel(), new Rotation2d(targetModuleStates[i].getDir())); + // } + // m_odometry.resetPosition(RobotContainer.ahrs.getRotation2d(), ModulePoseOdom, new Pose2d()); + // } public void resetOdometry(Pose2d pose) { - currentLocation = pose; + m_pose = new Pose2d(pose.getTranslation(), RobotContainer.ahrs.getRotation2d()); } - public void setHardStates(Module.ModuleState[] targetState) { - targetModuleStates = targetState; + private void updateOdom() { + Twist2d twisting = m_kinematics.toTwist(moduleGroup); + twisting.dtheta = getDeltaOmegaAHRS(); + + SmartDashboard.putNumber("dX", twisting.dx); + SmartDashboard.putNumber("dY", twisting.dy); + SmartDashboard.putNumber("dtheta", twisting.dtheta); + + Pose2d newPose = m_pose.exp(twisting); + m_pose = new Pose2d(newPose.getTranslation(), RobotContainer.ahrs.getRotation2d()); } public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { @@ -74,10 +125,28 @@ public void resetDrive() { } } + public double getDeltaOmegaAHRS() { + double delta = RobotContainer.ahrs.getAngle() - prevAHRS; + prevAHRS = RobotContainer.ahrs.getAngle(); + return delta; + } + @Override public void periodic() { - for (int i = 0; i < 4; i++) + // SwerveModulePosition[] ModulePoseOdom = {new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)),}; + for (int i = 0; i < 4; i++) { moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); - updatePose(); + // ModulePoseOdom[i] = new SwerveModulePosition(targetModuleStates[i].getVel(), new Rotation2d(targetModuleStates[i].getDir())); + } + + // var gyroAngle = RobotContainer.ahrs.getRotation2d(); + + // m_pose = m_odometry.update(gyroAngle, ModulePoseOdom); + + updateOdom(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 7df258c..1811dd8 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -9,9 +9,12 @@ import org.ejml.simple.SimpleMatrix; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.Matrix; + import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.math.Matrix; +// import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Kinematics { @@ -22,6 +25,8 @@ public class Kinematics { private double gyro = 0.0; private final AHRS m_ahrs; + // private ChassisSpeeds odomSpeeds = new ChassisSpeeds(0, 0, 0); + private int cnt = 0; @@ -91,11 +96,15 @@ public void conv(double[][] unicorn) { } } - public ChassisSpeeds toChassisSpeeds(Module[] moduleGroup) { // converts an array of module states to the ChassisSpeed of the robot + // public ChassisSpeeds toChassisSpeeds() { + // return odomSpeeds; + // } + + public Twist2d toTwist(Module[] moduleGroup) { // converts an array of module states to the ChassisSpeed of the robot //gets the independent x and y velocities of each module based on encoder values SimpleMatrix xyVels = new SimpleMatrix(8, 1); for (int i = 0; i < moduleGroup.length; i++) { - double vel = moduleGroup[i].getDriveVelocity(); + double vel = moduleGroup[i].getDeltaDist(); double dir = moduleGroup[i].getAngleInRadians(); xyVels.set((i * 2), 0, vel * Math.cos(dir)); @@ -104,20 +113,20 @@ public ChassisSpeeds toChassisSpeeds(Module[] moduleGroup) { // converts an arra //multiply the current x, y velocity matrix by the inverse of the kinematics matrix SimpleMatrix kinematicsMatrix = new SimpleMatrix(8, 3); - for (int i = 0; i < moduleGroup.length; i++) { + for (int i = 0; i < 4; i++) { Translation2d distFromCenter = moduleGroup[i].getDistanceFromCenter(); - xyVels.setRow((i * 2), 0, 1, 0, -distFromCenter.getY()); - xyVels.setRow((i * 2) + 1, 0, 0, 1, distFromCenter.getX()); + kinematicsMatrix.setRow((i * 2), 0, 1, 0, -distFromCenter.getY()); + kinematicsMatrix.setRow((i * 2) + 1, 0, 0, 1, distFromCenter.getX()); } - SimpleMatrix finalChassisSpeeds = xyVels.mult(kinematicsMatrix.invert()); + SimpleMatrix finalChassisSpeeds = kinematicsMatrix.pseudoInverse().mult(xyVels); - double xVel = finalChassisSpeeds.get(0, 0); - double yVel = finalChassisSpeeds.get(1, 0); - double omega = finalChassisSpeeds.get(2, 0); + double deltaX = finalChassisSpeeds.get(0, 0); + double deltaY = finalChassisSpeeds.get(1, 0); + double deltaOmega = finalChassisSpeeds.get(2, 0); - return new ChassisSpeeds(xVel, yVel, omega); + return new Twist2d(deltaX, deltaY, deltaOmega); } public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { @@ -126,12 +135,14 @@ public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisS double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond; /// Constants.Swerve.maxVelocity; double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond; /// Constants.Swerve.maxAngularVelocity; + // odomSpeeds = targetChassisSpeed; + if (cnt++ % 50 == 0) { System.out.printf("vel: %f, xVel: %f, yVel: %f", targetAngVelRatio, targetXVelRatio, targetYVelRatio); } if (fieldCentric) { - this.gyro = this.m_ahrs.getYaw(); + this.gyro = 0; //this.m_ahrs.getYaw(); this.gyro *= Math.PI/180; } else { this.gyro = 0; diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 3f8ab0a..2926e04 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -48,6 +48,8 @@ public class Module { int cnt = 0; + private double prev_angle = 0.0; + /* * * @param @@ -78,6 +80,7 @@ public Module(int module, boolean inverted, Translation2d distFromCenter) { configDriveMotor(); integratedDriveEncoder = driveMotor.getEncoder(); + // integratedDriveEncoder.setPositionConversionFactor(1/6.12); driveMotor.getPIDController(); @@ -140,6 +143,14 @@ public double getAngularVelocity() { //may need to multiply by the gear ratio to return integratedAngleEncoder.getVelocity(); } + public double getDeltaDist() { + double newWheelPos = integratedDriveEncoder.getPosition() * (2 * Math.PI) * (1/6.12); + double delta = (Constants.wheelDia_m * Math.PI) * (newWheelPos-prev_angle); + prev_angle = newWheelPos; + SmartDashboard.putNumber("deltaDist" + this.moduleNum, delta); + return delta; + } + public double setAngle(double angle_in_rad) { // //code to make the angle motor turn the least amount possible and drive direction if necessary // double targetAngle = angle_in_rad + Constants.MotorConstants.angleOffsets[this.moduleNum - 1]; @@ -164,7 +175,7 @@ public double setAngle(double angle_in_rad) { SmartDashboard.putNumber(name, this.getAngleInRadians()); if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180))) { - this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -0.8, 0.8)); + this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -1, 0.1)); } else { this.angleMotor.set(0.0); } From 391e84f22958be128da791568724c27b65aeb393 Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Tue, 2 May 2023 11:00:02 -0500 Subject: [PATCH 17/31] Refactores --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27797 -> 27797 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 36911 -> 36911 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 75965 -> 75965 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 31247 -> 31519 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/subsystems/DriveBase.java | 19 ++++++------ .../java/frc/robot/subsystems/Kinematics.java | 27 ++++++++++++------ .../java/frc/robot/subsystems/Module.java | 9 +++--- .../frc/robot/subsystems/RobotContainer.java | 2 +- 14 files changed, 33 insertions(+), 24 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 0ed30ff3bea3ef6956a6b58f37ab142df9a3edae..114e99be1c3a9767178192a7901cf0dfdb62e04c 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oMO1pqN61T+8u literal 17 VcmZQ>zc8b1qOQ9v0~oL*0st`a1ONa4 diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index d4c8abf22ae3377bbedc7d0130290972bcdc39a6..3da1217fea652f4ca0f1cf0389e6a42c8619a9be 100644 GIT binary patch delta 241 zcmbPwlX2=z#trvlSr%)WhfR+5lG^+?mV*h%vz;@!*Gmt`FuFc@KZJ2@@<#|`_2l^p zV5zH3(0A>?d?f?J) delta 241 zcmbPwlX2=z#trvlS!SFz_M05-CAIl)EC&;iXY+7!ua_Q>p|3Z2KaBAa!eE*_KLISI zJNY7v(e4cv;hww`!qA!g+FNRJW8x~1S;r>(`+%fvZcc85FwOxPQj^d6h=4>7Ob!gu zWo-qro`8kth6sas7bjQyN=-fw<=veeoeVO>*mHAl@(orX%V6i`%*-~BYpnw&Pt5{J cS_e)(4rbW=nEW$KPXOqCKL#-H6$Dax05pMO!TYG!!}oEWU&BQwsR)$^)do7jIK{E3^8PF1+uP9*7ODot)3hY zVO*VjFc&Oxb@FM5$kxe>K1Mu1#a9@>!1Cbag?Y)F&*eEXf=q3gtQZD1%y#l*eqX7{ zpM4}i!W}>k$Q;`$uxPw5SacqkGZn(=nYqGb>P&dC=su{!)|s zW5hSJls5|kMND!h3kFI}uI-fAytm^8NR7?2$t(LnHrPC$d>_oPT{~H^2P9%_IN7`Z F2>_LrU(5gi delta 262 zcmZ3#fNA{#rVZs8EHh3U`)#hy$YKGqY#vVD>tzIF=<7``3^8PF1+z50!9q-v;~@;) z$p>@6BD#}LLqxbIGx`|u02S*nfPv+~$qVz6H=oOMWCWRdY_eh)*f5)$llgt6CV%#k z012N1azN(T8~}^P`+`NEfH_kkoQsn;LX_MEGNdMV=Sy$yDRgE9YBKhmeAZuTa(|5Y zW|s11L7<4i&dGv-Qj=>tB{uKvcmYyl9WZ%iAIJvlz{&T)44WU56??#L_nYk9{{#TO C@@b#| diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index e1d88b9cba04f27225a2e1f5d7584e491bfcea48..571870acd0f2afdffb71ca6fb958b3f829ce5493 100644 GIT binary patch delta 8826 zcmeHMdsviJ8s|MsW-2*2+)PA9?g8Tr2$g~Y3koTUpf0+fmyel2KAamf!>Aj0S*xu! zT-xe0yiZoUYXDzm{OLd8wl2X<~{D*K! z=DW~Nq@6G?N~0|(UBZ}+1x7PxR1{l`1`p{|AAG2;I`psaPUWvY+)&x^(Ym_Y)~=m3 zS(k#sXFB2Dqn4y@LIHRfHx6Gb%7-gLq!=-h@OKesDC`cl=9A^`z4OF-7c-lpQf>tQ z!SDhfY;8SPThGC>)59KneS@w*ZHq6_$8!nlgapoLW4UF;`el}4RxkDk7S+m&V7(^_ zY9jJ-6i-rjfv-Qa%*5%qg5ua>*2Xe%%D8yNctspTwz4P^7&18vjU-!lfeb_t4!$U2 z^sKeW5hLFXJmz8+8b#jLfj68N0zItNlqzT#MImRlSQQ4V+Gy4?;)kOPZ?zCss)uMh z8n0>c@nh5$9^`1fPqeP&tviKSSozgMfsC5fYt5`hYqn|4j+oRvLXtH@!5^EU|5rx(^=Uij^>k&^EuaTkdZ&m zL=TV|`{?9+M;4W#kR%$G55fqO!&r+rv&O`ltt*&9vzjjl#%R(S)HeHv=y<#$QK5_- z&)~|p{PEr#8Ch6{;)Yo`y-BBKl#1~xhRDMq0o@9K2@pbxB*ph;js-vkn2FQXBOO6I}@=9cwUX z%?i4bn4DYRr*j3pW`d5`h0h}B#O2IE!DQwET0V{9?6GY!KPES0Zf}H25zpYQd3@xO zg#oxTPX;FidpRFiZ8d8ZrXr)(z%8dU&xxTD6!CNhXo0r#u$oi8gve;Zrv+T-Z8$ph zj!?2-tgr}8zeWKh`yefqF9k5EJP0yhzF2M|C|f8MDmvZ!e1!2Cb9yBCkw<*RBOVE- z*;$PS8)xC@oaYE%ZuZc2lWg$b(==D|Q$7kJ+YV9m6#?zl3yr<&eTFi(K;+9a{6eP9 zV~Oz-I+Ef#AnUC_7lMJcEsv!Y+++BabTG+wZ*lcUK`eaEF94NSp@r7F#a_2KWp<=k z&4n7Rp}UoQIv@EF)BQ9xJ4n+LM4H~9WHM%sw}{mAM0VzbAIXje@i(N_ka1lJH}#GGkTUY(Bld{6w_@)IQd(d)Ka>eSs3!0 z^N@iKIBER-dB{QsMPqq?p6R}nRDg;QJXuBQbJ+xFCr2criq zNIAK*o{{3z1OCLAfrbj6>qO6KPI%5XYD#t83AZ1NR{BIo$#yBuDvQJ6a%d@E+mU%|o#C_hDpaD@Y}~_}|p6ppbmTKfTxrk4O&lPbXU; zV(@JChuN0r19(P$E8OvU1k&43f?{>A8{pPFHa@v%3 zuK0tU?>=LR%ql)QYK-w4+EkG{edjSoRdC~CMKfLmvlG$rA z_u!ysqR=No)fE#}2gqtay9y=<+I97OLa;P%`hqjr@I>Lp9rZ%&+LaRU@OW%{E$W2* z>0wj2I|>kzO*?08`!{%1ETeZl<1$OEDlQ=ocRhc-cMC~Phjxdi6OSU5T_6eURb&E7 zQ-TI~lKvGqYhGlZ79xcMJV))u@j{OEN1`5^&}j=FpNf@ijKnua(ruJ*f9}hqk5Mh1xYB&MaYB66e4m}{(wV6wkM-Z^iMILfh||G zKnSr-LaFF;XUx4PqxsHR&Z29YT$+rM&}nBJ%U=XJ`Ev@IjE*{E9-oRHN2i>y#O-_J z+vh$f5+GZDg~mJOa^WUkTtlFAGe(OqB>PP!%A+aQ4MgpO1UTkIS0AX3`~BhWE1 z+YBe&_T+C*sUv^mpEdqOIu#8=4We78v!!&O*Ii;xgHk4e&j?ScNb=7%o zH#!T&x~3mY>HAXcq^$c83EV)h#p1o(De)lP9o;~Xb{B7iIQPT@9C$RU?`~4{%L!#S z{TIjvRMZ_sVmHyNkju($!d;RL{Bh34Xjkrfyb6cAP9%zVEl0V*uc{it-IW`7F?ci{ zeKOWX%|TTIIqv8UR1hACnNOo!ksRjV6+Jj<4kd9H^kCvO7sa?PI!tg=3r=xGYe;if z0iN!XVo(zWf#k|OIt=~D2L8k_A4P~|*;6*)72Tw6&~0Mwq&_qQD&!{h)C_`14ub@J zE%$}jB1H)|X#tfRg3Hk>-T2m2iqTnnAu?g{F{0i>Z-#tGfN%dE3d|)famAMB`pBJA1s+yC)h~z7^>T^9w3p z-?V>EWwyT-y!pz@{>H1!&PMX}$v;zf@CM*)r+ zy$XpL)mJW|4@0-W*K94xOfAiCh%$0#lzmuSR3t}mT;4_AN#uP{ebD1vv(9$M98Djo znl?O|lPXJ7D!U~;ww@D~|BNhP`$)f~RNqpwb3jL1Mp?5E4UuD$W8dcEB zr_;Uf;5GvyV&c5;ekjqV2fx~iT?Vh@M$cN{(%=VM@kA|%kZ8j)Drk+vAwCgSsG?&D zEgq;vdm|=E%wQSH8yL_g9+bl^+$e+`3U|JaM5^1YFmO?L76c6-r^ZILP z?xST`WKH&~&7Hthi}3qV>I{MS(|s_-yZ6C~S}Dw_!;fNWAv2@LE}CF&7r4OC3@X|L z=HylJF|ZrlUIpTegNJTCpohX9Sou3Iy0{bl{9fQ5>x~idzpOAIhnQ2t=QDyW&oUJ12ta!(HVj< zU;y;dW&l<|1N#M63k5_!;M{0~Z}3k+8}l+cm3^4z3KnEn}s@ zL9pdcTw$cSLyR<1%Sb*$;Hi70?kIDZPjTTC)_V|ZEFS`+=f=)CaFR-^#W=*+xW^e_ z{xGc2Ijo5|r%MG{Hs(!sA@#N=e66J}UFL5~GYoPx&N40_wtJJgNQaqpGZA-mpa^q( zhy}JWMjbZaltb1WOfzSQJUI*kO$js{0V{6%)7&r7X#}jf6Ktp37%5fnAEY!SH|1}| zrw8Kx!ry9fhM8;1;hXu8yIvWoh>*rBWpVbhaA{l|M}W4oq?dehK(uYLAC~L`4Y%1 zJNE}+3%_4zx=Y;1ySD3l4e&oG9tAt@2s0M?kHVkjY1NC}rk6%%6{zDoE~m>^RcQvk zXQagu^m%Nw0d3z;ABj{u8%OqLFs5(zE#-e9I$ZcK51D<;-J6RU)1LAR>N6j!wh!h- z1<%|3%VWkwrPD4)(x%(0;N3Mg*kVk@2W-`z9#8;_o}N^e{59x!#PUA{jGSqBL%*(` zZU5pvc0I22Q^?|@V)t-HsLP}u29ALd&L0PNc%sp}9>Nmw)D8>&+JSquU-Bi|Xg&@%#QDizoUaqMJlLb7^?3D%s;IJG3acj^O82rg-cqW!jlls> zdq0HzJ~rOXf4Ke)f3DbAZo(z5?AG5zqYr6U)K&Te+PaYCxWE?@sZ5=O8Lng%6;;$x>=?ITcTP5D%JKEyp5HVvI?i#Ff#Kah(@&#-G>1UYzeu z-00|)SRC*sjx_gb3V!ZGTrr5^7YuQV2FY4z+;xvY@*@k{mlM-iKs}pe$niA%z1I8v6tM- z1s`o7YGeh$`0~oMn001*j1bhGh literal 17 VcmZRcbt>7YGeh$`0~oM{001*P1ZV&N diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index a6ece5318092f88b3e69e0f3257cc511f3a0387c..b0c2d24925a2002f384642f8be9264c7d9b027c3 100644 GIT binary patch delta 1000 zcmZWodq`7J7~f^HhxnLnKDdF-nj;%JovEaWMw*q*$3(-G$jC=#RthE>W}2zZ%EuR8 z#0So|PJC0S8Tf~PScHj?Or0i{My^aqiwx}Cb0o+PoO^!v{C?m0zTf%o9i8HhPVst! z3PnBQTSR=s1tJv^UX!zTH0-3fm^^55_NxK+YZ%F=VNv0VNVyKI!Y~+w$RZZ_ zRe>~>$N}PZQ7M$|qGA}^MLBRY90$~cmVkTEQydD}Dk0AxUiD)toG4co`9QpihTQvj z+n?#Pq-fBogl32eBeil*L0^f4)kcFMi-wjW;_nU~ zr&*f@@yZGbJ$xLkt{&*&jHV+y`NQiAo+5r!Xq;X_-AStL#}QKXBqGu9_huHYC<$;i z_M+}#kNzR!e6)H0%NbGFHyff%x`Bo1Z)HrR9VK9HTT_C8WKE?7MC*h+)Q?C=)6oOc z>fZ-M5`ImP^xTO&tsabPs?r?Xgufl*&4HCp0%?u*Ot^qTri6jMOtOG-HAAN;aTboL z1`Lhu%KWL8om73v7x$sMX=^1ngp?A#Nkq2nP3^H+*5`Fux4FO(rkMPqGmf6Y5#pI1 z5mv<#s&=syP%U8t|oFgrx==@|xw$K?}AbsXCc)v_LIcZcYG)8oZOsFk)0A z;tI)O`|BXJ7B?uE*%l*lPc80WAfb7BP-rn7!o)ivxebEd|de6KCoK5^EWc);-SNsr^Q>ObsiWZ}_j5Kd8NkvsEsMurWe zFsG$~+kO^(yB=osR+yqoY+6elMi`uG^`dNdoJ&vz)k@T;~n^=5)2)*ow#7Eb=E;4`;^*g SD2fs_kkl*s0n5(ryMF*XwP4l& delta 1303 zcmdmck!9~imJO~g3Ky~#K05tn?p@CX8P|Ty=J?9Mz}Q}~K{SB@3e=HTAEVo8@ za(BxXg-6X@6SJ~vooDub`MIbij0>c=J$y!wCd)~@slSO}Bc69{%VXw?Zh_hI>P2SicJUxS-QJAG- zugv+$@ljF=yI(S`isq>4eQzipEua$^57lxRswHdsYko#G9-#8O5D{i3pdo4?{f{Q! zEti_SG+KnEJ$>Kv$-JF)L8`7cb52G7t+M~Shf|}v=`bW%_T7PK6)y$Sdd5Hk6i$jD zEHJHV)7k@qC!bVuFn@LFY{-Y`*slYW1OmI&)9(v1s!e9@l2b@vTDp0Oif6bg$nDgwHWf9^k+B9c2-dMd(+D%6^{nasGvN5MJeB%`E<@#h(fBNm-qC;J!d?hW&ILzJ`3nVh@CML}lP z1C!eu@_*Ipr=-KbwYTf%etQ19 z{4N_v5}3RCZBk`zU9798Hn~1lYVz|Y5tfGP=B~;Ao1`WmT?yvQ ko_u$ultjf;o=;gsS}(~gRX7nU$w3d0BK3a#{d8T diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 2643e4012171a5cc49655f862ac626ab4083ca2c..801efbfafef1a3e72c36f9c60030365a569e5e38 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~iPy0st&21G@kK literal 17 VcmZR6A~{9FsWEOA0~iQy0RSwj1U&!% diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 71e3859ae9a0b10b527bb0aded9660d5210d0594..06990ca7fde96f08c3528015d937cf07bb70e8b8 100644 GIT binary patch delta 399 zcmeDG!Z`mM;|3E6#`?{s5`m2T6Fq1BWr@4U00vd1n`RHj#po09Hm&$Dr(y*GiZu&m1j3TJ$&i_zg zb9U$L^}HI4Q1QQ;eGLoQ`R$rs7pVwlKm~7a-fFReNu~b!!bdAYIFq5G(_u!yXa)x5 zpBoh=cs4tFoHLvpUS=?Pd!>?5rQr9=hWi)ocAa|QX+7XX5z} T1q=+zmKzms@N9PUIA;g|pnx3x diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index c81e79d5643034bd6ed7a02045d3660d225cd987..c2ddebc64ec8043f7c1fbb5a37af0bbcc9399686 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~l}|1OPP#1i1hJ literal 17 VcmZSnIL9>cl}9%Z0~l~D2LLqe1fKu^ diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index d26e3b15142f0e0b683e1c70b5b758704926410a..92be894e8dcf622dd01c60b888b37aa992a27a23 100644 GIT binary patch literal 8 PcmZQzU~Ipud_x8R2wDPt literal 8 PcmZQzU~Ipce?}Dm2_FK& diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 2c159a3..4f728dc 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -16,6 +16,7 @@ public class DriveBase extends SubsystemBase { private static Module.ModuleState targetModuleStates[]; + private final double[] moduleDeltas; private final Kinematics m_kinematics; // private Pose2d currentLocation = new Pose2d(); @@ -42,7 +43,8 @@ public class DriveBase extends SubsystemBase { public DriveBase(Kinematics kinematics) { m_kinematics = kinematics; - + + moduleDeltas = new double[4]; moduleGroup = new Module[4]; for (int i = 0; i < 4; i++) moduleGroup[i] = new Module(i, @@ -113,6 +115,11 @@ private void updateOdom() { Pose2d newPose = m_pose.exp(twisting); m_pose = new Pose2d(newPose.getTranslation(), RobotContainer.ahrs.getRotation2d()); + + SmartDashboard.putNumber("X", m_pose.getX()); + SmartDashboard.putNumber("Y", m_pose.getY()); + + } public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { @@ -133,19 +140,11 @@ public double getDeltaOmegaAHRS() { @Override public void periodic() { - // SwerveModulePosition[] ModulePoseOdom = {new SwerveModulePosition(0, new Rotation2d(0)), - // new SwerveModulePosition(0, new Rotation2d(0)), - // new SwerveModulePosition(0, new Rotation2d(0)), - // new SwerveModulePosition(0, new Rotation2d(0)),}; + for (int i = 0; i < 4; i++) { moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); - // ModulePoseOdom[i] = new SwerveModulePosition(targetModuleStates[i].getVel(), new Rotation2d(targetModuleStates[i].getDir())); } - // var gyroAngle = RobotContainer.ahrs.getRotation2d(); - - // m_pose = m_odometry.update(gyroAngle, ModulePoseOdom); - updateOdom(); } diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 1811dd8..3d70b5c 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -25,12 +25,27 @@ public class Kinematics { private double gyro = 0.0; private final AHRS m_ahrs; + private final SimpleMatrix m_forwardKinematics; + private final SimpleMatrix m_inverseKinematics; + // private ChassisSpeeds odomSpeeds = new ChassisSpeeds(0, 0, 0); private int cnt = 0; - public Kinematics(AHRS ahrs) { + public Kinematics(AHRS ahrs, Translation2d[] translations) { + + m_inverseKinematics = new SimpleMatrix(8, 3); + + for (int i = 0; i < 4; i++) { + Translation2d distFromCenter = translations[i]; + + m_inverseKinematics.setRow((i * 2), 0, 1, 0, -distFromCenter.getY()); + m_inverseKinematics.setRow((i * 2) + 1, 0, 0, 1, distFromCenter.getX()); + } + + m_forwardKinematics = m_inverseKinematics.pseudoInverse(); + this.fieldCentric = Constants.OperatorConstants.fieldCentric; m_ahrs = ahrs; } @@ -102,6 +117,7 @@ public void conv(double[][] unicorn) { public Twist2d toTwist(Module[] moduleGroup) { // converts an array of module states to the ChassisSpeed of the robot //gets the independent x and y velocities of each module based on encoder values + SimpleMatrix xyVels = new SimpleMatrix(8, 1); for (int i = 0; i < moduleGroup.length; i++) { double vel = moduleGroup[i].getDeltaDist(); @@ -112,15 +128,8 @@ public Twist2d toTwist(Module[] moduleGroup) { // converts an array of module st } //multiply the current x, y velocity matrix by the inverse of the kinematics matrix - SimpleMatrix kinematicsMatrix = new SimpleMatrix(8, 3); - for (int i = 0; i < 4; i++) { - Translation2d distFromCenter = moduleGroup[i].getDistanceFromCenter(); - - kinematicsMatrix.setRow((i * 2), 0, 1, 0, -distFromCenter.getY()); - kinematicsMatrix.setRow((i * 2) + 1, 0, 0, 1, distFromCenter.getX()); - } - SimpleMatrix finalChassisSpeeds = kinematicsMatrix.pseudoInverse().mult(xyVels); + SimpleMatrix finalChassisSpeeds = m_forwardKinematics.mult(xyVels); double deltaX = finalChassisSpeeds.get(0, 0); double deltaY = finalChassisSpeeds.get(1, 0); diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 2926e04..991f56d 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -28,6 +28,8 @@ public class Module { public CANSparkMax angleMotor; private CANSparkMax driveMotor; + + private double oldWheelPos = 0; private final RelativeEncoder integratedDriveEncoder; private final RelativeEncoder integratedAngleEncoder; @@ -145,10 +147,9 @@ public double getAngularVelocity() { //may need to multiply by the gear ratio to public double getDeltaDist() { double newWheelPos = integratedDriveEncoder.getPosition() * (2 * Math.PI) * (1/6.12); - double delta = (Constants.wheelDia_m * Math.PI) * (newWheelPos-prev_angle); - prev_angle = newWheelPos; - SmartDashboard.putNumber("deltaDist" + this.moduleNum, delta); - return delta; + double ans = (Constants.wheelDia_m * Math.PI) * (newWheelPos - oldWheelPos); + oldWheelPos = newWheelPos; + return ans; } public double setAngle(double angle_in_rad) { diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index 48a8b45..1a010f5 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -29,7 +29,7 @@ public class RobotContainer { public static AHRS ahrs = new AHRS(SerialPort.Port.kMXP); - public static Kinematics kinematics = new Kinematics(ahrs); + public static Kinematics kinematics = new Kinematics(ahrs, Constants.DriveTrainConstants.wheelLocations); public static DriveBase driveBase = new DriveBase(kinematics); public static AutonLoader autonLoader = new AutonLoader(driveBase); public static TeleopDrive teleopDrive = new TeleopDrive(driveBase); From 2cd750f026c812b5fde9c476cc35051ec097c661 Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Wed, 3 May 2023 16:40:21 -0500 Subject: [PATCH 18/31] Changed Kinematics, 180 turn code, odom prototype --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 75965 -> 75965 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 31519 -> 32879 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes simgui-ds.json | 11 +- simgui.json | 17 +++ .../java/frc/robot/commands/AutonLoader.java | 5 +- .../java/frc/robot/subsystems/DriveBase.java | 36 +++++- .../java/frc/robot/subsystems/Kinematics.java | 119 +++++++++++++----- .../java/frc/robot/subsystems/Module.java | 46 +++---- .../frc/robot/subsystems/RobotContainer.java | 1 - 15 files changed, 162 insertions(+), 73 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 114e99be1c3a9767178192a7901cf0dfdb62e04c..8132658ee8e2411b9d45e86fa3c83af0421affb0 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oL@1OPEH1V8`) literal 17 VcmZQ>zc8b1qOQ9v0~oMO1pqN61T+8u diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 571870acd0f2afdffb71ca6fb958b3f829ce5493..77676198db0aa6db3ce4c17ff6335a6cfb89d779 100644 GIT binary patch delta 11772 zcmbVScU%<5+jnm{Iiz?&0V_qsf{1&!yH^lVELcFnpiyJM?A`8(B8ZAYEI+XXqcJv= z(Wr=xXe?+jVg<3q5+jKLF*Z~*0Yt@;BBJl?>>VD*`}t%y{^NJ&p6C0Nd1iLzS-$zo z`J1nt4@A7~oVqcEOfn?KC(N2on>4h+sO4snaVZ*V2K!kv;h!QS>GdVBZMN*=jKy^x zZX=2xhc_=OeDm^P#f_iK{j#>LT(aqH-l@8c$2w`$oRKobO`U3pW0I%FPvbsgjnmXK zlHw)_OxWl7K5^J8Jm2{k7VGcYV~|VMyy&ut!MO!TUJsd(jP1z4IJXS%vn{};3T(OZ z{;;Fkh!>i;$wQBnRNqc@$p}5`0U{q_8Ln=sy3k*6_maQ?O9;7r89Z}+=Q|2_Tr=CWIGH220 z(y4XmIrd0IK!m#L$@K8p?f3GaPL>?|3JLo(2r3;%eX_I9@|h+6U>hF4`+N=(d`<$d z8mznB$}H%6L=Dz~^CX&Cc6z&_C;^;ZI)KRJDlAbZ>GGzu&(A+`zyW9IW}{x zJoNWUICO2~{5ZQ=;c4q8wsc9_wH}85)Lh`ySsk)*g1=W zk}Eooe4ZJZ;rT`P{8u&DQ_O>Qx?UR=%q_XIU%6)5u+BYlk-38Wx~JHW1OK(E?H=Tf zJn#YoceFC+{7Q|CP%3Q`4lB0{h#V}-aJ5Y@mcth&U)7Eo%5!qic8F}Ft8G59{pRv+ z4(<(+ZB^Q)G2-=`W~p=C$9$N(x@GA7tPA`k{JJf_#~j((PJX-YZ=4vHU?8W= zA{i4iZL*P_JaGz3C26>cv!)5_-P;9(WXZ;XK3Ot@Ec`CmdJ^*j&)&+My$O=!j3lMi z8fb&Yq%*20n$YR=T8+ub>NHvv*l&kNd9YKa;L3F|F&!4Y{{HJ0^jL>yFSbQsl6mYP zk5r}T*0}Owaq!$0qvsrqE?Yfr!jX~y$(EzB>i02wu`EFmG|5tGjasiE88u_jnpl%Y zr6WknWCXEt{8MPP%xRb|X;uG%w-4h>XJgJEO6HKN5aW@=8gOOF?bpWCNOOjrVU_;13nVQ`}^hlVNY{o_8x8ZVA`Uv zIu$0BL#o0EAzr_a_HEUUcwUxTy6Ko_;4xM*4Igj0gm{4?K2q4cW=35onDFU8!8v~f zcinI$x)zg4!|z=Zbw~(ow42QI?uW3xxAa7*r;XQL}1R zuO=9^(ZDi#R;5xIRVLa%F<__@&ewBkl6JzqNw?(n_S+RdeOpo{nM10Axt=dg-ZP=~ z&cdA^j96c_UsK#pG7S#r7pS6S-au@);oylgZr=)5u8FB0F$7xMm|y#o6$ zJW_SCT#)VFePey1+a=RG&i-BXo*J%mHq4G*&X6sYln`wH8cY|;d@s)4`15m9Z$4zzRYgh%Sfm1^IO^Z zGXvB+^ z2a{h8tr+TVa@pH#Q1f#MDUw}K@Gd~A2Ms&gVwly((FeY{X7Im0bxo)GS?gx2F{_`g z@B(2=tzG*dt?*XQOQ*|gCbsdsIn!E#-z^_DaHoRqt#L=8X0Y!Mj=DOn^^Strkn%CH zi8nD@F}@^h#$2FpCt7CDt6N{{{bgo-dfuh%L$-P@fEf=LFMJoJf^ujLuFI=4z}3=Z zMRtn)RJ-gSqjz97B8?vv?kKfkUZ*G58Sc$J@xpW0oCmJWY?KsyWPb>e6}#a}p=X)b zpFImY6YlgyfzfM1Xu^2R>SrsQE@X{s%z)26> z5gKr*Z6B|4imVzL-*xnLuYm(BiEfaw9Z(3S`hRpsacyAX$Ga+)By?5#?QJ54_PmC) zc}`@KCQ%wfsPggGW{z7GQMT-dIScTzRi}Gd<-yTP&?<+&wMALyuFU=GQ0|*p(_a&( z4n8|{kCh_eMH)@qigq|Zxr0yStRDX+vHh>%L#AiPDYzywwgZkrgp;~?z5Z(c;=viK z>CAaO!{VEW@frQyst(_@$Cc3Dy+dAqX z`I{-*J1QP;s%s)p(OMQcv zHxaYiB>4EeXH8eV^}LvV}+Tbo=KXP0tgBWh zl!UeSl6*%;JZ6u!U3hS7ReCF z%kji~ITVgoS`9^!dJ}D;7`e zMH|&7&O~!6gMlJcMvdCU>UCPc^ui;}J)@{QXU063xs%;<%Do^lu=KceBGfdpZVZYv zkIKCAYlluJYafiu&-*&(&W&T)(#_lPhj8Hxg(gaj^=NlEQMj`I>P2sU7?$>}U);+( zNss3`m|ybZX>dp*(ey{~RNW`w_6S8t+2x}V#fKNT-;I=N_aji70uSoYPZuwh%EYKu z8ohxdNrus}Cb*6{Qbn*F>@VmH;4*=a7NV_lw^zjsyV0w3_LCAjDzgtJkp%@S$?#Yh z>+PG(*M9h_^>ioCLGpb^4#rBfI#x(8-%#*0aXPqv=?RrqZKUBFO0D8lI)+qhVY7@P zK$;340}V2B_O)_*Np_oevuDqBxx^WBa>X)OE)%E-G=xFRae6&#WI0ktYV`zR)Uh;0 zm<&dkb1k6N_$P4vQa=nGLfU^brC>zeq>E1TFG?q|ysgokvLx*NX$gbLNa>7>TBqjV z3Wn=f%jgVRLI<9z@o~^GiExo4tv77uK03rp=1uru>=`4aB7m_;2S~Ba(bN+ zZW9~~QD#oyoytV9daX$XKBiEnJ2wA|s7z68?r?R{?3ZI67fI%@R`8|>qXu@- zNzS0xF?tLCLPe7rok>O0dWs-G$9M2+&&jtkPq61C<|cI0MCh=BII3Kl<|x>2 zG_raP3*Ys$UIkOBg1J;{XqsdU>(cO;b>6tyf>@O}!2&wYs5ypJnMgHFa->GfXebjy zQW_1xX&4=8;6Pt3J^?JnEG9(jV3opU6BVVJAq}uq$}zN-GLQs}MQem#L9`T0L)#|s zMvKRQ=Q4{C@jAIU?qroNQ6JL4a68;fqSDm&@?ySR&6Oq|15%q?ToTThB0d%YAA8(e zup?gQvSgijX(l2-7Y9oyV!wtY2qwgej{u8XNHW2HfB+gtsX9r5TVk8w=0u4Vj{yCh zp+~_{GV{(L;}{j&^PQr~sSXEE=$>xKp5qC>yn!!B{-a#;d zR;-9)@SKK#UA#mbXfv@2P&)|T0B-5<9`avKrq|R1Cq3R-zNLVNv>xv!&q2_j$Gggl z5L}_hedSk=@B)YRcqe%kg17Z}7kMp$iXhz2=}#UKY1zv$`1g@TD%>4J2H~#qD~Q4r zgsbGY5!@7n2gqL`cqIt$DX&5BT@c<&UXLKz8xNGH7xLyzJQQR%^o9;UL;@Fk;{lF{ zj7Y1HW0_zp1+R~8!MLjf5+Kr+%K;ONw*mu#aX0y~T|8AxFf?0);EG`A`KcU!^k^`Q z@(>z*7!0FagW_@S1EV~FAk_!To;jyvjM@#5E#uC1n2WG7s1>RT;-V0Ln3Xj3flle-Ep_f zzK{S7f_8nOXdx0E#>0yU&hCr*%kLt%8;-VkD-e|D;6T|AdhnblSa^*0z^NZhxBx@? z;k_Kc=0}ONUBj{UV1EZ3{sm$`7|dxTo6`>uv_q&2B6$^pj-k+O9fG|>VXZ7W&eO(0 z=$OGnBCR|Oa@BZ4uJ1x2*G438F%-`3b_DBrhCK-O><{5#1Y`O`ffER>=HZVBp5q0I z5q!nN^b@=_zX33al?aaEVHSc*2S9huq53$?L!s^-41hCv3C(Ydfe_w9P&E)vYBhqN z48;B9X}fve^nq|4EJHANAgqO3dHm@0fspJWf_8(T+Hy40y#_&n-GUE;Aa5Ch|KkPj pAh>%Fy&?L#xg=3|3wNf{|f-jUhNY4DWzF{XguIJ4^ro delta 20864 zcmc&+2Y3|K_Rq{_!H`6j010A9La%}8vqKFX4MmE)r-C?@ETljZiux%*KoJOSAr~Y< zqJV(!Aqp6Ju}}mAeN_K66+tPYAP_0?KQo(5W;YL+#mED`kF$60y}xtLJ-3~6?!K2B z`Ce}1s-8C+q%0UaOc}*%5~pceN_NU{E+9sF|bHP8r#W6hTpgI*-cA&PYSQ{*2Z3&bbxSaLVAf z-|P4D_&t(4UN9Vb9Fvx(B$s8#&tj0*ta{j z*o&z(y$PRHLHTE}X|CkBh*gt{A8lS~aMFR{`vWyM&b(K1I3FRF1C8-PW<((cOQiM zUZKl*tV5NZUwVJKLo5!(7NhMQ@cL-gc`T~RXGY<%Fl>=m+!%+>Ls{q^D9gKm@s*A5 zR#fr2-e_2RXR{&Oo4(zxzS!ne?We?vksFLJ{}qZMbf6O+jl372x2;Cu;YY9nG_x}v zkG_P$M|K;9Z-k-owXy2x)&@}x0{rt*`TPXvlH25u^?s(RyxPn9Rm$0Ux*L2={UczFPcUJ7a{>78io+@~ed_A+x z`T28nS%`ufju2OUdf=?^C%h{+4DPn}+~tD0P}J%8uyw2dJvpn%6T|jA9-naqigNq) zT)j87DLJ8a`=|CrK5+<&8h1@TR5zvSfZyM~IdZ`G9-F;!lbaS@yM$fFVm4ep$tJk^ z_Z)nz7xuE2|IvN}uBhnNWo&Pk`&xDA!qt6OUB%Ez9*->giNgt(D^THJxs6bt?zlH9 zP`%hv-7s~l4)?k~ccRC7;0vMx^@*)4G4D#~+a7qKD^QKzyGo2VN3C46>Ir-V<_gpv z`tU3kgTVHyqCV|0XVG)XIR02UeV_yt?}`eN@xet^RXjAh*2)d^N9!*he>(D`Ek_c1 z{}TQ4UVRWgC@5s^L_jY)P5oU z3~IL!m$2qu^8I);WELKoD7tBuBN&O42sbZKg2GX>n^Ra$Q3RE8tF5v##q=yKBQs4^ zTIChuH9Dty(npOQ>*f+Urj`@ z;ku_kJXoofyx}$MFTj(*!vE;S&c^AoKH5T|DMBF_j*w}FBPkxllu1Gr1wwK&BGI>v6pCrg@XFza7?*m2s$4w8(LaBB~narW8RT zHG!i}mMJW8~;Vmws5f&z{ep8=Jl3&a=nH+$?N*arm9(I9A3> z`a@EHkC$)CbU?tk^KP(cHzBE-n-VFCmo!>n+&sa_9N}l(Fq*K{q&$cf=|(d*tG|{nfkVvy-vEgiqL3vk%`2CM&SG^KQ2!aFXVB3vO9c6^(L} zf~GKPz?h0_?d{%jmOiGX!sdx}Ywfw6dA;o;?M1xL)jNK|290k=SSx1q;kQ z5b&uW#p_`oWtccxgcF@HfIV&xXf3=}smhbh+HC39weM^!SX}l&aD!E05Mwt~9HE~$ zd}DVcdh<(jBTkH2`^@_dCVcwYlR=MN5YkzIz2aft)@si(Yu!tQX#DGVEpJkBrRomX zPW19~D;_JL$Jcu&?8LT+ZCHhaR9J`AZeq0_D; zlV)_@mR=P4QrL7c*l`jb6#9oCN3XYipsZ|9ww&TgA8JbZ{rq1Wtp6@z$OkhHPkZrL zqi8Hx^wtlK9^}Fh#A2u;P2c3%BCQkaQUVFILI!D^e^4))gjH~;;7&BTSt$0}Wjo3Og#xYuDyx1Hej z`5-2Sm^n%=8;Yy=$i(j(t>*VHNq5R2>8t+6iD3l}c8R(7h;N*P$JQ?0kG_4esNJeg z3&uuOmJ%A?%C8;tQugOKm3=tDGJpSBJ>y1gnAI2hto^LX7Ek`gA>`_-bWG!*xwi#m zxkts*sv;4h#4`jzC=4xAszh=;!*H4`5~7Cs-oonHxwe4=D_zZo`R#DcUc5K+k*BwA zoPD9D^nT*k^@4@7b5S$l$}ZowM7j6qZk>$RG(5*dO7eu2Z{+`Wu+fxiUw`r263hL% z0wD0L4AU1CFA=<~F@ivmZccRzEbS%22_3d~=KpUg7J*lN0g7)Kv zyfxv}t92tfq+(mfwclLk{j!B<$D{9uSmtK5)qh7DJ84P(hCQA9NAsB-#O6sa4Xo!m z;L1N@ackPbYZ(-t<%%jY;J~32Syly>SK+!34joRE1&UD_Qjj#XehOZ{tBu~p5t*LU zY;kDm0C{#dTj1&6e;)*jj@>=_{RZp3*cParA@T<`BRmPV0PKuWDS$CZQwI%xbHVQJ zm7IwS8c$KKtSbvC*#opQNESfz)ELJ~Dy_TRRFa|zmg6XfmjyzWM1dp-7BSQCMs|j% zU)O;>JyMrU&uBH%5a{+t?@j5q=YzbrChThSX~^BAvTe~0q@7z$0Hnq6I0zAl0>_9d zNrQYmMbeVQYZT85f+#99DxQYN-g_zgH5ZRTi{|2$jrDAaV{F~j)eF-5KKtVjg@wD; zSdFcsfFSl|*%IAuh1XQZtqE?CAl(q_0C@;iCIy9pIHrcy<>K*w%6?sg^V{DxKDhDy zs62Ujua#eav1prhH>>~*VEQ}rbRI7;w92WfN;0YZCe;osuK4WS; zK~rUs<^@(0M1dkiPNH~K;A9m-!V(3+Glb8?>)o5__A~K12GerHtZ4Md?j5(DjGEDX z&7{Uy#bWp~O>2ap5pG3M1qIx}EW?lz?B2Y}Xl_lTCA1Tmw&QTOIPPCy1Q_BmE~4hY(cwiY)AhM+G?Ssz{sk~!10eD364@l z1$I9-&G9TxD-@bK3$OQN5HOuP_U@dalq{=-B*jt@-_@uWzP$dsZ-4x$-_)=HZ08zY zDxU?|ayu3o2C^v#R^$bSl2u8DNh2|CktASelm!O$nT^-Kx7kgbjmH^gcV&F4l0Rba z?PZ=T+yaF)5?97yf$OK?cmPw6ttBgqUs!EWG1`!a2B_)mJB^gC~@do!j z{G*Y1c#Oe(^yR^|Yt6-Ce=Y90VAbk_7t7KkDijNZuxMV8+zKbtGQr3!scI^(Xgo_n zs*9Ud6_U`5 z`-aLIw{Zo=i-NVzdn-{787{Fmz#_8g2Q>?Oa^^{Qj=MKttJy*%n|28>@0zAixzE2e2FtZ27e9 z$XDuOE;;!II_iV;qT3O$6j49+2RhNs8EwbqY-f+_=kMiq1vk=056@c(LhI_>AtB@SDKC#=(V%RQUQK1eFq72 zt)UTju_o9q@AftA(6PH%#JCyo^vJSgg_K*f)~%oO$sOXC3*S}3W?Sm|7Ss?|;EH;t zNUy$bUObiywap7?NVyy6rkQ$0>U4f?Wo)KdwY1y7tMb9=dhG-I`eF0Y%(-}Uy{wFo za?8*!Tl7jZ_cz7zp+od)F&;fmuNSzOzqe)u8YI4w+vUU!A;nBYL(GqW_|=b)!jZFuhCy;CUDtYY*d>3 zaTPz^4D`s^8JYc`^6?V5Osz56?3x>fA^nY?p_#xpe(Qdn4eDFAsWUbU*!AP6dbv~(^r%C17$ z@j|_!*>+{6By|~7>KzHq?tGW^oOxRh;Pz@Z`;7wxb zoDB4*Zn~jYo;PQz1Dg)x1C>jju7w2pJL3+X_CnR-X^Y;*jIk`Kn_Swp)g!4pB$r*) z>+iZXH^vxDsP0Qf3MqHyKCm2QA%48}sdv%u4aOZ z;)=C_sEQ&aLO>WraB~t1uTJ%3F34C>BnIA-sdIB2gXbnV{6K89NwH#ZGD8V&fmb0; z1`pOKT4N|ef+wvkp|J|Vp$@!b2znFqN3qq)^%L&@BMj)<3wT^S!z+wLa3n#~ikoD3 zPUBbtqQnrrV<}lcw|U24bOkp7?Kk?s;fN@aZR8R_5?DRug4fkfsx+t^qJx@3K?(rG z3waF{EW-Pvr$QaE!8A%LfJT_pZ1mMuJT8vqcupdDc;g3SNx{dv2^{#E1b&FG$oMm0?RGIn|M}m^KO!cXfDaZ zND(XXUg%($BcVhY_8U#85_F@!hzmD`vWqkTT?qG=2_A)$EGdG0D;gp5kh?)BJjY3H zH)Q`nD#TYSK@syJf^O6kb&B+7&jwO5`Z>~3->N-ZjTSy?HZp92WHg#&1z3hiLLp%e z$&v;?#UeqT79<4GJnCD7C!^j~O`QkP>ed6a>W4mXnF_Rx)dvl#X4T+iv~VLHhN?xC zD~v$9qs;ybt2?~tmFi|C`210sMrWcQYnWew)p*gOnr2h@JQ<~bf>%S&M3*ak6uDxo zEhnStJAj?f%NItVS8Dk;Z}ga>aTyPj(bQddIQro+|4P$qJDOQ5!RK)F)Z_k`+rGpj zQCMubLKpfx*1x-)uK?r4I{r1G@KHp^`PV!i=g-~Dct=AkBk;Kr>XBgfRCoxVheGR` zoxtb0XhU7|dqa(vkHX%XS;j4`@{~l994ShIfc7_b3@-t; zRsYl(SQYj(2@V7rC8O-7=3KRll#CiQD{B<$h>shn4CRAP`MUK{ZhG!sySyD(| zBhc21unr$?W=`*scH2y99&DSw$lbym3Y$0s(4`ip=8WY);vubz=V3>oAS+E$RgHna z1WEI7_;S+<5?eX?ptllD?t*BQ+%ou9{qtIxLfMAOEHNOXfy}Ccs7Zp%NHR@B+A^UC zf=p?U8cWlHMxyc8@Tbu*!XL&?tAC>Uq*Z%yagruMhNmXNxl>kUh9`Me6(KKIhOAXt zK{=!&%gUOdGPKBu1g{XX0x5M0D-x2#@ghxeG)xgR?gl;x&7w?@(xV;6z}`Wr@RKn# z4{1;e1xb#gs=|R(5z%GJG0KXSiJ~+Seo#lES(tmI(V!-(dJFG_ z?$9QbuDf{At*Vf%#KMbjg`z}4CLlW$e#%ARAt_uE(c8CxzmFNSk-;A=Q4Bnu01gS2 z0{aeyIr_74YvohUP-4 z^R@kYZ~toTXyJU*AWyV*BswP>WYso~R?cY#*}09Qr8CzcN9*JigIwMQc%NjD$J#hr zgiq8-BIk?~%R_~Xqb6$D))5uOp0?)v(IS7jajMeQxG(!*Qi*}CY zVL$k4obHIo_Kq4Tsy(o9!RV_+dywWAgB;Qxr1{+-XXs?kUY*CS?Hy$J1f3*u{zZZh ztz$F@=IQ`Fylzxz)&c6iX^_wA2S{zEZ z9e~o%jv)CDMrV(A1a5vZ$mEV-P8SVwERf-seeF(n1Bj%fcn!3*qoZc{ufEc0&3vUX z=(^t7Z$`7oPSDv6gKVdhw+u3^6IjR}23e?+6ZYvUJET{5-5{Nvft+HHY-bR2nn4cL z$yo+DzcX-l)3ELxI(f$+uXcvP-ffsb?Jhu`G{}x!VBEho$kZ-yBD!Rdd0k-ETs6q8 zUBFys?9h2Q*9AIz;|rav(G{8%7~|TZD^yr&Kn(2){jN92>3W3?2D!c~^t;_4zt`)2 wYLMaGfc)AZ>26@0|1roR-JscFgPhXM5nJulyc*}Oi$~6!zAfmEjb)erAJ&W1X8-^I diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index ce58ce8e4a9cf8e9343f6bc9b87401d98b3b453c..37ab197d9047f168d86d777db495ab4f525ac904 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~oOD0RS@>1VaD- literal 17 VcmZRcbt>7YGeh$`0~oMn001*j1bhGh diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index b0c2d24925a2002f384642f8be9264c7d9b027c3..797f918de97523955d3a9905013307b0c760635e 100644 GIT binary patch delta 1511 zcmZXUe@t6d6vz8UXAA=h^2SP${-C1#4s27n|*Z)@QRAy>FfGuF5hz|7URJS2CG!WYKK;QpT=~ zPADue1X@~JSIEPGgei1p3PwufP`+CQ@Y;*+%k21{y%pFw0z^kHs z$>F&J@y8rRiK~iV8`#ojyZR`eim4j>jAy%bnrBPBO`gW}VI(jE=jQb{sgnf>jQ(xMTk!nDYh{c0w(hKjLJ zDI{aQHe|-9H)zYyRalhFvb$v&wQ#O1R^vX7wMuE zKO&2iYVjvd-U3Wt!+pzemqa#flBgP-Z7gX$Mx?33xJJnXY?vp}UaXU-4A0%43v+Tk zjY+92!es*832Fz=qK!ZHEzI?Pma zeKVF4A{pGkG9m<&s7asr6+x z`5;>Lr1-E)Bt?1fUYd}G>$NP8M}a%F%+sUBdQRT$I3iI2E^ubD^q@^tN+vz9*pv#xBJd-Rt zeXJwR!Yijoi>^%IjCt(ra6--Zhi|)6Nt>aa#52KTljq6vKfUMMWqHd>%F2{^PeuK- z`eSmmNcn}2Z{!gvxnS)zK+}7bB%?wB*L^C8IYa7gx_Am2&Nzy8fdtuSCZ delta 1527 zcmZXUdrVVz6vum^E(-&pu{^v?+Cpiz5$S-81r~xv8Bs=+5@fQGrq1aIb6d?EEDi_{ z6)O|!K@J98fxaP1p<6O^>YO2q8iUO;2?#QVz_Q^S*>FCxxZmUcvw4%-`}zIO`F_ti zH@&~}LHYTh+<&=I-gzzkPT>2-YC+vJr%yMm3&d<)-1hD>VtT(!CLEgl`{j>h>`Gh< zMTNS+fdkeM`IL4c2W`nhDy3{Nm5E?#R?|L~y-Dc2mq& zh&WtpJoe@EH^0~PJ$`SSRr{AJV?C`lrJbMW)#6_w&(sX^^CG!5^o}}v@AfO|sNy%u z#|wr1yG{-=mFnO6~0NxQ}d+Htm~Pn-5r<_M@~-#||9l`HlvBPGm~Q3Cxjb06*m9 z%*Gyx@?fx)&1J?F@yyp*$$XPwsY40+Ha7lY$Zum=o!CXP=zI7IuS@hTK9Ohw)1K$i zSy*qQX|8ZQPPRz9E;X~QK!qqM;i8TDy0Y*g7q~WH${Hqhm0`I=JFr!v&+w8&%@91s zjNO==zy)7pDIr?oJ61x$N}wY_U^T|@oRqkLcR9Hgs8TVpI}wW{S_kHK>f$bCE^*vt zR3#|srkia(i|?-N&hTZOlF{IO<0-4+&ri8ltedhRHC0z&`MuJ;`;jJ$KNVGMeh#?`LsZaq>;@Sq2(^1v)7dV!fuLf<8076g5@BFeL#h zu!C_$r5On0=>k8hIq|{t?vj}6q*gSY3DT)T$Qt*Yb@aIo!Z>O$M?*)H*j-tcY82+Q zh*sVNxil8EBRKjGuY(^D7QY)FrY$eX#yRkB#T*=3$%4f<)rt~71Rr}Tml|n}ssP;A zvVe>~k5!XX98h*23~LFYNX?f}$m2eQ#Xg36wUxw)(7kbyp*Qselzs%mdQ|AP%9CoA z{S3mm2f?CV1!SLmylW`F8ezN*piuw4t(uqk!q?(1Ei|08gbp=!oqRKFn2zxHm#bWz zRMnsN{c9u2^|@~*ZG1kTzN#-LCL=Nc~Wir!kk=lk*L9cZ;3VA9QXOq#u#dhsfD z2#hma?KM4}qT*IIe;nc55x}9l>o6Qu;TPV1AqeCYuF4G1Qb72eGL3K^6SUD;kUVwE zzUcSnXz>Cd7_nqjPI=wv-_eUQ*wd(6l<0awnu}Mq%0*y&B&=q<{7U)syctRx@!yJ2 zP9nT$!ov}dp6{A**_X>h=_3$rL50qQlqbEim_=9|0i;`mLek!zw{*MLq23snunyw^jx=#L7*-sdU+g)&cqtY z=PaDpHP*Vnt<>O-nm1aounO+%z{cst%nAzMw#Vsh++nN{mmej+B$$wXc=|FEUy{9Y zF8s7q3947}_Jv>(xy#oXCP8|VSR)4J3fb2aRr?0T4iL25)AO)ORmNO$&}7$z0iA4n zSJ+n_Ymho3i!|`CgvXxL@bIvf608A9i9P6*;OlW;U-Z{ZTj2j-aM2qtJ5e0bC`F!~ z2-%qwd*!##9~9J>Tb#QgY8i2ulfSF@g`?HVdGOPIJ>9)GoVs~i-hLk6&iYkA!5Ye0 zT7?5VHwE8@njo>kOeGN!?$0H%Z-?1FGx*WA~NX&pT~$@K*dpDwn(z#3T{-B#U2G)w&WH{O_1at(-#A#rh68h+KeNOdE|;Q`EZdCwst-LZ|7EdEh&9+6hUc|!?0=V; zr`G1I;H$7^EZ=nGTXWskZ??tc5EsAxYRpwO(HV3zN%RyQQ;p7D&E*M?1C}zgyVqi? zsW4g7%iiq``8I7;l9Z9Be30Nvg4P=pO7S1XFSYu}$TiR;0X3S$t-K{wGCLQ=8VJ$g zbVxnHHVx6}4kmK-*ZyjxoTxCm3u6svusa23YbCxnq>d4wKP8Se9N7(c`OYTg$xqrG zvojZR){FzI3$}IM^lWyV-6KC*=9Nu>71xwiLxEnZ75-rc=PO;oI zvvwdL0h6vF7n9u~d6RA;FO#(*@w2`o9|4o~BuTSYB_IK_jwY`GlT|1&lb$H>v!*Ez z1(VG#8I%4lsFS`g*R$O)Apx@%GOq!X-7_$gJv2y@wKPeS7Bxt-b~VQVvsE|X0kcgx lcmtE&JQcl}9%Z0~m0=0{}HO1m^$% literal 17 VcmZSnIL9>cl}9%Z0~l}|1OPP#1i1hJ diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 92be894e8dcf622dd01c60b888b37aa992a27a23..de0df2ebde9b0f47d0458724175a50a414967aa1 100644 GIT binary patch literal 8 PcmZQzU~GR}7hDMd2-X6& literal 8 PcmZQzU~Ipud_x8R2wDPt diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..f5fffb7 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -15,9 +15,14 @@ "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 264, + "incKey": 265 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -65,10 +70,6 @@ { "decKey": 263, "incKey": 262 - }, - { - "decKey": 265, - "incKey": 264 } ], "axisCount": 2, diff --git a/simgui.json b/simgui.json index 23f00b7..edacf08 100644 --- a/simgui.json +++ b/simgui.json @@ -66,6 +66,13 @@ "types": { "/FMSInfo": "FMSInfo", "/SmartDashboard/SendableChooser[0]": "String Chooser" + }, + "windows": { + "/SmartDashboard/SendableChooser[0]": { + "window": { + "visible": true + } + } } }, "NetworkTables View": { @@ -81,6 +88,16 @@ "window": { "visible": false } + }, + "Plot <1>": { + "plots": [ + { + "height": 332 + } + ], + "window": { + "visible": false + } } } } diff --git a/src/main/java/frc/robot/commands/AutonLoader.java b/src/main/java/frc/robot/commands/AutonLoader.java index ba1e788..59a5187 100644 --- a/src/main/java/frc/robot/commands/AutonLoader.java +++ b/src/main/java/frc/robot/commands/AutonLoader.java @@ -6,11 +6,13 @@ import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.auto.SwerveAutoBuilder; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; - +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import frc.robot.Constants; import frc.robot.subsystems.DriveBase; @@ -25,6 +27,7 @@ public class AutonLoader { public AutonLoader(DriveBase driveBase) { m_driveBase = driveBase; + m_autoncommands = new AutonCommands(driveBase); chooser = new SendableChooser<>(); diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 4f728dc..e441f42 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -1,4 +1,8 @@ package frc.robot.subsystems; +import org.ejml.dense.row.decomposition.eig.WatchedDoubleStepQRDecomposition_DDRM; + +import com.fasterxml.jackson.annotation.SimpleObjectIdResolver; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -16,6 +20,8 @@ public class DriveBase extends SubsystemBase { private static Module.ModuleState targetModuleStates[]; + private static Module.ModuleState testModuleStates[]; + private final double[] moduleDeltas; private final Kinematics m_kinematics; // private Pose2d currentLocation = new Pose2d(); @@ -36,9 +42,10 @@ public class DriveBase extends SubsystemBase { // SwerveDriveOdometry m_odometry; Pose2d m_pose = new Pose2d(); + Pose2d simPose = new Pose2d(); double prevAHRS = 0; - + int cnt = 0; public DriveBase(Kinematics kinematics) { @@ -46,12 +53,18 @@ public DriveBase(Kinematics kinematics) { moduleDeltas = new double[4]; moduleGroup = new Module[4]; + + testModuleStates = new Module.ModuleState[4]; + for (int i = 0; i < 4; i++) moduleGroup[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i], Constants.DriveTrainConstants.wheelLocations[i] ); + for (int i = 0; i < 4; i++) + testModuleStates[i] = new Module.ModuleState(0, 0); + // m_odometry = new SwerveDriveOdometry( // m_sdskinematics, RobotContainer.ahrs.getRotation2d(), // new SwerveModulePosition[] { @@ -106,6 +119,12 @@ public void resetOdometry(Pose2d pose) { } private void updateOdom() { + + /* + * ENSURE that Kinematics::toTwist and getDeltaOmegaAHRS are being called every code cycle + */ + + Twist2d simTwist = m_kinematics.toTwistTest(targetModuleStates); Twist2d twisting = m_kinematics.toTwist(moduleGroup); twisting.dtheta = getDeltaOmegaAHRS(); @@ -113,12 +132,18 @@ private void updateOdom() { SmartDashboard.putNumber("dY", twisting.dy); SmartDashboard.putNumber("dtheta", twisting.dtheta); + simPose = simPose.exp(simTwist); + Pose2d newPose = m_pose.exp(twisting); m_pose = new Pose2d(newPose.getTranslation(), RobotContainer.ahrs.getRotation2d()); - SmartDashboard.putNumber("X", m_pose.getX()); - SmartDashboard.putNumber("Y", m_pose.getY()); - + if (cnt++ % 50 == 0) { + System.out.println("X: " + simPose.getX()); + System.out.println("Y: " + simPose.getY()); + for (int i = 0; i < 4; i++) + System.out.printf("m: %d, Δ: %f, vel: %f\n", i, moduleGroup[i].getAngleInRadians(), moduleGroup[i].getDeltaDist()); + System.out.println(); + } } @@ -132,7 +157,7 @@ public void resetDrive() { } } - public double getDeltaOmegaAHRS() { + private double getDeltaOmegaAHRS() { double delta = RobotContainer.ahrs.getAngle() - prevAHRS; prevAHRS = RobotContainer.ahrs.getAngle(); return delta; @@ -146,6 +171,5 @@ public void periodic() { } updateOdom(); - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 3d70b5c..38d7226 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -30,11 +30,10 @@ public class Kinematics { // private ChassisSpeeds odomSpeeds = new ChassisSpeeds(0, 0, 0); - private int cnt = 0; + private int cnt = 0; public Kinematics(AHRS ahrs, Translation2d[] translations) { - m_inverseKinematics = new SimpleMatrix(8, 3); for (int i = 0; i < 4; i++) { @@ -115,11 +114,11 @@ public void conv(double[][] unicorn) { // return odomSpeeds; // } - public Twist2d toTwist(Module[] moduleGroup) { // converts an array of module states to the ChassisSpeed of the robot + public Twist2d toTwist(Module[] moduleGroup) { // uses forward kinematics to derive a Twist2d from wheel deltas //gets the independent x and y velocities of each module based on encoder values SimpleMatrix xyVels = new SimpleMatrix(8, 1); - for (int i = 0; i < moduleGroup.length; i++) { + for (int i = 0; i < 4; i++) { double vel = moduleGroup[i].getDeltaDist(); double dir = moduleGroup[i].getAngleInRadians(); @@ -138,40 +137,100 @@ public Twist2d toTwist(Module[] moduleGroup) { // converts an array of module st return new Twist2d(deltaX, deltaY, deltaOmega); } - public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { + public Twist2d toTwistTest(Module.ModuleState[] moduleStates) { // completely for testing purposes + //gets the independent x and y velocities of each module based on encoder values + + SimpleMatrix xyVels = new SimpleMatrix(8, 1); + for (int i = 0; i < 4; i++) { + double vel = moduleStates[i].getVel() * 0.02 * Constants.Swerve.maxVelocity; + double dir = moduleStates[i].getDir(); + + xyVels.set((i * 2), 0, vel * Math.cos(dir)); + xyVels.set((i * 2) + 1, 0, vel * Math.sin(dir)); + } - double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond; /// Constants.Swerve.maxVelocity; - double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond; /// Constants.Swerve.maxVelocity; - double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond; /// Constants.Swerve.maxAngularVelocity; + //multiply the current x, y velocity matrix by the inverse of the kinematics matrix - // odomSpeeds = targetChassisSpeed; + SimpleMatrix finalChassisSpeeds = m_forwardKinematics.mult(xyVels); if (cnt++ % 50 == 0) { - System.out.printf("vel: %f, xVel: %f, yVel: %f", targetAngVelRatio, targetXVelRatio, targetYVelRatio); + System.out.println(finalChassisSpeeds); } - if (fieldCentric) { - this.gyro = 0; //this.m_ahrs.getYaw(); - this.gyro *= Math.PI/180; - } else { - this.gyro = 0; + double deltaX = finalChassisSpeeds.get(0, 0); + double deltaY = finalChassisSpeeds.get(1, 0); + double deltaOmega = finalChassisSpeeds.get(2, 0); + + if (Math.abs(deltaX) < 1e-9) { + deltaX = 0; + } + if (Math.abs(deltaY) < 1e-9) { + deltaY = 0; + } + if (Math.abs(deltaOmega) < 1e-9) { + deltaOmega = 0; } - conv(computeUnicorn(computeStrafe(targetXVelRatio, targetYVelRatio), computeRotation(targetAngVelRatio))); + return new Twist2d(deltaX, deltaY, deltaOmega); + } - Module.ModuleState[] targetModuleStates = new Module.ModuleState[4]; + public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeeds) { + Module.ModuleState[] moduleStates = new Module.ModuleState[4]; - for (int i = 0; i < 4; i++) { - targetModuleStates[i] = new Module.ModuleState(vel[i], theta[i]); - String name = "Swerve (" + String.valueOf(i) + ") Angle"; - SmartDashboard.putNumber(name, theta[i]); - name = "Swerve (" + String.valueOf(i) + ") Speed"; - SmartDashboard.putNumber(name, vel[i]); - } - - return targetModuleStates; + SimpleMatrix velMatrix = new SimpleMatrix(3, 1); + velMatrix.set(0, 0, targetChassisSpeeds.vxMetersPerSecond); + velMatrix.set(1, 0, targetChassisSpeeds.vyMetersPerSecond); + velMatrix.set(2, 0, targetChassisSpeeds.omegaRadiansPerSecond); + + SimpleMatrix results = m_inverseKinematics.mult(velMatrix); + + for (int i = 0; i < 4; i++) { + double vx = results.get((i * 2), 0); + double vy = results.get((i * 2 + 1), 0); + + double dir = Math.atan2(vy, vx); + double vel = Math.sqrt((vx * vx) + (vy * vy)); + + moduleStates[i] = new Module.ModuleState(vel, dir); + } + + return moduleStates; } + // public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { + + // double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond; /// Constants.Swerve.maxVelocity; + // double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond; /// Constants.Swerve.maxVelocity; + // double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond; /// Constants.Swerve.maxAngularVelocity; + + // // odomSpeeds = targetChassisSpeed; + + // if (cnt++ % 50 == 0) { + // System.out.printf("vel: %f, xVel: %f, yVel: %f", targetAngVelRatio, targetXVelRatio, targetYVelRatio); + // } + + // if (fieldCentric) { + // this.gyro = 0; //this.m_ahrs.getYaw(); + // this.gyro *= Math.PI/180; + // } else { + // this.gyro = 0; + // } + + // conv(computeUnicorn(computeStrafe(targetXVelRatio, targetYVelRatio), computeRotation(targetAngVelRatio))); + + // Module.ModuleState[] targetModuleStates = new Module.ModuleState[4]; + + // for (int i = 0; i < 4; i++) { + // targetModuleStates[i] = new Module.ModuleState(vel[i], theta[i]); + // String name = "Swerve (" + String.valueOf(i) + ") Angle"; + // SmartDashboard.putNumber(name, theta[i]); + // name = "Swerve (" + String.valueOf(i) + ") Speed"; + // SmartDashboard.putNumber(name, vel[i]); + // } + + // return targetModuleStates; + // } + // public static ModuleState[] normalizeVelocity(ModuleState[] moduleStates) { // keeps the ratio of wheel velocities while ensuring that each never goes above 1 // assert(moduleStates.length == 4); @@ -191,13 +250,5 @@ public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisS // return newModuleStates; // } - - public double[] getVel() { - return vel; - } - - public double[] getTheta() { - return theta; - } } diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 991f56d..661bf40 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -31,6 +31,9 @@ public class Module { private double oldWheelPos = 0; + private double currentSpeed = 0; + private double currentAngle = 0; + private final RelativeEncoder integratedDriveEncoder; private final RelativeEncoder integratedAngleEncoder; @@ -40,17 +43,21 @@ public class Module { private static final double kI = 0.0; private static final double kD = 0.005; + double lastSpeed = 0; + double lastdir = 0; + public final PIDController pid = new PIDController(kP, kI, kD); private Boolean invertDriveSpeed = false; private WPI_CANCoder _CANCoder; + double curDrive = 0; + private final Translation2d m_distFromCenter; int cnt = 0; - private double prev_angle = 0.0; /* * @@ -97,13 +104,14 @@ public Module(int module, boolean inverted, Translation2d distFromCenter) { } public void setSpeedAndAngle(ModuleState targetState) { - double x = setAngle(targetState.getDir()); - double y = setDriveSpeed(targetState.getVel()); - - if (++cnt % 50 == 0) { - System.out.printf("Set module %d angle to %f, speed to %f\n", this.moduleNum, x, y); + if (Math.abs(currentAngle - targetState.getDir()) > Math.PI/2) { // makes the most optimal turn if angle delta is > Math.PI/2 radians + setAngle(targetState.getDir() + Math.PI); + setDriveSpeed(-targetState.getVel()); + } else { + setAngle(targetState.getDir()); + setDriveSpeed(targetState.getVel()); } - } + } //angle to normalize between 0 and 2PI RAD public double normalizeAngle(double angle) { @@ -125,12 +133,13 @@ public double setDriveSpeed(double speed) { double x = speed * invertMultiplier; + currentSpeed = x; driveMotor.set(x); return x; } public double getAngleInRadians() { - return (_CANCoder.getAbsolutePosition() - Constants.MotorConstants.motorDegrees[this.moduleNum]) * (Math.PI/180.0); + return (_CANCoder.getAbsolutePosition()) * (Math.PI/180.0); } public double getAngle() { @@ -152,25 +161,11 @@ public double getDeltaDist() { return ans; } - public double setAngle(double angle_in_rad) { - // //code to make the angle motor turn the least amount possible and drive direction if necessary - // double targetAngle = angle_in_rad + Constants.MotorConstants.angleOffsets[this.moduleNum - 1]; - // double cudoublerrentAngle = getAngle(); - // double normalDifference = currentAngle - targetAngle; - // double difference180 = currentAngle - normalizeAngle(targetAngle+180.0); - - // //if going to targetAngle + 180 degrees is not less than the distance of going just to targetAngle - // //then turn normally and also do not invert the motor direction - // if (Math.abs(normalDifference) <= Math.abs(difference180)) { - // pid.setSetpoint(targetAngle); // angles are in TRUE BEARING ( angles are negated ) - // } else { - // pid.setSetpoint(targetAngle+180.0); // angles are in TRUE BEARING ( angles are negated ) - // invertSpeed(); - // } + public void setAngle(double newAngle) { - double x = (angle_in_rad); + currentAngle = newAngle; - pid.setSetpoint(angle_in_rad); + pid.setSetpoint(newAngle); String name = "Mod" + String.valueOf(this.moduleNum); SmartDashboard.putNumber(name, this.getAngleInRadians()); @@ -181,7 +176,6 @@ public double setAngle(double angle_in_rad) { this.angleMotor.set(0.0); } - return x; } public void resetDriveAngleEncoder() { diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index 1a010f5..98d62a0 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -92,6 +92,5 @@ public Command getAutonomousCommand() { } public void runTeleopCommand() { teleopDrive.schedule(); - } } From c7b5036c536691b57e1cb6fbf8aec41f8168ae3c Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Wed, 3 May 2023 16:54:50 -0500 Subject: [PATCH 19/31] removed some unecessary code --- src/main/java/frc/robot/subsystems/DriveBase.java | 12 ------------ src/main/java/frc/robot/subsystems/Module.java | 4 ++-- 2 files changed, 2 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index e441f42..92d0e04 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -1,20 +1,10 @@ package frc.robot.subsystems; -import org.ejml.dense.row.decomposition.eig.WatchedDoubleStepQRDecomposition_DDRM; - -import com.fasterxml.jackson.annotation.SimpleObjectIdResolver; - import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.Robot; public class DriveBase extends SubsystemBase { @@ -22,7 +12,6 @@ public class DriveBase extends SubsystemBase { private static Module.ModuleState targetModuleStates[]; private static Module.ModuleState testModuleStates[]; - private final double[] moduleDeltas; private final Kinematics m_kinematics; // private Pose2d currentLocation = new Pose2d(); @@ -51,7 +40,6 @@ public DriveBase(Kinematics kinematics) { m_kinematics = kinematics; - moduleDeltas = new double[4]; moduleGroup = new Module[4]; testModuleStates = new Module.ModuleState[4]; diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 661bf40..6d9582f 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -61,8 +61,8 @@ public class Module { /* * - * @param - * @param + * @param the module number + * @Param whether the module is inverted or not * @param the x and y distance of the module from the robot's center of rotation * */ From f0a414868cc34f3121c236d36bb02426b57cd291 Mon Sep 17 00:00:00 2001 From: roboticsDev1584 Date: Mon, 8 May 2023 10:11:33 -0500 Subject: [PATCH 20/31] 180 code changes --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 75965 -> 76065 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 31247 -> 33423 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/buildOutputCleanup/outputFiles.bin | Bin 26057 -> 26111 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/Autons/MoveForward.java | 2 +- src/main/java/frc/robot/Constants.java | 12 +- .../frc/robot/commands/AutonCommands.java | 2 +- .../java/frc/robot/commands/TeleopDrive.java | 9 +- .../java/frc/robot/subsystems/DriveBase.java | 17 ++- .../java/frc/robot/subsystems/Kinematics.java | 22 +++- .../java/frc/robot/subsystems/Module.java | 104 ++++++++++-------- .../frc/robot/subsystems/RobotContainer.java | 2 +- 17 files changed, 107 insertions(+), 63 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 0ed30ff3bea3ef6956a6b58f37ab142df9a3edae..864483d6cc6eeb6ab03de7b3ec692679521f8f5f 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oO60st`m1PlNG literal 17 VcmZQ>zc8b1qOQ9v0~oL*0st`a1ONa4 diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index e1d88b9cba04f27225a2e1f5d7584e491bfcea48..cd75cf5fb2ae9dcf18a49af8053d284ddbe5c759 100644 GIT binary patch delta 14093 zcmeHNcT^O~^QUK60RfM+fPx?*Mp%M~iVI3qjF{6ZF6^Q!u#4;h22>2Er)R=Z+AN47 zhLhtA7y-|S2@{;2qF@FTLA~?LzwVv|#x;R=@BRMy`kqTo*QcthtE;Mex@YTR!>x-A zf9=<+Rj;+3qr{y(I>vOGB$vcW#O|u_2>BH8_$lK%iM7~&`pl$WzknN1SP6pMB+}5> z@uC<-s3JzD5KL4mZK8zPg3*Yp|kblJ((TYUDsdeJ-3sdqB< zl=QiR?X&Nk)3WT-GO(<#Ll@OIJJyzE+f4w=Xv(G(UV2a2!XDEpGw5>=@GV5~(XvQc z=tQT9;yAI$&Bd*wz+K=bf|+8fEfB%(K~z(ic@UTajwI8fgcXbut0rg)+8qK|uq24G zg4-g2F}PLNWqYY4R)9Jbgv(--DuG-TrihV>*gu+jjKhclMp2*xR|PYx)XZEIro@t( zB6+Mx5*Ded>y>f2)67~FCXSNEh?UZqIBATgpwAIvbE-bDU=q*hK%pEBxUfz`tB>uT zAdVEF{~B7DTW8R~8JDws=#qovzdUZ_0l8F#J&*oqYB_qBxUV184Bj@RjJbYy{MZa6 zAFGl@VmNk9Nz5@E|P3B&5az*M(Z#3u&0f-)lF_$85={z&KQ zzL=yz?ZJmSKjQ2$gT+^YK604dW%I_I+%X)2Ll?C=Ko=7{b){*?`~gigXS@KDAfWEIF90~xK0}*E;lt74GI|cv6!o%qln2I zjBS`S){?0jYzi*c3E5#2!c;L*LG%QLR4$v0qCv)X+DXt6%?gU)3M`v?l@FmQvcITK zxYZ}ZC?+)J;h2^rccX_>mayMR6hk-aG{B^jz?6?esa6ct92bEr3Whv0;yBx)rtl3G zyHhJ>*%}$EQpn?EN*M}^VDeBq>bTmv#pDzr)8X3@lnwm#6jD)Hr^4_f6^wC#nZBq_ z65hCuivVhF2^FW2pE*ebm#F~C8o4-bvJ(n-&J4LASHmo0wtTb$PT&g87FWY?B}xvZ zI>?zGL)x;TA1;x~Yg*x@QIt81)PGtA0~vNy&du^eq{$(vQ@9yYMsO zd`p;e0a^XM#Xormu>3c5fBFv8`M<6EmhdfB!}0w0)cvYypglK)8&LOk;F1Jnz<*Qs z)q$^B^Uata9vXjd-DkuJjtwvSHq1GZ@%NN|Xgm|;cSCExJu|NCzbpMr^K#>cR{nP1 zS@lhs$clee^v&VAmT1{%XwkP|av%S@nr{uqUq_MK(3%fZCr}-^$x^?1fAmkZ&-_BY z=fjQBl)7|ti$3F;YL+|+Ip?z_-^YP!{h4wfyc;->W_6{nhCj5SqNyg&@C%EdR2dT{ ziW1A@Xw!tY@MwJ^FE1*QI2J9KN>M*WoHS4o6EBXD2rz;+$^(A#{Hk5KYZMV19U`k{ zfK5oq8NczUoLM1fO`#X0E8A~$1z+9*Ew(E@%b0S>5qV3&9Kl0HA=aFtlDQmU1dIYbQeKH(fb zXiDi6*Y!;%Ghj|L6lhq7<(G6Y@;NTx{?{>Z#vS0M9$dhe6e(jvl~d4$I!Y-b1M@|1 zKWe*r-xs)Sja}IPZn^U4@SCTUU^4@xe5S?m#ylJ%f05q(>!G+0dWAu6fwHig5&wB$ z?+xi6g3hhpd4vwzr+1O@Ic@Lk&ueU<=#+7c`BCp862NYZM zXf31pFP>in2^9;im)rh*F5fZ(raT08Gri|@V-iAkO&I)RzrVs236(FlVS#LIu$*LM zUxiqL-cCs8T1Lyy7RZ0>`0&nbdxPznk^Jjf+cEg19azrI8#Q=owlFNM#WO4ZrtW{u z`14`G0Rmobh)h{;mR_~9Fv7p$knZ+pLQ}29AQ@(PQG5$6qmV_FR%2xzKb;C}y!dct z@`a073aTD~_RWUoZRo2ttxbMvfp5`!y`{G=5QW$Ez(z)V$Og%daQtxJc>A8q@rt8Y zs1ytS6(alO5pb}yACkKzX;ZUBWk0uEbgx-n@EIcexG`fi-vJIN14f-sl9rv?0M;gN ztHJU3n4o6#KhM+O(e%%m`L`cbJv8eoopFw+Pc8$k`I*nwP72zxF*z`IjOAjzr>ia# z)+NN!EhKsmH-gn(z()JGtyvGDT{*Dl6%$BMIcVvhUY640V4!8y%&ZR2^G}BuC6PI( zv;j$+v{HLj_`H#L*o@UZ`|sVPd=Rm8R5ih#aDtiTz}k22gZ@)rzPI*$+3uWm{t@a!oy1LVYW(f^`!W)Wd$(NeN8N9tl&5S zWht3ykUB#vV~@&jsAu-5`NN7I5pJ;wn$G?E1E`0}5#&pREM10v!BT zfBOV%)9!)4J_m-2ek7PdAI1Vk6WORKd8`Xy3mDkMVoq@jMl`B++@Vd zEdV$pNmUPO`&k6LAOoL>-hDnYNlU+en|OFI#vjWp{H*jl@`>%qKk6c25*v8#L4 zzYA`yqCcrsjB6-=`F$jkOcgUkns1vKhO8Bix!AJp4ev}_Kp3AlO-_3#E}D1kJWRv6NY z>cBntss9yChak#{D_PGgn!vA|R@L)%rWv$ei;kK&r(X3ssq_EtLppLpGj1Kqs_(y{ zVXV)#v2Kj&vp3soYA4X87t_t0i7an&YBx=P}k`{I-|2-g4cCu@Tmzt zDQDhY@L&%tG!OX9lT$$J?>*qd7dy?F<`i%6~)ecA()f%JDmo`8@y4y-tq?pYHkotJ|Eu+!U$b@my*b4Gb1hO^k31+A7|xy^Wd< z-;bdj;f3kcFgRl@)eaQYvdo}t1|@-wvqnk?M-CS^bur|F;{VxA4Y$ioX?o)&&T~e&B_ck;%1>wd+TCT@zk8W@lfE zry_vm{jfZ1?Q^@1Ht`4^*efg~?)qMQ#I`0id-N1a(#j#@@>+!|Of_EN5h0b2?-(wP zj+h!hRTb&(;Sr(!jLu*M^xi;?gkBq{P|yd8wdwWdlE_HfXyuqO)^lF(UDLSJ2)(N3 zoB9I6qY)1iDWh<}mibp2Mk20Ab*t)#gOWFYhF;s(voIICOk3vv&fYW{%l;!1V5ZL5re0hm^{0^4{sB>0a1gc#X^E&=IWNXD03lG24>6#kl{f@?9Pm zn{g4??pw9GC&EfYS_gHoeC+j0r}g&C6J{Shz5Ac3OVF!aP5&(BqlOJlh)}ogW9TK9S!zo zn`*Zg`?+oAJyQeOkT5c$H=>Z3(kkly8IPQt6jLBRsodZRAZEZkmYbz|5p*xeJiN(H zt$;=+qT&Hzz-VK7Gm?Eg_}8shLfikLSl;qo+{&qLfH3e+DjQDKEAA%d-gG+id&!gW zmNqwH2_Th5#=#_0I*rXius3T+!6dWIIb*#`hL4rs0E9w!cq&6Iz;Z z__Hfes@+6$$$e=d+g7q z#3bqYgqM3V-;ND`IbPB8c+id-A7D0~gH>=zGr9?KLeInLNu%G&t*-Xz6_YYSS4#ZP za%*w=tUF|FZC@{*IkzX1)G2woQ(#0L6eTyB_G){i_N^6dgw&Sqv3->Np~R*W-*C>O z0uw7x$E~FdS+%qDpX;f1iF;e=&t$NimNKCtPkF0=29Jzodauk$jzX+zv~+;rmkT<6RjtFW{k zfyWCIdaT-FmtEs`Uvw6vZmc*^w(v|oNmdp>)YiZaklOO> zZcWxLyyqUW2ky{a`M{(OYk)i0!fFiP?7qB*s)gU|%l~+dejpr5I^R-bV+PVwyEU@x+zxyS@A?x2Pimb8B(x z;CbOb_{?0z`M4u|*stx){cr1_BzSJXESeU;v2E#z?4qXo)0580#|q>DPCByn7NO~= z8rLH5qE`19g+gdFli3az`#oSB=sXBtGh$rP`NY^1k_jF)fS!Xoha>jdJ__=|5eS~2J^ zvls)L0NyYAv&M4J@u)@-SBYKeI0O|>`^A+1qGQ|XH=LYSX1W~XvcV%3%6Fp^5Y+ut zP_K?UTO)J&zmeq|%*^MqaV9vy0ylaJ8n}<8{xW5(p+s*-V4EdB_e2X|Ef=j?r1yCp zX>4e)bN?9KJJ$+li`TDQV*kTyE*n=3C)mxMo`zI5Z}ifoEM9lLb-|peFNQxpN~!|O zWpOW9+++Kb22MHd1LVDiuPfb>x0{f~UFp`my@b^Dq#X?MFzJ$*vIqbO zkGj(4aDXRm!rM$#gnQDiyzPWs=Se&Ab`$cvC+)<`BIE~8+L@P4$d27;0dGGc!!fy+ zkgL1V9eBG4nct0e)Zd9om&AKK^f5%LJI#lDA#I|U$+j-yL9viFg?>Vs&&$}2EgvI9 z-DVPUi4a-->NnhaK!`?}PFn8^(I{^n#;t~4Xp{@d(A>Qcb`c>%ywE6DszdLI$;9oE2-PQHkIMq0=CA^=M! zVe&0$J%Irq2>BN#=aPVR@I#i(C**KUE+XUtKV;5&5+M69xrvZ>{cymN32y9<$Ww%L z^+&UsPe_qJ?ZCT6$a(%K5pEGO(;o%Hu|wFsfA}M1Ckd$?fW)qo5OEGbfF#z20HpUf sLe9W|J%rpAfbjV7@sMzI@9>_{x(H#-N%vV4tuhWPaleYg8&KJU$Y^JeC~SyyK} zto2vR>=n`4umsx2ltu%q$!9nVm>nB=S>K_ksg@jKP4X(;)zba=a9!S{H<@;mc zkyeB=sDEyT!yE`#+PSg_1}6e%L`2a!7skM~iNF=y!Hp#s{|&Rt(a+=fr}#)~y7uBb z@IxouE((Un5%zS}p;7R>6RTrq7`Y4B(J_}t!6mzZD`1k@meg1a8}huurfaR@@v3`?n=r!Ozx* zh5|R#4|#G0{O$m7&cC_=&Zz{hr3f_PzYW4Dn50Ki+&=GWUm76=7ePt!^^11-S9j5Q z_vNrCkodIJO9?!LMC+@N><)&}owvApjwL6OLh%>BT{xp_KcvFz$>AA8o}WDlmeW34 z-lTmfA6oLzhxXNb(u=mZ!SUxo07HnFRyS}bFxd^X%-`A?`c?sZu}4a*ce{7L%RaB( z*(%_6*+%Hw54fSX?4Igch~V=o-~fN90%|&d@~X$dZ{dzG!Vzw~0PI|Z;QS@2Sq_U!d8v`X)g^jz2LvJ;4BQk1UfeA zw#pMY(0+Sc)hT@8PHsTWjq$ggXB6mvpKRA3te^qq#xT(72CLGLX$8N&4B8+$o}M0h zZo6z>@T7uVznB?cWz$i6J@7j^aC(FIsD-D?og0&89O;tTxq82TYwH%bd_XboJ%e%p zzr(y9U^G2tpBVmf1w?_{_&3~j6}Yw)ic4aOquhQJESotntk}cp@KCUpX^7v#huuL8 zG+zUbF!~x$fxXP|_i+{zjzibo-rG0kkI}>Pr=Kxhfzx_|>BP(^m~PH zR^21q`Oz<%k8C+!{yB;TD_aT$V#hxvA%B z%9!3C`-A* zd`|7HUGv!wyFIReg5F8y)HKqZmf8X1)^`&B3r(#Y?A2rb+;uU1_qH`GtYA7>VFy+~ z{|F+`TOgkF`1Xgjr@VqT9DWcg7`X7bh2u<1u#6^m_;?7!37S3o@|L8|4xhF>Y+KJK znT1Rj5ZnMkp69-jo&Ix2pTVOujoJ-CdUS4J3P<_j;S0 z?v#sPerX3FycT)+`|0R z!oK$(y{&WKtH#)pnJwWoFBB-A-T-2eMI7->;BR;Aao&HR@+Jrr)i7fP$J_(~=?tpjidB1q&_0*8oqkPw31O4{H-L_yS@v)uE2rm_ZY|QL$WkhR8 z_~o%3?so7P=d(|=XWU)P__GT7-U3}jxA3SAxdqxArtRnyRx3@Oxwtg##P&>sW-A`J zvo6T+S0aW70sFpR+;`0|?z{Ex>FCiCrHb)CO-UWC5e_+JipLXSS)xS@2-f{Zc0 zt}cUr*;42HqnuzR$cpL?=#7Q(nG(4Kca(r zRlamdZ1bdK_(nH7q9?Nwj-Zos>`N*(4F+QFgrmK;bb$VS!6aH=I@yIgyTEGb-w(Lv zZ=Ky_F40mQikvt~z5&~W5h8lm)`?>7Bx1OjXz5Ik+cw^XJ3BxTJ-)mxeRP`x?X*4J zg*!FC2Ie*F8bP$9({~u)-3X#B3Uu5F0+00r?)W9*nj+>-66W=4I3$V)qi>g`G?@f+ zQu&f5eh+`>MRcY6?HmVFdn4l4omueY01yY)_8~e!yFuU+ddHrLVy>&Bh`Ypv>(gKr z4v{EGJJ8~NBJhlk+M|aRgTWgxvLDii>(5AOCemk6_N^M+ANmdkpNP30P1hPc5F_fS zKN^0weOu|g^3il`fddne+rVMrK!k>i>)!C*72wgpQ8How7Efoczt-c!7a`d58(+&)yl{ZsHa;H9?0Uq>51A4b>KZyDq=i9w=JN%V4sN-9$$HT6;n zMQSBFjY6twq$(^bCcOM!2DShVh&!VqeLH2vtzmzz^Goc1wo}={8Q>p>6{aSgs8`J? z-C!Uoqe>$)Xbn=MRwW}zy+Wx}s}vHMOs|tSHX|}mh(0+vL?)5RyD7TKLSCjOGU=pb zQ-3l6oAWJw+{BTpRnMZ!H^mjc7nTf85HxZ1P3%wOUY{VRU%U2W%@%*Yacy$i{^z65 zOnOq><<_|8YYEUGi}^JTfJK&{8^%C_?5@@zkJdgEUF6SsHZov&6IX*9lzj5Bv7L z<#XoR=-sY)CVR8=llWrrzm(@R5gMPANQI6|F{Q@SPFGzlfnF<>>2x|$E|q9Vtx~U* z>Xm8~EOct6(nyjDV0bUwdXU-e!i2euiUj{{r5nSCPO zq&WF0D6}6_snDiGsWzaU8H(ysr9r1w>t$*K3hoe81HS=0d?qQGGV3SC*3D0}&Z~st zE77aGgG@JD1&(n0Zk<<|2TrBg0<3x^;PW=B*PlNMlcGX9I&8rH&)(^-{nWnIz~CM` zM???L2e#rWUj_Fcv|SvT%9THhga&)$JH+s+2v$ZNV9e5`~-Nmt2e=+u5X@l zPd_)8?l>%KrjdQ%;iyS4i;T%~P!V;Nthh-9+@Gn&Cm{ZL!-kPj%VWbNhnondfbtSAUJv(u&k<3YhN$FYB@c7Oj8s)xX?AmJT{&f72(k zr1_+0VCt70+tzs-CI*=@LEm7C2gLZ0OC3wpM^~CfdVzW4KR7Yiz_x~%7h8PC48q&1 zem%PIacS7bUW+HYyrJ_8dRpjZb13_3oHr~uwhI69S)KFP3(e}A!#3~jywg;8D^^}5SkUF(OB9Ryra=nbwNmXivLS@vG64D5VKLr7Nj%_G`Wv0^5 z{1%GyR~$}h@$Rl2`8VBk>qCAFuoaHaLCp%+aQNn>%8RYLYZl>#GslwnL75w77Ct=Q zagOtk-#uK(I$t*f1d*C#%|-d@Bx=1;q1DQy8Wp9{D&-ofM6E(+O41-P=w&csHWA1t z8-hk-LYgU|K0^?m9=j;>{8WF(hy<`RJ-oaj`^5{9Pe#8HVndpdSN~BwR?@ouu|J)A zziVy;*)?*?djY0n;=)s`v?edSZZ_euj;N#pC2(X0y`Iu4)f77JqlAu%)N5r5N+H$i zjBwi=A~2dq?__d{DItxFt2dD8VDMto$dN|^f<8J^d-~FL_PLlBC?69w2DMgr{CENI zu~9t$HqzV=$IM=Qu%EpsWO1iChWlF^N=p0y`51`>Xx%mXD0P&aNx4x{nOve&sbmVZ zUMtb-NUc;NQ9?x)(UFgdMnu0i*rdZ3CsMFN&%RwfXT+he3l?Qo_Shq+jciyJ`9ShH z)o>tLj;BH;0#d6|kd#!8^i#{Ea-B|Xl&RHPElC>WuqKP}esL(fmrZ!UWebQma6DTn zoa~=ave-O)^z|#n#g&`b&emK&sP@HL5;Yow+DIuhMy-aFNHwU|f%K73da2eRLv>6e z+?q}J{-yZ-Sn_8VIvwA!-laglcF2Ywzgt$pUJaW80||Z6x;yvPDdd!jqA00?GU!!$ z8EMcMBxtv{S28moE z;Kf3R=MryXq4kb=?(olp`=1VU$?3OgMkmm`7>$LN8zow!L}M^eS_4W4D-{Z<4z1p5 zN@3I(jdC44fQ06gaOnNci0Ignro^{Raa59dz>s%w)XBQmJ|@PnZdFOfvf8C$Y7&}`U zr_hj630fKTS_K@IPXxZ$?XvO-AMAGbrzIE)lMg>z(_8-jwug5%vrpj7fPvi3dS$`M z4+*E&ON~+ur68qhijqhTMpOYYD5W~1QmxZNcbe$%(#<~{UqEY`Cn2C2jd&(nW3^4|07 zM&A8*^AXSNmeHf*gVU>f%uZvEd^2Dmq5sZ(oL)SpMx*;UO|Iiu?fy^QkZGHm)Z~K9 z|K#q9n0s}Fsg=`qSKH$f`RlqwW7(FAxwl9fjy13K5(5g8;>K4wE&?CxnenxXQa3QH z>l9`YbDe@+1h)AWLpYE5VZBHo=3ZiGIPbs0%>q8B8urZsG52Bt8{OstG#_+!sa?#i z^s|TEW^_KoZY_U{xutjZUXxEf+=gNjs~+B@9L_ou@hZdj<>6a(HLQj@wU}F`wpCdh zP+4_cF}HqfJ<~Qo#I;y~tMYeDNSM zg&fbipu(-=*oyLUhd>n(GqH&g6%uRB$hP1g-!^~Nen}TLd=4Qy_G6>vMF*}{rJc(Tf zg`3x9ZxsK2c9cQjCSaR4mB}D*v!yHqUI|7%_YCS2aO~Wsn48*Un}|o^kI&Ac9+#E) zYm#te?JSxhOUH~THxXz}-!y9~jX~mO?O3`rL*9#7LAa?km|P1wgG%Oq9e`(Qfn$0O z`t*nuvII?H*4AzF|MOgO_vS@=kk8_^1XO{T8&NNpGVEIy`hsl4Z9RYrnw&t(av4Th zRbloUAlJ%T-!1p3e0(0meeB2x@Hxy~K)45_CJCB^p}RX7rre`lKq2Z7POTx_(-}T) zFe%){@KvnH001J!|KeqWCT>=N+sxW*4;HeTuqT9|$&4u5HNOQTbxljK2&ZbXG+d*f z#yy`t*$RBl?l~pN(zy&RB!VV(tg@)^hZDEaJ7hwVx$g(K5!aC`d*W6ZPn~v3bv=PfJP}`L;zCZkXDH+f*)S2_W;0 zA08R*H-201<)h(SQ`Q^gA#oe!SOraztVLHD=G@{6ckFrAjFX^A%4ySd#{8!bo_D~G zhdD>oeU2e$k|L_iX1FtEEVMXF-8qZBgBCQ|LT_4$+R*8bB%)a>7;4j_Zlco#40Y|1 zLUesOL-|#A6D`El$m$SL#v=K6BQ7sca#J$|FmWb{yWuA3<93ncrkfBCB ze#;KC&3-Hs9bd&fs(9Q@v<5$WfS#Q#W}XGtbQ2veWT-JU5>eR~43%FKEIP}~6MA<| z+jMTaw(J2jh{rMy*yEu=n>F-JV*Q-t=2OcaGt3L;&lQ4s$Uca zGQYN7lGRJZT%*BrfxFq9U&3(aUVb8~7pURJjiAX!xab>(yXdoE^vQhO{4UPlE?<24 zz;-96y!m%VFy9h?bXukOaFPjD*Am`vY%SqfTK*?7u)FBY_vkZX=%^KT5Z%Y5QY-8% zdWcD#R@h1O2PWrfg@K|2hZx>1T48|b7$$$!3Om-3ExHSX9Din>N-~PFkSD_3QlUE> z)?Mi0P|I{7$++u=f(#0I?cfabRP^alrnkcG!nZ`TF?poBFhn#9leOK2p`t8I%6kaA ziLx5pLqXyu*<;cQH9E9BK0hCes-* z<1i!R*Wp5`LncE?GQO0eWhKg8=vEMcI`|B~_liKQvoSf4A+s31rh_v4-Z2sl_%2L_M+!q74q8~(3;_$_pGVM*j+{tjy`z?=MMgBV z7iOD>M%sj%3-#&)nGfX~W$T^q{>W!4n!sPo5nTN^Qy^&;3u&Zxj$mf{6)Eim; zAohn=eGqvLlhQuO2`*sr!#=_w(QQnA)(1_Y2blc25AvOy{frE!`k>Bce#ek@(WqGw zHgae*VpxSC-j7E8Zo}kMhG9D<*G8j$cVqG=hIbDppEBf+m<;TTJoP6`4(p4Woy6pn IzQU0I2PM}OF#rGn diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 4562e346f4f8e80e88c83065c8bdd32bf53e4d76..17dcb6eb15705c9706b6ee99ed7be4cd6990130d 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~m0~0RS@z1T+8u literal 17 VcmZRcbt>7YGeh$`0~oM{001*P1ZV&N diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index a6ece5318092f88b3e69e0f3257cc511f3a0387c..47528923a502ef3c105715722d15bb3916dabd85 100644 GIT binary patch delta 2029 zcmZWqYfw{16wajqL6L{Kk$8C|5TIBP1~byyYJId~r9MDK4NeHub_Rx`MQa&fq&g}_ zP>d@aBBBOGRICxfakRC8GNP!JijPqNGd_SaQU$?MO2>5X?zTVV&fM&mJ>U18vuE$! zd+CwKv@=jWn2-7I}7H!5rmt6x7a(JX7n+T7MBXZ(ml zB5gt5m7(Am!%dX7J~4No%t`?k$TnNKN4HWBr%m7#!B zG8mL`@doGq=l3VXHC+4q`gC{ePW3&J-G;2+QT8|s9^b0r@_^A6*L|Qn+%WdfwC@87 ziK58(J5~6~hAVmu<;IL({5Km8DDU&es3B1?^vFC5GoA4r5)DzKxlriixd2r`p(gCb zyo0W7d-vN*4K0o$uGZc+X`6TK4(9Whl z03QVvOoBNKMZyM#BH#yxqQFj(_BeQtBMPk*1@0E@wCi9kp2B5nkC8I}B4r-hP0CD$ z2b7<@6GjQN;Cz_MP$8^hs2(f~T}4#|WL?eR>`Ry5OnlX-w}?kkdx&p4n!SgxdY}Ze z#6I-FuM7=>gQ0&Q+K)u_9GYPgX?hPR#8bFjeJHU_2XBGnkce1=2_hiepGu=a z$50FuFf<+N7+Q#K?Ip%q2nnD99W2F2Op!`bc#{-xB!DA1zJwncF%ACsA1Reoyc!bz zLnw70aoI>*6x5|j$?>AKflRtWmKs-Y-$$IciVGJ&qoRp&;d2!=&4*1CrTBxDp&)on zk+vH37K%&IUV&MGM4)YiY>dQBy-)H?7Ml$B$2^lrzU zGmpM-OL<&YzLLC^c0L0=J}khzU~DvG=ldbh12@8e|E3Mu<}jWuEdVq^^>*eCR4=2P%tONCDtCC zE7|aFqorx)y*F+Jb{3sa{iF|u`z?!bw0obdY0h3%ciZW(4?eL;1&cr*!U-z%Vq3YX zOvvxb3(9{c)W)-D9QAYl8q6VFC|#LHiIpv3nVr@FbF1MG(EV_^tmy1l>Waw0q%pLnB(#dzGpsF3Acykt=MQzyvDqk)( zSgu2tmJ5bsU58Z{V--Dys_?$Xt9mc$$D63s6YtI2?Vi%-lDZ_vJbV7RvNatU1xZZN zOBG`;4_9pMn7@ADjo-|H@i#^&qi*2yT5Jz*!%Q^hq>su~UGh;yaLvU;^P~=8qY*!@ zBz}iF29%>J3+c~#)M6198r&^8lH)bsevZImrfqii?b@ci|ikkvHz^uU{*|<`&J-4Z1+HA0UYtEBPKrKvlV*$F%qeQxaSf{)^GbE uH;jux+fD@G4o0LS#}*-2|8h@*xqL>=V1Hcfgt#InDv4M{-6i|*82K-P3fV;f delta 1877 zcmZXUdr(wW9LM+Q$}4#y8&b8#S@+dz;F>?%|*?0>#y;o8219 zN276ea+8n!nedK|&_xL+x_5t?@j@a&dCj_W4cM_t3$HxTWNEXmbpzg9B|y94cBsrW4$j=aklkrG_ zaZ~UTM~9phNf|-4R8bsyl?o&tnL`u@wo9?1+7kvn(T6Iz&$`Sbc_G1Bg)w;EFFhxz zwxn<>(>j^3I>l32udnM=wD)9BIu$d%=93k)X5T8(M?B=y@T5#qGvL~L#cjOtQU1Ha zj30ApjV|Ap)hM6B1ZNZo{W7G0tqMxPdT+*UScdh5GBMc-KaiwI9nj0jss~?h=9>fYoW6o=PWjkS zM4J|Yg@^Wnx10$IAdXWmWN_L8r#a=pT}I|6a34z*<||lTEZUh1A)SQCnC748m;-do zZ;I)d6>x{uTk7Erfel;_Z*#f~shm2Xf|CW;?xXAKg;73i`A?~@Kkh4);R*Yx?;xJD zU#z+>fy1Oqq><3V$qrs|a)e3aSavjyHqf#VC?O#-?)?aAyB^#HB*RSx(f9sL803OT zc+;0j;~<$+JnZ9?3>P`A#%%|v@p=eQFu^8BASAX(r7hfP3ot6k*Fb`4aGn=#gU6h7 zpj0w(1}xx|g;yS=E?-a=Idi$8L^9F(OB!^5G=DPv-GkJ*KwP*0Q&g-`5xlQrrUux> z$hs50?&qzxMLz_2@P6w@j>otsP!!kf1tlyfoYINT^1(ddJ!D zv$grJv&>Iw!flD3-r!`{U1p7Z&>MX+ci^z*bqy>JK!W^t-8$>e%#Q;-vSWP|ekQX2 zkUg~2)`|^F1*i&O^S9vTO9lC@sdKi3hky3)RPIIX(^j+cf1_GgB)nHyc6F?#+pfAl zWbK*wS+1*JXZa|3!jB3gx3VOK3G$bm7W;@io~jxLYUSW6Q*sVS9ob zmkIh}$HpyBq`SjIZ_P}qo)txQb8*_fe^?V2GSblJcPPx@u-F}Kk4$~mGR}CLd_Q!3 z8<}FvhT)2+7@J*t^T^vH@c!}{^T#-G#Y9qxJx+P69@O;p%#XP=LRg@0&$=%8jprko zqu`HgD+0;H{m!_vLV&&QNcIky|2a!LCJk@+?<%FsoIksoF1gIz;u;&3$PD|BGex8q p+;t~gcw7)ts9a1nm0QzyuMB8tG{r^4>RIU^D@|+BLtYRH`wxkjtQi0R diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 2643e4012171a5cc49655f862ac626ab4083ca2c..5203172dfa1bff9fd5641b4f6cf66324f03cc7b9 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~m;X1OP1Y1b+Yk literal 17 VcmZR6A~{9FsWEOA0~iQy0RSwj1U&!% diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 71e3859ae9a0b10b527bb0aded9660d5210d0594..55d6aebd2e342e026aa3208425555f8869e2d6d5 100644 GIT binary patch delta 3377 zcmZve2{e@JAIE3J6{+`?xb}50L$(-8WzSJ0yDMucCA$!2FoxW2Nz&7xge2jLKgMz? zm%>EW7TJs360Rg$w7A^&o!q}^&Y3gkeCG3ff4^s$XWpbaY*IX1iH0DyicS=a!Q#(P z9Pzv>Qjm}N_Z5}9g1#>B1?|fc{?i=c)CN!_qyu4hCUcZtx{EN98huE}5er}Ks>Z6* zxEv4~xfLjhXya4d{B}H#+}epi@-b#?O6}}5JKKyqD9;0~A|-J8RAC__pUj5B7Qi7Y zC9i!k*qb+|0Zmn524v_4LO!5X7~#}xASN0EXJF;@;~T8*KM3M#}T@%b-I9dbeJ5d_90GbIbQ7uHuy9KcXWys4F#HIO{0y3;y$srGdEUC; z7cD_xbPuywhw)e#NkfYd;ZcvibAQjIhI)x@sINqqm07Jx3Sp7K^eNm;Ed0*JDqmG^xE!HT zaUfpK8ahIu{(ZrA1!d!)T`moto`J7vwi0y^NR|Mu@>cLnBbS!(4Ni2wND#=Cx59rM zkoNG47@kMW=n7_2v-+v!tzjZN5gtnbQ}QE_8jt+oaT*z6F&3Z+p9X)e^*5wtJt#&4 z0I(-mLm5^d6-ZrX2qRB|41yJmY3KeN6ehfYz^J3(4Z#a$PZ7k=7VOPH&3Is<;01FI zb5uCBhJHgJ^$k5&!4M1GHg+Pout_5@rkg&b=!nJh*%1=PwoIdI8fndJpYLqm*=GeQ z!H7+=1xiX@ut456%BCvX5(+~>qEZ?xCSUXN%jb_;4)3A(v-lPTs$%a6exFoMr z(k-ylb2Gw7smw;*_p0v@XU|1>^cJ9`DFwZl2A$ywA!s2AFwxY3RCCH?WLD(R@n1ox z<`PshZHcqc_z7V#T)_7ZI=b`GOJGQ=8TS9= zeW+Mu4qf+18pzPrh68%7DozAb8qv%Edb{>{EJWP16f${}fWYV=V6poX969M5VWHS) zk3h;8(A^`6qh#;2?IZ0%Pc2o5IZfs#21C#(mFoS8Zn(cg%|Ly1tkCM4ckK<=V~zb= zJ`~qo5Aey7_5BdN3dR55sND6@r|vB_Y>+MX5M9qV8Bh=NK}rS9zet;BQ`GevzThtL z0{sXLJCT;%FQE{|>nPrC*$b=(k zw;CR&1^_iXK8nNbexr`WFwqAA-qWN^a|(rYXx-S&T&s9xGfJb}El|o+--|y%vRPM{ z21$L`WKl(2;*C8A?L|RbH^`09fRlUN{F}bERe+3oUM`Famo%e3u>o2ym*pW4UYpgP>OJ) zUirS(gL!!Y|EcnQPGqSiMxc2hWAoU4Gc8ev)2tyylOt!QysN8E-fJy(6Zh)^+0J|v zKRIH`ll(ZRPRw+;(MTriC}ZkE>)t1KBy+KD>cy$}|CDB~m0nS^F4-ncLR`W`y|JOH zmo+1?%;&o4GOu?~L|&*trjX~lL1BiNy-`-p3%zh?bRzh{FN2TPZo>`h*k4^S;Mt?B zh5M@M;ZVu-gVUp%)*mdD#>F8`OUXC5?`6T7G4O{=-G2XWar5W5y82!&$YNIOZz$7r zpAw_CS2MhXzcBasY0nwfkd_96ARxKFhH6d=de*O@&uX~yC)wm;L56VxiG zTW-MBto@U17!^erG&#n`?OTKA-#71A+s66`1G2hyP_C?Q*M7`G{uQ+=`jXx&fR|a;a6S#+ALBSWJuj z(8R86DY*VEbNs+T1MZvOaugngEA9i<#R5kXEsgmII{uv}6;G+zL z5{fMk_;vU33J5tJm1RxRUXWL2a6dIQ3;gx@p3DhpR-?G<1#bM#*d6n7<;>zF*R^NY z4ZBiwAvFDpoztk;*-*Uq<}G#W#`hn<`7(F&kIM-%j805=kvc(PU13g%%#r@8L+O&K z+_DqMsEGZ+)rYx437`D)a_#V$hb=pF+Mcs!jLZ5s=ryUFu!$InP|MgBj*(*NHH%k# zfmfo8Kd1Vwc$o9vWl-wEM}e=?3yxj9tNj*R+FViigLCIy(`e@N$~{afF8)d2$(1(N z(g{f_ZlPcJlU$G2Gj1ClF=aK%MjD$8@MFuR0~u_C9#QP;#`jgau|45YT;k4y~?tKeL3vN%0@Scl}9%Z0~l}}0su7&1iSzM literal 17 VcmZSnIL9>cl}9%Z0~l~D2LLqe1fKu^ diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index 4d87dec7473749f17f8f1838d0a1622b9c687eb2..5d1322567181368c0e6c016b9c71880800a581d4 100644 GIT binary patch delta 97 zcmX?kn(_Z>#tkMCjLSEhN<=6K1m&FyEOOEpWdMWJlbc&DB3J}+i;`O>zyIY46}`Rr ps!ar=K$)`f^o9Sua-pJcH>=uJGcxYns2IVq+0mmzc5;1$830mgAZ`Ev delta 42 ycmex=n(^dm#tkMCj7v6~N<=7ZUTP7*vYFR5fN^uFT`?nL;YP&;9Ge|II%EN61`ksJ diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index d26e3b15142f0e0b683e1c70b5b758704926410a..f35a33761a7aeaf94c784d97e25f8320eea50aff 100644 GIT binary patch literal 8 PcmZQzU~K<=Ur-hR33&pM literal 8 PcmZQzU~Ipce?}Dm2_FK& diff --git a/src/main/java/frc/robot/Autons/MoveForward.java b/src/main/java/frc/robot/Autons/MoveForward.java index be6792d..e9e7f8b 100644 --- a/src/main/java/frc/robot/Autons/MoveForward.java +++ b/src/main/java/frc/robot/Autons/MoveForward.java @@ -1,4 +1,4 @@ -package frc.robot.Autons; +package frc.robot.autons; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e365bf..296806f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,14 +33,16 @@ public static class OperatorConstants { public static class MotorConstants { // 0 indexing - public static final Integer[] driveMotorIDS = {11, 21, 31, 40}; + public static final Integer[] driveMotorIDS = {11, 21, 31, 40}; public static final Integer[] angleMotorIDS = {10, 20, 30, 41}; - public static final Integer[] CANCoderID = {9, 12, 8, 13}; - public static final Double[] motorDegrees = {91.494, 182.373, 347.783, 102.129}; + public static final Integer[] CANCoderID = {9, 12, 8, 19}; + public static final Double[] motorDegrees = {91.494, 182.373, 347.783, 188.174}; public static final Double[] angleOffsets = {0.0, 0.0, 0.0, 0.0}; public static final Double[][] angleMotorPID = { - {0.1, 0.0, 0.005}, {0.1, 0.0, 0.005}, {0.1, 0.0, 0.005}, {0.1, 0.0, 0.005}}; + {0.5, 0.0, 0.005}, {0.5, 0.0, 0.005}, {0.5, 0.0, 0.005}, {0.5, 0.0, 0.005}}; public static final Double degTolerance = 0.75; + // max amperage sent to the motors is roughly +10 of the current limit + public static final int angleMotorCurrentLimit = 10; } public static class Swerve { @@ -67,7 +69,7 @@ public static class DriveTrainConstants { public static final PIDConstants ROTATION_PID = new PIDConstants(0.1, 0, 0); public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.1, 0, 0); public static final Integer wheels = 4; - public static final boolean[] invertedMotors = {true, true, false, false}; + public static final boolean[] invertedMotors = {true, true, false, true}; public static final Translation2d[] wheelLocations = { new Translation2d((TRACK_WIDTH / 2), -(WHEEL_BASE / 2)), diff --git a/src/main/java/frc/robot/commands/AutonCommands.java b/src/main/java/frc/robot/commands/AutonCommands.java index e2769fb..f8234c1 100644 --- a/src/main/java/frc/robot/commands/AutonCommands.java +++ b/src/main/java/frc/robot/commands/AutonCommands.java @@ -1,7 +1,7 @@ package frc.robot.commands; import frc.robot.subsystems.DriveBase; -import frc.robot.Autons.MoveForward; +import frc.robot.autons.MoveForward; // This file will be used to store auton commands (activating intake, etc.) public class AutonCommands { diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index f175678..5cb245f 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -70,9 +70,12 @@ public void execute() { SmartDashboard.putNumber("Y-Vel Input", yVel); SmartDashboard.putNumber("Omega Vel Input", omega); - if (cnt++ % 50 == 0) { - System.out.println(m_driveBase.getCurrentPose()); - } + /*if (cnt++ % 50 == 0) { + //System.out.println(m_driveBase.getCurrentPose()); + for (int i = 0; i < 4; i++) { + System.out.println("Module "+i+": "+m_driveBase.getModuleGroup()[i].angleMotor.getOutputCurrent()); + } + }*/ m_driveBase.setDriveSpeed(new ChassisSpeeds(xVel, yVel, omega)); } diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 2c159a3..5f71ddf 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -47,7 +47,8 @@ public DriveBase(Kinematics kinematics) { for (int i = 0; i < 4; i++) moduleGroup[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i], - Constants.DriveTrainConstants.wheelLocations[i] + Constants.DriveTrainConstants.wheelLocations[i], + kinematics ); // m_odometry = new SwerveDriveOdometry( @@ -65,6 +66,10 @@ public DriveBase(Kinematics kinematics) { targetModuleStates[i] = new Module.ModuleState(0, Constants.MotorConstants.motorDegrees[i] * (Math.PI/180)); } + public Module[] getModuleGroup() { + return moduleGroup; + } + public Pose2d getCurrentPose() { return m_pose; } @@ -105,20 +110,22 @@ public void resetOdometry(Pose2d pose) { private void updateOdom() { Twist2d twisting = m_kinematics.toTwist(moduleGroup); - twisting.dtheta = getDeltaOmegaAHRS(); + // twisting.dtheta = getDeltaOmegaAHRS(); SmartDashboard.putNumber("dX", twisting.dx); SmartDashboard.putNumber("dY", twisting.dy); SmartDashboard.putNumber("dtheta", twisting.dtheta); - Pose2d newPose = m_pose.exp(twisting); - m_pose = new Pose2d(newPose.getTranslation(), RobotContainer.ahrs.getRotation2d()); + m_pose = m_pose.exp(twisting); + + // m_pose = new Pose2d(newPose.getTranslation(), RobotContainer.ahrs.getRotation2d()); } public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { targetModuleStates = m_kinematics.getComputedModuleStates(chassisSpeeds); } + public void resetDrive() { for (int i = 0; i < 4; i++) { moduleGroup[i].resetDriveAngleEncoder(); @@ -141,6 +148,8 @@ public void periodic() { moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); // ModulePoseOdom[i] = new SwerveModulePosition(targetModuleStates[i].getVel(), new Rotation2d(targetModuleStates[i].getDir())); } + + // var gyroAngle = RobotContainer.ahrs.getRotation2d(); diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 1811dd8..93821b9 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -137,9 +137,9 @@ public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisS // odomSpeeds = targetChassisSpeed; - if (cnt++ % 50 == 0) { + /*if (cnt++ % 50 == 0) { System.out.printf("vel: %f, xVel: %f, yVel: %f", targetAngVelRatio, targetXVelRatio, targetYVelRatio); - } + }*/ if (fieldCentric) { this.gyro = 0; //this.m_ahrs.getYaw(); @@ -186,9 +186,27 @@ public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisS public double[] getVel() { return vel; } + + public double getMVelocity(int moduleNum) { + return vel[moduleNum - 1]; + } + + public double setMVelocity(int moduleNum, double velocity) { + vel[moduleNum - 1] = velocity; + return velocity; + } public double[] getTheta() { return theta; } + + public double getMTheta(int moduleNum) { + return theta[moduleNum - 1]; + } + + public double setMTheta(int moduleNum, double angle) { + theta[moduleNum - 1] = angle; + return angle; + } } diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 2926e04..1547fd7 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -25,6 +25,7 @@ public class Module { /** Creates a new Module. */ + private final Kinematics m_kinematics; public CANSparkMax angleMotor; private CANSparkMax driveMotor; @@ -34,13 +35,16 @@ public class Module { private final int moduleNum; // ZERO INDEXED - private static final double kP = 0.2; - private static final double kI = 0.0; - private static final double kD = 0.005; + private final double kP; + private final double kI; + private final double kD; - public final PIDController pid = new PIDController(kP, kI, kD); + public final PIDController pid; - private Boolean invertDriveSpeed = false; + private int invertDriveSpeed = 1; // global, always applied to drive speed if motor inverted + private int invertMultiplier = 1; // inverts drive speed if 180 degree code tripped + private boolean invertMultiplierAvailable = true; + private boolean trigger = true; private WPI_CANCoder _CANCoder; @@ -58,36 +62,43 @@ public class Module { * */ - public Module(int module, boolean inverted, Translation2d distFromCenter) { + public Module(int module, boolean inverted, Translation2d distFromCenter, Kinematics kinematics) { this.moduleNum = module; + this.kP = Constants.MotorConstants.angleMotorPID[module][0]; + this.kI = Constants.MotorConstants.angleMotorPID[module][1]; + this.kD = Constants.MotorConstants.angleMotorPID[module][2]; + this.pid = new PIDController(kP, kI, kD); - /* Angle Motor Config */ - angleMotor = new CANSparkMax(Constants.MotorConstants.angleMotorIDS[this.moduleNum], MotorType.kBrushless); - configAngleMotor(); + m_kinematics = kinematics; - integratedAngleEncoder = angleMotor.getEncoder(); - angleMotor.getPIDController(); + /* Angle Motor Config */ + angleMotor = new CANSparkMax(Constants.MotorConstants.angleMotorIDS[this.moduleNum], MotorType.kBrushless); + configAngleMotor(); + angleMotor.setSmartCurrentLimit(Constants.MotorConstants.angleMotorCurrentLimit); - _CANCoder = new WPI_CANCoder(Constants.MotorConstants.CANCoderID[this.moduleNum], "canivore"); + integratedAngleEncoder = angleMotor.getEncoder(); + angleMotor.getPIDController(); + + _CANCoder = new WPI_CANCoder(Constants.MotorConstants.CANCoderID[this.moduleNum], "canivore"); // _CANCoder.setPositionToAbsolute(0); // _CANCoder.configAllSettings(returnCANConfig()); - _CANCoder.setPosition(0); + _CANCoder.setPosition(0); - /* Drive Motor Config */ - driveMotor = new CANSparkMax(Constants.MotorConstants.driveMotorIDS[this.moduleNum], MotorType.kBrushless); - configDriveMotor(); + /* Drive Motor Config */ + driveMotor = new CANSparkMax(Constants.MotorConstants.driveMotorIDS[this.moduleNum], MotorType.kBrushless); + configDriveMotor(); - integratedDriveEncoder = driveMotor.getEncoder(); + integratedDriveEncoder = driveMotor.getEncoder(); // integratedDriveEncoder.setPositionConversionFactor(1/6.12); - driveMotor.getPIDController(); + driveMotor.getPIDController(); pid.enableContinuousInput(0, Math.PI * 2); pid.setTolerance(0.0); - this.invertDriveSpeed = inverted; + this.invertDriveSpeed = (inverted)? -1 : 1; // if(_CANCoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED) { // throw new RuntimeException("CANCoder on Module #" + Integer.valueOf(this.moduleNum).toString() + " is not green!"); // } @@ -97,7 +108,6 @@ public Module(int module, boolean inverted, Translation2d distFromCenter) { public void setSpeedAndAngle(ModuleState targetState) { double x = setAngle(targetState.getDir()); double y = setDriveSpeed(targetState.getVel()); - if (++cnt % 50 == 0) { System.out.printf("Set module %d angle to %f, speed to %f\n", this.moduleNum, x, y); } @@ -116,13 +126,8 @@ public double normalizeAngle(double angle) { } public double setDriveSpeed(double speed) { - int invertMultiplier = 1; - if (invertDriveSpeed) { - invertMultiplier = -1; - } - - double x = speed * invertMultiplier; - + //speed = Math.abs(speed); + double x = speed * invertDriveSpeed * invertMultiplier; driveMotor.set(x); return x; } @@ -152,35 +157,42 @@ public double getDeltaDist() { } public double setAngle(double angle_in_rad) { - // //code to make the angle motor turn the least amount possible and drive direction if necessary - // double targetAngle = angle_in_rad + Constants.MotorConstants.angleOffsets[this.moduleNum - 1]; - // double cudoublerrentAngle = getAngle(); - // double normalDifference = currentAngle - targetAngle; - // double difference180 = currentAngle - normalizeAngle(targetAngle+180.0); - - // //if going to targetAngle + 180 degrees is not less than the distance of going just to targetAngle - // //then turn normally and also do not invert the motor direction - // if (Math.abs(normalDifference) <= Math.abs(difference180)) { - // pid.setSetpoint(targetAngle); // angles are in TRUE BEARING ( angles are negated ) - // } else { - // pid.setSetpoint(targetAngle+180.0); // angles are in TRUE BEARING ( angles are negated ) - // invertSpeed(); - // } - - double x = (angle_in_rad); + //code to make the angle motor turn the least amount possible and drive direction if necessary + double targetAngle = angle_in_rad + Constants.MotorConstants.angleOffsets[this.moduleNum]; + double currentAngle = getAngleInRadians(); + double normalDifference = currentAngle - targetAngle; + double difference180 = currentAngle - normalizeAngle(targetAngle - Math.PI); + + //if going to targetAngle + 180 degrees is not less than the distance of going just to targetAngle + //then turn normally and also do not invert the motor direction + + //what it does not do: does not go right or go forwards + if (Math.abs(normalDifference) > Math.abs(difference180)) { + targetAngle = normalizeAngle(targetAngle - Math.PI); + System.out.println("module "+moduleNum+" target: "+targetAngle); + if (invertMultiplierAvailable) { + invertMultiplier *= -1; + invertMultiplierAvailable = false; + } + } - pid.setSetpoint(angle_in_rad); + pid.setSetpoint(targetAngle); // angles are in TRUE BEARING ( angles are negated ) String name = "Mod" + String.valueOf(this.moduleNum); SmartDashboard.putNumber(name, this.getAngleInRadians()); if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180))) { - this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -1, 0.1)); + this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -1.0, 1.0)); + trigger = true; } else { this.angleMotor.set(0.0); + if (trigger) { + invertMultiplierAvailable = true; + trigger = false; + } } - return x; + return angle_in_rad; } public void resetDriveAngleEncoder() { diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index 48a8b45..6169943 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -62,7 +62,7 @@ private void configureBindings() { public static double getLeftJoyX() { if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { - return m_driverController.getLeftX(); + return -m_driverController.getLeftX(); } else { return 0; } From 1c4fa533b1fdecca9b6f5b02ac2b0528920850fa Mon Sep 17 00:00:00 2001 From: roboticsDev1584 Date: Wed, 10 May 2023 12:53:13 -0500 Subject: [PATCH 21/31] Modified custom odometry wheel delta code --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27797 -> 27797 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 36911 -> 36911 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 76065 -> 76065 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 33423 -> 35259 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes src/main/java/frc/robot/Constants.java | 4 +- .../java/frc/robot/commands/TeleopDrive.java | 10 +++- .../java/frc/robot/subsystems/DriveBase.java | 16 ++--- .../java/frc/robot/subsystems/Kinematics.java | 2 +- .../java/frc/robot/subsystems/Module.java | 55 +++++++++++------- 15 files changed, 52 insertions(+), 35 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 864483d6cc6eeb6ab03de7b3ec692679521f8f5f..3924fa61b6ce4973abed6e401cbda1d61e8847dc 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oN(1pqNE1Uvu$ literal 17 VcmZQ>zc8b1qOQ9v0~oO60st`m1PlNG diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index 3da1217fea652f4ca0f1cf0389e6a42c8619a9be..d4c8abf22ae3377bbedc7d0130290972bcdc39a6 100644 GIT binary patch delta 241 zcmbPwlX2=z#trvlS!SFz_M05-CAIl)EC&;iXY+7!ua_Q>p|3Z2KaBAa!eE*_KLISI zJNY7v(e4cv;hww`!qA!g+FNRJW8x~1S;r>(`+%fvZcc85FwOxPQj^d6h=4>7Ob!gu zWo-qro`8kth6sas7bjQyN=-fw<=veeoeVO>*mHAl@(orX%V6i`%*-~BYpnw&Pt5{J cS_e)(4rbW=nEW$KPXOqCKL#-H6$Dax05pMO!T3(0A>?d?f?J) diff --git a/.gradle/7.5.1/checksums/sha1-checksums.bin b/.gradle/7.5.1/checksums/sha1-checksums.bin index aa7be5df19b88de6849c1f6c75eaff2a28757cdb..de6b5188e5bd8ea63f5cbfef06acc4f9805c6825 100644 GIT binary patch delta 262 zcmZ3#fNA{#rVZs8EHh3U`)#hy$YKGqY#vVD>tzIF=<7``3^8PF1+z50!9q-v;~@;) z$p>@6BD#}LLqxbIGx`|u02S*nfPv+~$qVz6H=oOMWCWRdY_eh)*f5)$llgt6CV%#k z012N1azN(T8~}^P`+`NEfH_kkoQsn;LX_MEGNdMV=Sy$yDRgE9YBKhmeAZuTa(|5Y zW|s11L7<4i&dGv-Qj=>tB{uKvcmYyl9WZ%iAIJvlz{&T)44WU56??#L_nYk9{{#TO C@@b#| delta 262 zcmZ3#fNA{#rVZs8EQ>YG!!}oEWU&BQwsR)$^)do7jIK{E3^8PF1+uP9*7ODot)3hY zVO*VjFc&Oxb@FM5$kxe>K1Mu1#a9@>!1Cbag?Y)F&*eEXf=q3gtQZD1%y#l*eqX7{ zpM4}i!W}>k$Q;`$uxPw5SacqkGZn(=nYqGb>P&dC=su{!)|s zW5hSJls5|kMND!h3kFI}uI-fAytm^8NR7?2$t(LnHrPC$d>_oPT{~H^2P9%_IN7`Z F2>_LrU(5gi diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index cd75cf5fb2ae9dcf18a49af8053d284ddbe5c759..7a20392c45db2e6df2add0f5bfe33a9bb4b49100 100644 GIT binary patch delta 1467 zcmah}X;4#F6y`ig!WMZA4WV(0gvAQ2B9%d9QIQhDA|Ng8NL!qcJWDDe%BBoj!`9G| zEj-{VOhGNJWd>PCA4_M17HbhfD_}tjLRlopFtrfdDOj33&>!&!_s>1^o$t)O_k8EO zm-E?|^VtpIB8MFwo{4ggw4;f!vGUZMH_)x{8jlirF(O6 z(4w8w`ZY!r@~gybNlTd<`82MZSxQ%Db;zg=8B~O>f2x`i#6giU#cqS2H*<)_?J?Vi zLWQM6m8P``+D@oOfW>mh9z`?5OPFi6+rls+uPMEr8+= zh*rf|&Hpy0Y3B>WOK5U2+t>(i>fP>{>YN`UiT&OwS9z6$ zGJi!vQnEN98A&x@Z#J0UtaPt7(f4{U=7<_JV7-bBn?V&RZns0T8sL}?(7Z7d6z&TS zwo#1ehpfedL;p0KoMZVqxBfzB=gdi5<;=h7dbrc&YoVjq`w>4voXx&TCu>arp+EgF z8$`|m$NUxz-ZzB{=)!(%6Y84_p)aZX3IF(5to~P(|nRoIxETOCaRpWSJ!CMbFY7 zIL|-+?VeR$KgY=-V=28(xpY4 zB_i^sx1K)d)4~ltM_>8k>pJOv?GZe18+>%iCFo$?s4Ti!2j!?{I)8BEv=5lZrT9jf zo9LhgmH*!K!|#-w82PsU_u$G6I;iLy#val$C*+HoH{~BycYa9P<+@PXIk*nIn1u{m zR^`^N=L>erjW*+WJ$+ms)iV#v0qYvpnBRP`VFA$SPK-@yJTWcfl{qEdW4UXNWg#nH zOobZ`r=CHz`(k%6+;EQ2-9T`qy7po97;dOP>L`VcRHZ-Wjp2qm>x{b@SqZT$j-jgX zo!*yvQ(KVu4F>8KVrMyqGD6*&yLCq_e&i z3f}_^swW8h%k~u!kWzvLAUqiRlu}5r0WME?yJGZ~eSsx0l=DpPHQaV*R{tH$QqgJH Hf!_ZCQRZs` delta 1467 zcmaizYfw~W7{_^DVYx0GmJnuPxZ{9MTvFI377>wa$fcB0r|`JD=mzcry9hWA7oj0T z29{I3L&`-!L2?8HIs#LGago4aQc5fgjN2|~Tr^50V-MQgICAlBr*4PYFlvCkVim57HzBhi3Kj400p6Rj*r>i z;~w{3`b4^Csc{+wIb1sI9w>klHs$6~lk$1QpDJ0`2fz2ppqc`aIc3*vfQ=^M1aYUe zJL-?-4?ih5^^}Pm40tEs}g&?Li2h2v3+T9j_J%28PdWj*an<( zQ=7j{ZLgSDwNBfl9^gNQi)w)Gm~m41&h?#v9eGH@|_gBDTx#)XATBk{ZD+Uu71 zW?BXCRe)3?lamrT7AnC`pCPNY)wA|Pj<`VyX6xFhK9o~ryA4(;funbm;l{Obq%SyF zI_~K>uc+jnR^~gLp5%5${m4}34JabdncKHHw#&6N$dUBaQXs~(s)2llM7b^YVDcjg6UgDI=&+&{;jZ<^m-V(2A?uk>kZ!zF`Uu-)MuB^xFzZ@|STw$isxeTq^ zW_iHgcj*0jF=Mk#ZH%*K-PNmsXht?3V3ljV%&su$Sd%J04DZ(qjJ($Ei9HTJ`MFjt zsq(%@!uu|m6&;{g?(DNB#eO&aR59|8{xv6$G8Jsi}e!1=6 zQ(MknVx=zL{6@W2ee}QscpCWnz*aUzH!K6b%R_8h>5U%gyt1d1=OOdwiPgm>z9^5- zTuwDXxBH?YL}>029u9`hboUNaj|ffwap#v{MVAF2A4F*SycXO|*dz$$a5P6Xv3FE-h#>aZXp7p4uj&2iX!ni( E0g*e0)&Kwi diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 17dcb6eb15705c9706b6ee99ed7be4cd6990130d..33234710cf2d11465d984ebd749146dfe5512d62 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~l~l0su3A1gQW3 literal 17 VcmZRcbt>7YGeh$`0~m0~0RS@z1T+8u diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 47528923a502ef3c105715722d15bb3916dabd85..01b2b2df2f942a2d561891fc7b138d64ae8ba61d 100644 GIT binary patch delta 965 zcmZ2@iDls>mWC~i{=)Tn71rxD+(OcQyX{j?{`bDjz`)r4`}M^g3}C=%2&D9kfrKUy zD}t~c9g>?E755qLKXH^})*8ja zCOxo@5HA&=N>(5iHktgfRCu!78sX_bg&6~amT*qpKJz$--f{WE4>|3RJOC>{W^n_k z6bK~yfdrZb3d_s58Eg`DYZ78Mg)Q)u1?xzymt5F;rMye+|pYgPV_sUkzDd zHNEI5Sk2K6Cx{w#zv=qCjB3+Ai!jP5EPEVpzq-7JyWrf{PtSD>CxMkNcL;_kTv8 z#1}<^5JLpM18Fp0cs`Cv+L@&xzWLY2%CGPG{h>NUrk_+{)CC%yoONOPZy593bX`@b z=#}a5s*F++p{!B2*}wEf{@76QhTT?mCs=!O)`{s8VG55=j*pU>epeOj(B!OB(>L-n zN=@fiV-&UrnSKf4G?sQur=8*0u|tZP;pRhE-jkdSQ9)3%uR(M$+f3frp$c~4-N|>$ zr6w=4aDJ($*2-%@?|{Q4#Sp36ci zN{zp(!8-OG045v|5ZwSI(87-U%DIjkTA9jy=YxK)ywq+2)gcX3Iaz3xxI)UN=7%l0 zaY1Ws=1(>Mzm^dyuLLrWfkEc>_Cl#3QLX|}qtt+&XS%gBY<7rxL7glC zQO*U*2xtz7``jqOK2P_^-_LS?Hi|tIfa=hP=&<*jeqRunygW64$;<7`?0csa`gl@r zw0_IAy15IYv=kWMKu~z$)%11^#&A$lYMC_of0NYozZ#6ff*|fxh+zymWC~i{=z{50XoOd{VfP>F#F#SYV+2efq}98&Z8xP3}C=f3#9anfrKUy zD}u1Vx&QID%1pYql?aW$4>n^?ZC6XOZgf|_1($q%OI*H z7XoQCHyr)mx!>!}oHyF_V&5_vi-7m z_e;%e+Z9cOlhSkk<_uIOt_*r&;6jaAY zhz_x%m^xaX`Bi=YC*$;8^9;MZ{uy9Qg91lnKTzdlp;h7v9Llq;m#b{q6*|-OnN&^Z z4XFGTh>3DbCvRM+26A!L@GyLXa+IIH%U$ZtHCHN2;$~L3}cuAr1b!Zi#759 diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 5203172dfa1bff9fd5641b4f6cf66324f03cc7b9..45c06a03c76195af71b4abdfad87a624d609a6a0 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~kmx1OP0l1Tz2t literal 17 VcmZR6A~{9FsWEOA0~m;X1OP1Y1b+Yk diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 55d6aebd2e342e026aa3208425555f8869e2d6d5..b5fa480df9ba35ce450dcac4589b885ec1afe02d 100644 GIT binary patch delta 2890 zcmZvedpuO>8^FzEk}aK+kxM8O3Sp38hNu+DA}JyiTVg0$mu|>y7?R9*8ylJiU4y9=M@Rhct)Y@+^-D5yq?rOLewG?wSuN zd)JvEEWsUk>G;E=kv&;if8~BdU_1|0>oh_#Ki!-vdA%29G!V6>1pfALsq4mQ-Teqm z)C8KkRCuCG^U5l8qHN_~402ISUNG;v6&Vlq8Wu_O!hIm-J#GAP`zyy7uq!BQ@mIkS@ z@AFi@UPOyz16AV^SdWvT()r~k+AxV*fWVjvlkL*hu$H^EASR<3h^I}{8iGwAxzPcw z^Ad0=nULm%G0&c|s|ZW{gRf`ejfWlT4|pW7=pG17!hrvJXAeAPBcNN-cc->r> zgJxI)9Lz|NeBRZMY^Cc6V^V;x85Qil||iX}%8m{0?T*T=xeKjhn*eYL7WqX~T94R{=0e`_g8h~sax zbu`N^{^Jd3)v#g&NcmkQRyp)EmYN1uDcc@JRHWb#pYWyPhAuxl+mmtB5;? zckEtZk~Vp0VVlCTOb*r<@ZfmAK1kRckQ&fVkT4>G<~>BHj#x-?Ftxm-CvrO6N`DI2 zfkbE$Gz9D>k8jw!ST&c^^bs@%6CrxG=;xHNeQ}OaY^MQRTd*XIXt;gPFph=$;!qiy z-1X$K4Yr~Vp;oxEKAm-JUgsmB^QUZ3dXgAzx6w|Gqd3^%XCFHJuwv#t{BFSlJ>{p| zc*V6A?2m!1w}of1hFrCuN62MkQN=YcwHw_{5Z0L5r(`3aF_pnsH6gunO@;iN@ox)7 z|1XkM=j82kmhqO`66`)Qr_8p{o9i())r%<3(mojqFNiwjSmT>)yP+n&!~F_a`yeoC@%FA$UmJj6!T(ALwG zwLkBsJXUM|)j{R%KgY}prIylc8MnK(e0HypoQaK^bQ$^TylGVVqPfe#qo&y_vBt=e z249P)H{Rn#CT#cou>q{nCv|5vD8AdZ^|#*~|CG{OH)s62KPovT$z?vBJJZ!ACAL2s zOPy0@_ovw6_NcO^zg3%D((|d~g)<+7?^-FZ`S&K9Oj*bJYKYe{eybmAIC`Cs)s+(b ze$KCdCB?jW{|{bq-aR{eqrHo2x;A&$3s=0H`Sg8PX7j{#uZTkH>@&@2T>*`)@32Nu z95d(Ct?thb8k{JVc+n5m?10avK*FtpzGwrR8qb&c8b3IW_77Y+xpbSNs@{}iix0T3 zjx7UisjoCK*dIfFvFQL^N2(8N*tGT@8KT~_s%SC~lSad& delta 239 zcmV|e5VUzwV zNRwVHUz61=!IQ2nr;}|iFq5?|kh5Jcp#qaGG8mIyGN_XtGcc28Gr*HRG{CbKHTMCN zB{wmXZa1fsEjTcfbvUS#H93*9-Z@tSlTAGElWjd1leIm-lZ`$ZlfFJLlLbF9lTJU^ pvpqn`0h1m=FOy|L8MBr`KmwEQM0b;YMfw6MIk6$w2D3p(PC9kKU9cl}9%Z0~qi$0{}G81b+Yk literal 17 VcmZSnIL9>cl}9%Z0~l}}0su7&1iSzM diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index f35a33761a7aeaf94c784d97e25f8320eea50aff..2a8062866445889d36426c53babe86c1d2aaed3f 100644 GIT binary patch literal 8 PcmZQzVC-Ns-0BVh1rh Math.abs(difference180)) { - targetAngle = normalizeAngle(targetAngle - Math.PI); - System.out.println("module "+moduleNum+" target: "+targetAngle); - if (invertMultiplierAvailable) { - invertMultiplier *= -1; - invertMultiplierAvailable = false; - } - } + // //what it does not do: does not go right or go forwards + // if (Math.abs(normalDifference) > Math.abs(difference180)) { + // targetAngle = normalizeAngle(targetAngle - Math.PI); + // // System.out.println("module "+moduleNum+" target: "+targetAngle); + // if (invertMultiplierAvailable) { + // invertMultiplier *= -1; + // invertMultiplierAvailable = false; + // } + // } pid.setSetpoint(targetAngle); // angles are in TRUE BEARING ( angles are negated ) From e5e538da3dd6c17be165b24d64d9428a062cdb7a Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Wed, 10 May 2023 17:02:34 -0500 Subject: [PATCH 22/31] old update --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 76065 -> 76065 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 33423 -> 33695 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/Autons/MoveForward.java | 90 +++++++++--------- .../frc/robot/commands/AutonCommands.java | 26 ++--- .../java/frc/robot/commands/AutonLoader.java | 6 +- .../java/frc/robot/subsystems/DriveBase.java | 4 +- 12 files changed, 63 insertions(+), 63 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 864483d6cc6eeb6ab03de7b3ec692679521f8f5f..88092ca4c2dc7000f25ccaf8d9b9f05da7e7bf15 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oLr0st`q1P}lK literal 17 VcmZQ>zc8b1qOQ9v0~oO60st`m1PlNG diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index cd75cf5fb2ae9dcf18a49af8053d284ddbe5c759..88129a0e923b2a9681bf833c10e3529887c872e9 100644 GIT binary patch delta 3443 zcmc&$Yfw~27WVWEh=ah)AdfNx2NeUTNWiElDuN=Q^3WuJh&sX$hPQxb1u2JUt)iF! z*Kk%61T9MAs?n(BfRA9ZfEa?f5#OrU1*{@U94Gu_|mbNZaG zyU+dhZMWFB-Qviiz_F2^X<{{LYZ5cFlTzbS5|TBOv(?&6&y1ABw8=w+AK_L99)iax z?}R8WA!w{+{x1C)-P<356ow1|&9OG^hxW&uR>hh+g#Jk|t|DPSbO=E>t(|RN!(Wac z$T{8`*yif{LbWaR6BcyiMXmm#7Pc&O_MZ4B!Gy%1RPU4kg%Y$l3Bz%GE^#es0tL#0 z$qHwXn%BQ(#IRBpPOUhJhT*uR&yq4UN^#$C`@1 z*C7cT&IxfQ@>7~&IElxC218y)Zoq0hjy*>ou>&YP0{+WAK+tAN|Y-!6w62|>Yq`5 z;!Cdf?Nm}vPnL4DkV}57fGY-KWAW=WLp}At=*r?FFlJaP{T#({Up`Ra`Wt;mtfxGE zAtB!qRCK~(?7Rc^+{`U59fH|O;g@jD+&N}Fy;nj?qAi^;r48o8Z~Q}C z!iqq7J4}aXB4BSjctDp3@VpD2#vm&nz!+@23v%Vs-_O1r{cG@UuYX2%RE2rBKqUtn z8%|X#V=3146~i2K0mR%^;AXAt%*X&Q=f9gM-($FXIQHgLubNh9QO$1X2lG8BID$nokyN?Tk(x+S-=?ej5{O4 z)SD~{nDL+#5Ig4(TjUxd6wx`*`lj!&a+)Uer>_eWw$T%BWx{aqMtl+`K z+`su$*#vj6jCYTqhja64Te+us#UITPd%Q}c|MiMOuBYV+tKX{2Iuo4MDWg~WQ%L&M zaz}UW7wf*#S9X6MpRtxg*{YjI&$KUZs`gqe4K{JoagJMDNdvo^i5UJfgz2q1!3!KC zO@6+bWAVJD`lB8*#T9M-fiC`<6y)-sSMTZiZBJqPys^Rm^=l~o@mTnge*@m0N-Xpb zrw}(nyoK*Ae^+gW0&I%Uh-L4b-QG9wwl^6LJ;v+ofE#C!Hwpa$&;}RDJ2_xgE7;hv z>nwTgI&*KYqV0&an4xGjlb8&2&7MgX_dU+OV6C%=41H#iS>&2{4+_VTOM?&@mvm|b zHq0hIubVJ>CXB!>zSMSQup5s56zp)p95S0Uif#0w2a^^tLT~x9zNXfn-!M$_CtlDi zB1Jlvc#^aIo?!#5ljhT!eCH9nfyGnUoNtmc19^j=1Z{fb$i@Ds4#-?+n^Y+k3LLAxsKg?A;94}LCOZ=4;c(+ox5Kzh2D z2l>$g?}MD%TIWBI*t8(xBp>{s@R$4aTxi#|!q8b@f{j6xAhBfo)G$lM4W%#+=1~2S_ z31}Hk4)I1o59q08>K&*a#= z=3>QK@;=vb%L7{YWJ2)f1`>eNRuNb6=S%roi!uskrlSAf;t#3#+lggbfto?WOdpd- zx`5TqRR8}}8Lmf<)qMxV%fP7;y(*VnldAIW&K_pj6rVJ_JtZUb*Cm(t_42K|dFMF^ T-@NBWV0b6k>nE%uIZgioOlBY1 delta 3876 zcmd5 zc*ao?aRJ=w9z`iy_nn{BwrX*}1!Pfi-|O7mic34=Gtf@(cZ^B%MWc@*AUC|cFnjT3| zXpXj(&)z0G6Vslt`de zSHpBQam2?bfYYrrb9xj>!e%X~MgX~B>M2MOKrSdg4e9v4j6N%k$z5$v?C^LsamIkT zG-1FEiamP<_ka?mtgg%=={R>H+LNpMXnpNc0noBOO;0<{8(j!3CZT>TSY2E{R6!@1NJ3H&JF z7`x-qUrp=aDgGy3tfpl1R2KXWvr2APg9LML^4ExGHx2#gQ+B7NlLG`hyOHVSlc%)9 zg3!4R9K#jaS!z}0lyLQogc0hDnTiah$KMn)6z1Y%5TrWSwyXMI>XZ$xscrM1G%+K- ze}+0voz-6#)ZaAdxsVnVIQQQ$xSJk0PMxL5N(fPnNmaxru>3E1=pdJnPH~U7;$>y``+(#EiZ|j{t9X4xmPL|qog`Im~ zRXvP^CO$po{0N9O8p1yZ=Qa7P{>km-`eOL*vK}>Pv{75g%DiY0At||Zx4}fkjP(3oc7vp zI@}F9)w0^v{`*xglY<=k#RZF>cGHb1XMo9IsAGQ>c`guqse{yMpwlhd$Sr3w^lUm|aQZzYc$q zsLp-#yh58x<6!|s3?HUeC=)VxbkT(?s;|8o?=0xqY1_A{_BZxy)1qJXz)ely+Wj!I zifm~e>p$Jvw&QxrUs{fAd+KXvwhZq!fxAr&18laSw?*Mcdg2$&AQfF^P8{0|1Fe2w z|LZI;!jI15Z<|4Eb)LCeZ8&j(E4EUP&1vS@VF8GH0!SB>wSd&-B6I#|1tpjsNC)~v z>J(jNPE@ymtJQV(|ELI`_<>xsw`@=22TwfK0zE{vI#eV5@3^(8#JO^WV_V*yzR%8` zjS;V8bUc!yb1Qu6==)5hh?=)%Q~vYSMC=( zIQ75ypqi}~Tf#E^ZPQb3)pM;+nalpl$Tfo+y5%nZ`5MD*!LnB9WA&C5!*$1458|lI z>$Gv6S^55~pva+`Nd-~057=t0lrnti&~Vb2Sr^oOk?fgmN#|X_n!`>(w8)UV?)rY!bsv6#}y#Ctydn=`Sa$;~BC;!GBUbE|$+y+u? zeh6crmAA)$b~=P$Kn}4$hbPciexec6uY~QYe6zUkx*ejKPkz|0y#r_FX?4Mn`IyrN z=QZmdgZproHUIr}UAk}V?q`6vy@{=+X)x(Sz*j2|w2ffj0bmgwbJ}siI-@)O`UU9( zExaw$Pc(p)rql3)fQiucQM;MS&cXaQZ3LT4)3h6^Eb5_<81sjc&&YAY`MwL*mMhab ztT3PsI(OA?VA@+yCJa++5yO0Jm(FT+J=i+vgH2nF`#jFR=#VA))k6o2^&ysE5{yS_ zG`9H=KQ^@(Wis^nlGxx*U-Bh6Yg8)prudObc*G3*tCCPD;wGL<I?J=TQ>oM^uf+6f)L4Wq*CD9uvVhIu{kqZaYqEdKoxBPUM_||W8N6DQ1uym9!pjlnYZ%D2&_;LccYiI^fzO)UMCsZ*T(;;Sa#RTWZ`(U-vCUh zYHlZ!|E-A5g?obLc0_t!2BmyVcY(@i+4={dSAiW`G?)KW!Uyt47lm{3_>TtWqvy8( eeqo(T-(q%nKA$)n>>&2Osm&n{nxI)kdFemEY%1>n diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 17dcb6eb15705c9706b6ee99ed7be4cd6990130d..4156d86cbda3ec2a0bec79f74d350fbafc381b21 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~m1V001)=1VR7+ literal 17 VcmZRcbt>7YGeh$`0~m0~0RS@z1T+8u diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 47528923a502ef3c105715722d15bb3916dabd85..7156196589942e5f95cae672e1e6a0281d1c5a4c 100644 GIT binary patch delta 993 zcmZ2@iDls>mJO~g(>Dt*|cn)N4Z7*pMtMK zRPJ96R#!d|^gaU{I|*A_x)U)}DNK(TX6y-1VY7>t?#`w&dz)whsUX z1uzAyd94mnBDyUPNiUO;5}d=*Ael&#$rJ^iE#G`43={|#eK zo30B>1M{cHt1?PW=U0O!kGYd0mu*m(&SN{_yGWkXk0OclJAZubfLag&u|Py~^22mt z`2sa28|UknY7-moey?8S2l3|iLWmmCwUY~Ht4(fPo(xVpk0#$8CZoep-8;bMpTtsmVuIg46AJ{>gV&N=dxidHkMI YBKwKc4a|qb&$LTGU2m|Nf0a-@0DYZK6951J delta 1247 zcmZ2@iDls>mJO~g3Ky~#K05tn?p@CX8P|Ty=J?9Mz}Q}~K{SB@3e=HTAEVo8@ za(BxXg-6X@6SJ~vooDub`MIbij0>c=J$y!wCd)~@slSO}Bc69{%VXw?Zh_hI>P2SicJUxS-QJAG- zugv+$@ljF=yI(S`isq>4eQzipEua$^57lxRswHdsYko#G9-#8O5D{jk>4|ELY9Iq1 zO}<+$HF;^Y2upkVzUPy9JL`f}U2W!^ivC+=|920kMs?F+NVx2~1JNs93Z(UnfdnX^ z6hT;ETGght2Lw+(spMe(>eAVe57Dt-2Pg>ycB`k~7i3hM%-kiXkifKbBeUbfwx^T- z>HnVIAOKN$m1Cpf!kg2(Ch;j07G+cfbRGZPe=tQPcdqnQh|h#& z!Ri?p{WnX_HDF-m#*`UKD)#4dg(kRkfO$uX3`L2S=VA3d-koj^s!F0T_?isEOnW7&lzg0 z6hw#Moay?!p!nG!TC$mOi5p|R$NleatvC31{^?u1wV9>=1)_f6YKVFPEDm3Bep=#1 zU8Uyw38$8yOy3;`)$s`GrUoF5=G7a*a=(R@XWjbYrnm8?R^~*I4qyto0aQ8tp9-V6 zSH`?n>63;(=5(F!-~Q%Hn=VwrRfv(yGcb(|WRCg2kgGvs)@Gifb?0rTLv`Gl9Dt@%b3Z-*U4EAhBneESn;@2P zEWxyFg`3z33#$iDZ=ULq$oO}u7pmg~M91X9LXpY2%gb3oe)=={_Cl%2%q!eLS=e{7 z?}`mU3%M`-z9PW=H~!CcQ3KxHjUe5?I8=w|=9`9T->+sa#)AzjZLXInm(9%TY=PSM z7NUbQb24v}DyY~n%$!^wD>ePJ2BQc|Lv?f43DmQc9v?D$l2^ ZE6u029lWu-`_u_es8QE8^RE)B2LKJ-tGNIG diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 5203172dfa1bff9fd5641b4f6cf66324f03cc7b9..29bbcbdb0c5b06bb7b0bbabfa2bd24f51f2ba6b3 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~m<=0{|>h1Kt1t literal 17 VcmZR6A~{9FsWEOA0~m;X1OP1Y1b+Yk diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 55d6aebd2e342e026aa3208425555f8869e2d6d5..df669ad650788d7204bf9174fb75bfd324b1ab29 100644 GIT binary patch delta 444 zcmeBgWt!j4w82DzasOmfiIx1;;VtKMn)MmLpgDT;Rfz;vfhAi`PZMVgo&gbUy1ZFe zZXr9rt;YxXsi$0_f*Uskw2Vu;>Q&5j;^Ru0BM<&r=w2*LuU^HOulnqLTe?9D9pKh~)Q6qG|%p`f<0A=cpdw=d3B zRRcl}9%Z0~l~!0RS}}1kL~e literal 17 VcmZSnIL9>cl}9%Z0~l}}0su7&1iSzM diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index f35a33761a7aeaf94c784d97e25f8320eea50aff..7190b3d5028aa03876cb32ff5d37509c5e8efcbe 100644 GIT binary patch literal 8 PcmZQzVC-ODv+)oB2KoZ9 literal 8 PcmZQzU~K<=Ur-hR33&pM diff --git a/src/main/java/frc/robot/Autons/MoveForward.java b/src/main/java/frc/robot/Autons/MoveForward.java index e9e7f8b..d71fccd 100644 --- a/src/main/java/frc/robot/Autons/MoveForward.java +++ b/src/main/java/frc/robot/Autons/MoveForward.java @@ -1,45 +1,45 @@ -package frc.robot.autons; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -import frc.robot.subsystems.DriveBase; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -public class MoveForward extends CommandBase { - private final DriveBase m_driveBase; - private boolean doFinish = false; - private int count = 0; - - public MoveForward(DriveBase drive) { - m_driveBase = drive; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0.0)); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (count++ >= 50) { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); - doFinish = true; - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); - m_driveBase.resetDrive(); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return doFinish; - } -} +// package frc.robot.autons; + +// import edu.wpi.first.wpilibj2.command.CommandBase; + +// import frc.robot.subsystems.DriveBase; + +// import edu.wpi.first.math.kinematics.ChassisSpeeds; + +// public class MoveForward extends CommandBase { +// private final DriveBase m_driveBase; +// private boolean doFinish = false; +// private int count = 0; + +// public MoveForward(DriveBase drive) { +// m_driveBase = drive; +// } + +// // Called when the command is initially scheduled. +// @Override +// public void initialize() { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0.0)); +// } + +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// if (count++ >= 50) { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); +// doFinish = true; +// } +// } + +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); +// m_driveBase.resetDrive(); +// } + +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return doFinish; +// } +// } diff --git a/src/main/java/frc/robot/commands/AutonCommands.java b/src/main/java/frc/robot/commands/AutonCommands.java index f8234c1..13cefd4 100644 --- a/src/main/java/frc/robot/commands/AutonCommands.java +++ b/src/main/java/frc/robot/commands/AutonCommands.java @@ -1,17 +1,17 @@ -package frc.robot.commands; +// package frc.robot.commands; -import frc.robot.subsystems.DriveBase; -import frc.robot.autons.MoveForward; +// import frc.robot.subsystems.DriveBase; +// import frc.robot.autons.MoveForward; -// This file will be used to store auton commands (activating intake, etc.) -public class AutonCommands { +// // This file will be used to store auton commands (activating intake, etc.) +// public class AutonCommands { - //public static final RotateInPlace ROTATE_IN_PLACE = new RotateInPlace(); - public final DriveBase m_drivebase; - public final MoveForward MOVE_FORWARD; +// //public static final RotateInPlace ROTATE_IN_PLACE = new RotateInPlace(); +// public final DriveBase m_drivebase; +// public final MoveForward MOVE_FORWARD; - public AutonCommands(DriveBase dBase) { - m_drivebase = dBase; - MOVE_FORWARD = new MoveForward(m_drivebase); - } -} \ No newline at end of file +// public AutonCommands(DriveBase dBase) { +// m_drivebase = dBase; +// MOVE_FORWARD = new MoveForward(m_drivebase); +// } +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutonLoader.java b/src/main/java/frc/robot/commands/AutonLoader.java index 59a5187..dd30e31 100644 --- a/src/main/java/frc/robot/commands/AutonLoader.java +++ b/src/main/java/frc/robot/commands/AutonLoader.java @@ -20,7 +20,7 @@ public class AutonLoader { private static HashMap eventMap; private SwerveAutoBuilder autoBuilder; private final DriveBase m_driveBase; - private final AutonCommands m_autoncommands; + // private final AutonCommands m_autoncommands; private static SendableChooser chooser; private static List pathGroup = PathPlanner.loadPathGroup("trajectory", Constants.Swerve.AUTON_CONSTRAINTS); @@ -28,7 +28,7 @@ public AutonLoader(DriveBase driveBase) { m_driveBase = driveBase; - m_autoncommands = new AutonCommands(driveBase); + // m_autoncommands = new AutonCommands(driveBase); chooser = new SendableChooser<>(); eventMap = new HashMap<>(); @@ -47,7 +47,7 @@ public AutonLoader(DriveBase driveBase) { // for (String path : Constants.Auton.paths) { // chooser.addOption(path, getAutonFromPath(path)); // } - chooser.addOption("forward", m_autoncommands.MOVE_FORWARD); + // chooser.addOption("forward", m_autoncommands.MOVE_FORWARD); chooser.addOption("trajectory", autoBuilder.fullAuto(pathGroup)); SmartDashboard.putData(chooser); diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 8ceba04..25cf51b 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -131,8 +131,8 @@ private void updateOdom() { m_pose = new Pose2d(newPose.getTranslation(), RobotContainer.ahrs.getRotation2d()); if (cnt++ % 50 == 0) { - System.out.println("X: " + simPose.getX()); - System.out.println("Y: " + simPose.getY()); + System.out.println("X: " + m_pose.getX()); + System.out.println("Y: " + m_pose.getY()); for (int i = 0; i < 4; i++) System.out.printf("m: %d, Δ: %f, vel: %f\n", i, moduleGroup[i].getAngleInRadians(), moduleGroup[i].getDeltaDist()); System.out.println(); From 23e18b923bbe20e48a12908021f5fb65230dda69 Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Fri, 12 May 2023 02:50:11 -0500 Subject: [PATCH 23/31] Various refactors + changed AHRS angle to radians --- .../java/frc/robot/subsystems/DriveBase.java | 50 ++--- .../java/frc/robot/subsystems/Kinematics.java | 178 +++++++++--------- 2 files changed, 113 insertions(+), 115 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index db43812..05ef9ef 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -1,4 +1,7 @@ package frc.robot.subsystems; +import com.fasterxml.jackson.databind.module.SimpleAbstractTypeResolver; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -33,7 +36,7 @@ public class DriveBase extends SubsystemBase { Pose2d m_pose = new Pose2d(); Pose2d simPose = new Pose2d(); - double prevAHRS = 0; + double prevAngle = 0; int cnt = 0; public DriveBase(Kinematics kinematics) { @@ -47,8 +50,7 @@ public DriveBase(Kinematics kinematics) { for (int i = 0; i < 4; i++) moduleGroup[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i], - Constants.DriveTrainConstants.wheelLocations[i], - kinematics + Constants.DriveTrainConstants.wheelLocations[i] ); for (int i = 0; i < 4; i++) @@ -128,39 +130,45 @@ private void updateOdom() { Twist2d simTwist = m_kinematics.toTwistTest(targetModuleStates); Twist2d twisting = m_kinematics.toTwist(moduleGroup); - // twisting.dtheta = getDeltaOmegaAHRS(); - + //twisting.dtheta = getDeltaOmegaAHRS(); // needs to be tested + SmartDashboard.putNumber("dX", twisting.dx); SmartDashboard.putNumber("dY", twisting.dy); SmartDashboard.putNumber("dtheta", twisting.dtheta); simPose = simPose.exp(simTwist); - Pose2d newPose = m_pose.exp(twisting); + m_pose = new Pose2d(newPose.getTranslation(), RobotContainer.ahrs.getRotation2d()); SmartDashboard.putNumber("X", m_pose.getX()); SmartDashboard.putNumber("Y", m_pose.getY()); + if (cnt++ % 50 == 0) { + System.out.println(simPose); - // if (cnt++ % 50 == 0) { - // System.out.println("X: " + m_pose.getX()); - // System.out.println("Y: " + m_pose.getY()); - // for (int i = 0; i < 4; i++) - // System.out.printf("m: %d, actual: %f, test: %f\n\n", i, - // moduleGroup[i].getAngleInRadians(), - // targetModuleStates[i].getDir()); - // System.out.println(); - // } - + for (int i = 0; i < 4; i++) + System.out.printf("m: %d, %s\n", i, targetModuleStates[i]); + System.out.println(); + } } public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { + double xVel = chassisSpeeds.vxMetersPerSecond; + double yVel = chassisSpeeds.vyMetersPerSecond; + + double angle = Math.atan2(yVel, xVel); + double vel = Math.sqrt((xVel * xVel) + (yVel * yVel)); + + // TODO: Ensure that angles from AHRS are along the same coordinate plan as odom + //angle -= simPose.getRotation().getRadians(); // for simulation + angle -= RobotContainer.ahrs.getRotation2d().getRadians(); + chassisSpeeds = new ChassisSpeeds(vel * Math.cos(angle), vel * Math.sin(angle), chassisSpeeds.omegaRadiansPerSecond); + targetModuleStates = m_kinematics.getComputedModuleStates(chassisSpeeds); } - public void resetDrive() { for (int i = 0; i < 4; i++) { moduleGroup[i].resetDriveAngleEncoder(); @@ -168,19 +176,17 @@ public void resetDrive() { } private double getDeltaOmegaAHRS() { - double delta = RobotContainer.ahrs.getAngle() - prevAHRS; - prevAHRS = RobotContainer.ahrs.getAngle(); + double curAngle = (RobotContainer.ahrs.getAngle() * Math.PI/180); + double delta = curAngle - prevAngle; + prevAngle = curAngle; return delta; } @Override public void periodic() { - for (int i = 0; i < 4; i++) { moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); } - - updateOdom(); } diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 305f63e..c94fc58 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -8,32 +8,19 @@ import org.ejml.simple.SimpleMatrix; -import com.kauailabs.navx.frc.AHRS; - import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -// import edu.wpi.first.math.Matrix; -// import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Kinematics { /** Creates a new Compute. */ - private double[] vel = new double[4]; - private double[] theta = new double[4]; - public boolean fieldCentric; - private double gyro = 0.0; - private final AHRS m_ahrs; private final SimpleMatrix m_forwardKinematics; private final SimpleMatrix m_inverseKinematics; // private ChassisSpeeds odomSpeeds = new ChassisSpeeds(0, 0, 0); - private int cnt = 0; - - - public Kinematics(AHRS ahrs, Translation2d[] translations) { + public Kinematics(Translation2d[] translations) { m_inverseKinematics = new SimpleMatrix(8, 3); for (int i = 0; i < 4; i++) { @@ -45,88 +32,22 @@ public Kinematics(AHRS ahrs, Translation2d[] translations) { m_forwardKinematics = m_inverseKinematics.pseudoInverse(); - this.fieldCentric = Constants.OperatorConstants.fieldCentric; - m_ahrs = ahrs; - } - - private double[][] computeStrafe(double joy_x, double joy_y) { - double[][] temp_vel = new double[4][2]; - for(int n=0; n<4; n++) { - temp_vel[n][0] = ((joy_x*Math.cos(gyro)) - (joy_y*Math.sin(gyro))); - temp_vel[n][1] = ((joy_x*Math.sin(gyro)) + (joy_y*Math.cos(gyro))); - } - - return temp_vel; - } - - private double[][] computeRotation(double omega) { - double[][] temp = {{omega * Math.cos(1*(Math.PI/4)), omega * Math.sin(1*(Math.PI/4))}, - {omega * Math.cos(7*(Math.PI/4)), omega * Math.sin(7*(Math.PI/4))}, - {omega * Math.cos(3*(Math.PI/4)), omega * Math.sin(3*(Math.PI/4))}, - {omega * Math.cos(5*(Math.PI/4)), omega * Math.sin(5*(Math.PI/4))}}; - return temp; - } - - private double[][] addVect(double[][] a, double[][] b) { - double temp[][] = new double[4][2]; - assert(a.length == b.length); - for(int n=0; n 0) { - if (b >= 0) { - this.theta[i] += 0; - } else { - this.theta[i] += Math.PI*2; - } - } else { - this.theta[i] += Math.PI; - } - } - } } - // public ChassisSpeeds toChassisSpeeds() { - // return odomSpeeds; - // } public Twist2d toTwist(Module[] moduleGroup) { // uses forward kinematics to derive a Twist2d from wheel deltas - //gets the independent x and y velocities of each module based on encoder values + //gets the independent x and y deltas of each module based on encoder values SimpleMatrix xyVels = new SimpleMatrix(8, 1); for (int i = 0; i < 4; i++) { - double vel = moduleGroup[i].updateDeltaDist(); + double delta = moduleGroup[i].updateDeltaDist(); double dir = moduleGroup[i].getAngleInRadians(); - xyVels.set((i * 2), 0, vel * Math.cos(dir)); - xyVels.set((i * 2) + 1, 0, vel * Math.sin(dir)); + xyVels.set((i * 2), 0, delta * Math.cos(dir)); + xyVels.set((i * 2) + 1, 0, delta * Math.sin(dir)); } - //multiply the current x, y velocity matrix by the inverse of the kinematics matrix + //multiply the current x, y delta matrix by the inverse of the kinematics matrix SimpleMatrix finalChassisSpeeds = m_forwardKinematics.mult(xyVels); @@ -134,11 +55,20 @@ public Twist2d toTwist(Module[] moduleGroup) { // uses forward kinematics to der double deltaY = finalChassisSpeeds.get(1, 0); double deltaOmega = finalChassisSpeeds.get(2, 0); + if (Math.abs(deltaX) < 1e-9) + deltaX = 0; + if (Math.abs(deltaY) < 1e-9) + deltaX = 0; + if (Math.abs(deltaOmega) < 1e-9) + deltaX = 0; + return new Twist2d(deltaX, deltaY, deltaOmega); } + + public Twist2d toTwistTest(Module.ModuleState[] moduleStates) { // completely for testing purposes - //gets the independent x and y velocities of each module based on encoder values + //gets the independent x and y velocities of each module based on theoretical values SimpleMatrix xyVels = new SimpleMatrix(8, 1); for (int i = 0; i < 4; i++) { @@ -149,14 +79,8 @@ public Twist2d toTwistTest(Module.ModuleState[] moduleStates) { // completely fo xyVels.set((i * 2) + 1, 0, vel * Math.sin(dir)); } - //multiply the current x, y velocity matrix by the inverse of the kinematics matrix - SimpleMatrix finalChassisSpeeds = m_forwardKinematics.mult(xyVels); - /*if (cnt++ % 50 == 0) { - System.out.printf("vel: %f, xVel: %f, yVel: %f", targetAngVelRatio, targetXVelRatio, targetYVelRatio); - }*/ - double deltaX = finalChassisSpeeds.get(0, 0); double deltaY = finalChassisSpeeds.get(1, 0); double deltaOmega = finalChassisSpeeds.get(2, 0); @@ -174,6 +98,8 @@ public Twist2d toTwistTest(Module.ModuleState[] moduleStates) { // completely fo return new Twist2d(deltaX, deltaY, deltaOmega); } + + public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeeds) { Module.ModuleState[] moduleStates = new Module.ModuleState[4]; @@ -195,7 +121,12 @@ public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisS } return moduleStates; - } + } + + + + + // Old Kinematics Method // public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { @@ -250,5 +181,66 @@ public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisS // return newModuleStates; // } + // private double[][] computeStrafe(double joy_x, double joy_y) { + // double[][] temp_vel = new double[4][2]; + // for(int n=0; n<4; n++) { + // temp_vel[n][0] = ((joy_x*Math.cos(gyro)) - (joy_y*Math.sin(gyro))); + // temp_vel[n][1] = ((joy_x*Math.sin(gyro)) + (joy_y*Math.cos(gyro))); + // } + + // return temp_vel; + // } + + // private double[][] computeRotation(double omega) { + // double[][] temp = {{omega * Math.cos(1*(Math.PI/4)), omega * Math.sin(1*(Math.PI/4))}, + // {omega * Math.cos(7*(Math.PI/4)), omega * Math.sin(7*(Math.PI/4))}, + // {omega * Math.cos(3*(Math.PI/4)), omega * Math.sin(3*(Math.PI/4))}, + // {omega * Math.cos(5*(Math.PI/4)), omega * Math.sin(5*(Math.PI/4))}}; + // return temp; + // } + + // private double[][] addVect(double[][] a, double[][] b) { + // double temp[][] = new double[4][2]; + // assert(a.length == b.length); + // for(int n=0; n 0) { + // if (b >= 0) { + // this.theta[i] += 0; + // } else { + // this.theta[i] += Math.PI*2; + // } + // } else { + // this.theta[i] += Math.PI; + // } + // } + // } + // } + } From 285fb995908ff7dbccde53b7b00be4b161a364da Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Fri, 12 May 2023 02:51:21 -0500 Subject: [PATCH 24/31] Moved fieldcentric code to driveBase, scaled joystick vels --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27797 -> 27797 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 36911 -> 36911 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 76065 -> 76065 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 35191 -> 35803 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes simgui-ds.json | 7 ++- .../java/frc/robot/commands/AutonLoader.java | 7 --- .../java/frc/robot/commands/TeleopDrive.java | 33 ++++-------- .../java/frc/robot/subsystems/Module.java | 50 ++++++++---------- .../frc/robot/subsystems/RobotContainer.java | 2 +- 15 files changed, 40 insertions(+), 59 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 88092ca4c2dc7000f25ccaf8d9b9f05da7e7bf15..8132658ee8e2411b9d45e86fa3c83af0421affb0 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oL@1OPEH1V8`) literal 17 VcmZQ>zc8b1qOQ9v0~oLr0st`q1P}lK diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index d4c8abf22ae3377bbedc7d0130290972bcdc39a6..3da1217fea652f4ca0f1cf0389e6a42c8619a9be 100644 GIT binary patch delta 241 zcmbPwlX2=z#trvlSr%)WhfR+5lG^+?mV*h%vz;@!*Gmt`FuFc@KZJ2@@<#|`_2l^p zV5zH3(0A>?d?f?J) delta 241 zcmbPwlX2=z#trvlS!SFz_M05-CAIl)EC&;iXY+7!ua_Q>p|3Z2KaBAa!eE*_KLISI zJNY7v(e4cv;hww`!qA!g+FNRJW8x~1S;r>(`+%fvZcc85FwOxPQj^d6h=4>7Ob!gu zWo-qro`8kth6sas7bjQyN=-fw<=veeoeVO>*mHAl@(orX%V6i`%*-~BYpnw&Pt5{J cS_e)(4rbW=nEW$KPXOqCKL#-H6$Dax05pMO!TYG!!}oEWU&BQwsR)$^)do7jIK{E3^8PF1+uP9*7ODot)3hY zVO*VjFc&Oxb@FM5$kxe>K1Mu1#a9@>!1Cbag?Y)F&*eEXf=q3gtQZD1%y#l*eqX7{ zpM4}i!W}>k$Q;`$uxPw5SacqkGZn(=nYqGb>P&dC=su{!)|s zW5hSJls5|kMND!h3kFI}uI-fAytm^8NR7?2$t(LnHrPC$d>_oPT{~H^2P9%_IN7`Z F2>_LrU(5gi delta 262 zcmZ3#fNA{#rVZs8EHh3U`)#hy$YKGqY#vVD>tzIF=<7``3^8PF1+z50!9q-v;~@;) z$p>@6BD#}LLqxbIGx`|u02S*nfPv+~$qVz6H=oOMWCWRdY_eh)*f5)$llgt6CV%#k z012N1azN(T8~}^P`+`NEfH_kkoQsn;LX_MEGNdMV=Sy$yDRgE9YBKhmeAZuTa(|5Y zW|s11L7<4i&dGv-Qj=>tB{uKvcmYyl9WZ%iAIJvlz{&T)44WU56??#L_nYk9{{#TO C@@b#| diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 5a889ddde95dfd2f5b30fa494f7aa2c33784141b..8ee89d568cccf0c697242a1dd9cb0d96e53a112b 100644 GIT binary patch delta 1020 zcmZ49%e%ancZ1I_m5SM=dC3_m1(l|0rm4jxsU=B9#;I8uxkY*Tg$xkDI=SH2sp-uX zOi`0tev6CwU3lZoc5wfnFB_lSy!b$M-s%Y(zBd?dKKOgqE~Uc4+}z}%vWh&@v|{7T z<{~DXVq(vrfp1hiI$#h3+ zCi&?u4;g#-RnsM;{P*wKf22aJ`m?3e_WM9VCZQP*By5(c9eS`q%a7mwNWcolC6gIb zMJFp9;hH|@31g2yn0un$ec?%}>$$wND`kvdPj);aKHdK*<76Sb+O73ULV6L-?5`Yo zqGWq%mFL4Wo!eOiz`3glX2cJI70MZfv_e z^TdQ_-x>_J$V~Tn!zkmBUUR)$MffX6dvxXW)b=%&Yr(Sj*cp3$^HWN5QuUH^5{ru& z1b)ODOtP4`;e=u3%E)3Lvx8f}>b_e|U-5>~GllWI5mMn&vdI*pXKj&VK4e{ zldZ615>RM!)4F7lwc-`O+2@>Yvz5}GEZ3pAz3wgJdq(ky6_3lqHUw&IpOwvctv%!6 zWw3P-^4vQGcy$~0RDJq8%p zK@%I>^pH1v@#i>QbF}{fAd7ifHdamjQ?YbN>WE)N-d_)Fy61)pdz*6^R+;1|;OXZwAiAd}Fai_&wI;YpZ>*;FoU!e_e-k+J5l``=f7l>)Hc+f+T;0Pu98?=--?^vto!x?Wf~0U^D;9CL|l`6*3-9idhi>k zw_9runQVXjlJOrS%TbWtr>_~6C3ZJ@d=Xt>lKOGgPLnw)oL+UEvL*h(z@* z)pyUYU*hk%ETp2;_`4cn@&@hcK5rOh9Q>G`MZcJ|C39O#aK=Q-Xx+77z4KTZdwug$ zN^?^6l5-M^ix~u7%W(wm-cZt@BJlFw>)^wjTfyqySWjQ^hS4*HDQ~k{k#pAhGN#|3 z?!0*C0diY|;X)Ch>z#y3zJ&f?v{R5J|CG#%kJ>;X;aBdhzglxr7~eBrs4v{`3@GIP za51Bu)c&1|mA~`9FVkNI6k@!skyy%_eI)JwJfFXRWxFQJb!cv{d&~HqQJh!$+SAzf=W(%Rm0OLAnVq=>g@`h1p`V}{(evIIs{69r}aziTn_KEIH1&jjd;lEwblSx++ zBM7H6CNsHik2}NI&9+@Glj$12#HHdTSJlg`=3db6cmE_X;X&g&P{i$;vYn@rY4%wF DCA8*! diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 36ebc4e8944dcd778dc8faf6668bc045a7e5de99..6110ec686c7a73603bb6b42347e41ccbfa13f33f 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~m152LLmS1hxPG literal 17 VcmZRcbt>7YGeh$`0~m0|001*d1aklY diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 84da4f7dd1f105c2a15ef7e0dafbc32bc822b41e..eb050cd8dda8d42766693dcc34161ba933a67dd3 100644 GIT binary patch delta 686 zcmZ2@iDls>mJRl;3Q_OpfB5Wo?XjQK70Je{jdBbOj2(P8o~~g41NAAB1AWytx3;!0 zv+%vr7n#i4Sr_yp-e8i�@75D_2Gq`y%dzd>|x)l9y-XhqP%DKAqC^H!hH;@MNUYvq|Z zh%=-&K@`b=LI=&*TY_2pm6Tg~FF$zoR{3DZF|dv+qLUyx6vC(L^D?S!{=3+VQNlj( zO1Z?19iM0FxAIP35aJ0jT<_21_@z<`oOfQ9YDjXe(b#*|Nt-b+6e6k54>5=R}z+a_sPFJ)2mmCSfEO|AWB7qFdZ4wQzai^nzilD@sgYy+b+)p z>$utB0nx!{GP!WJ+T_ONKUq8Y-f(D7u3u3X)GN?mU!gJA{k>j$H*3W9BVaAB{}w>C z+`_bk@$+J?`e`p#+DxCmZ8zhY2T*4LQx*ttF;3=fQk`6~vRNU$=6bh^@K=uZ=*sD- Y?Q1NduF-`kWWG09aJBGetySOa0GN>2IRF3v delta 665 zcmZ2@iDls>mJRl;3Mckxy*+2Q-#MGN@Y&Y?Ve$+Nj2#?uvYZTHpvg5k&{u79YikQL z3x}N1(aF4>bwRJ?I0AQXC}~g;czN%2@L`}*kQSq@5G}HAfV7@5kkABTMGzL?mAjB1mayW|uu6)(A}US>7-f_}gICxHnMAS%rn z!3qmcSxpa=U=*HSCdw$ZIju`oBIx0~^6Bqhi*r7)41B-#O=J~FJ^LyfAFz4`PAedd z=9Y|C{WtpN^w-_sJRI-uJvkR_gVa2Ghz{<+$#)m62#RAba1P(?Aix^ov2$ffgb~CU z+S?$Cq**YHRajoe&0v$LTays8DQtnKEUFGA;pzIkjB1}K5umD~%lnQi{`izV|o_-V$zn(Z7snW6D^~4p$gYd=NDv@ K+N`0 z2Ldy8f18kg?a&;EVAtEpYh@bwStO^l%bnJRiluH=mEFk9U#4{KNlm>gM6h%B=CcYH z8TrGmU0%Q^TM1RUa`IFq1%VwG&!-!$T_Xrp`G50TB||3usf)Ln>^|8BRoOoIs!E2y zv4&SY;geslfvUVa*;Ms}!0Gj0jT-znhC)?3Z~m(KgIVCd`82yjpI9G5M7v8jKh^xn z$)6h;RCz2U6RN*_^Ht*=i~<3wM{;Y(G&fMH;+QG=LInyTnP>3i$)Y#1|=249N z$&aRW{D~`q8ajP)s6`-u`Sx`oZC+eZG5*bSEgo|6s~tB!-(bE8D!6lUsnY|2!rH9l z%2pQLkW1w1$fiqBtj8B>MF**naAtGl;v(wO*H=&p_at?Q-!v%n(Lxx8A3DWdn;Vs^T!fX#foL? z*qtl4ukhZ;x0R1e$rPbN0_H(&g*(NcRJ!L^dfg~~{@4BIap5Wxvc9Qt+p9 z?}y2WmBPWQ=@L@@`}gcWQXy9T+0qG9*Q=I^`XxybLUB8fxj*FEvjJ0u)QP1_?!`P~ ko0OG)_c6Dr;pD<9Vc`c7Hp|owJ=mb-$8UcmU zDFTz8C0~;jCcv}pCh!BZ^(yfRv;H)%0kg$6fB~~zI8gzU#yLo{COX&&lNLnRvjs)+ Q0s<}>u_5>Yvq4BULF70rk^lez diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index e3c07c702811fb0ba07baa4acf014b8478b4e7d4..f09f537708ce6baac6dceef14969c6d9f60640ae 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~qkM0{}GC1cLwo literal 17 VcmZSnIL9>cl}9%Z0~qia0RS|A1TFvo diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 7190b3d5028aa03876cb32ff5d37509c5e8efcbe..9aaed49a56f546cb6eebeb6d5826ff7823dcba5c 100644 GIT binary patch literal 8 PcmZQzVC>*KYMu-L1`YxD literal 8 PcmZQzVC-ODv+)oB2KoZ9 diff --git a/simgui-ds.json b/simgui-ds.json index f5fffb7..14bdc93 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -22,7 +27,7 @@ "incKey": 265 } ], - "axisCount": 5, + "axisCount": 0, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/src/main/java/frc/robot/commands/AutonLoader.java b/src/main/java/frc/robot/commands/AutonLoader.java index dd30e31..1db3dcc 100644 --- a/src/main/java/frc/robot/commands/AutonLoader.java +++ b/src/main/java/frc/robot/commands/AutonLoader.java @@ -6,13 +6,10 @@ import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.auto.SwerveAutoBuilder; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import frc.robot.Constants; import frc.robot.subsystems.DriveBase; @@ -44,10 +41,6 @@ public AutonLoader(DriveBase driveBase) { eventMap, m_driveBase); - // for (String path : Constants.Auton.paths) { - // chooser.addOption(path, getAutonFromPath(path)); - // } - // chooser.addOption("forward", m_autoncommands.MOVE_FORWARD); chooser.addOption("trajectory", autoBuilder.fullAuto(pathGroup)); SmartDashboard.putData(chooser); diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index a8719c7..f62a8e5 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.subsystems.Module; /** An example command that uses an example subsystem. */ public class TeleopDrive extends CommandBase { @@ -27,8 +26,6 @@ public class TeleopDrive extends CommandBase { private final DriveBase m_driveBase; - private int cnt = 0; - double prev_omega = 0; double prev_xVel = 0; double prev_yVel = 0; @@ -53,34 +50,26 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // if (counter++ <= 60) - // return; - double xVel = ((RobotContainer.getLeftJoyX()) * 0.45) + (prev_xVel * 0.55); //* Constants.Swerve.maxVelocity; - double yVel = ((RobotContainer.getLeftJoyY()) * 0.45) + (prev_yVel * 0.55); //* Constants.Swerve.maxVelocity; - double omega = ((RobotContainer.getRightJoyX()) * 0.45) + (prev_omega * 0.55); //* Constants.Swerve.maxAngularVelocity; - - xVel *= 0.2; - yVel *= 0.2; - omega *= 0.2; + double xVel = ((RobotContainer.getLeftJoyX()) * 0.45 * Constants.Swerve.maxVelocity) + (prev_xVel * 0.55); //* Constants.Swerve.maxVelocity; + double yVel = ((RobotContainer.getLeftJoyY()) * 0.45 * Constants.Swerve.maxVelocity) + (prev_yVel * 0.55); //* Constants.Swerve.maxVelocity; + double omega = ((RobotContainer.getRightJoyX()) * 0.45 * Constants.Swerve.maxAngularVelocity) + (prev_omega * 0.55); //* Constants.Swerve.maxAngularVelocity; - // prev_xVel = xVel; - // prev_yVel = yVel; - // prev_omega = omega; + prev_xVel = xVel; + prev_yVel = yVel; + prev_omega = omega; SmartDashboard.putNumber("X-Vel Input", xVel); SmartDashboard.putNumber("Y-Vel Input", yVel); SmartDashboard.putNumber("Omega Vel Input", omega); - /*if (cnt++ % 50 == 0) { - //System.out.println(m_driveBase.getCurrentPose()); - for (int i = 0; i < 4; i++) { - System.out.println("Module "+i+": "+m_driveBase.getModuleGroup()[i].angleMotor.getOutputCurrent()); - } - }*/ + ChassisSpeeds newSpeed = new ChassisSpeeds(xVel, yVel, omega); + m_driveBase.setDriveSpeed(newSpeed); - m_driveBase.setDriveSpeed(new ChassisSpeeds(xVel, yVel, omega)); + if (counter++ % 50 == 0) { + System.out.println(newSpeed); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 37bc07a..fcb3de4 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -25,16 +25,12 @@ public class Module { /** Creates a new Module. */ - private final Kinematics m_kinematics; public CANSparkMax angleMotor; private CANSparkMax driveMotor; private double oldWheelPos = 0.0; - private double currentSpeed = 0; - private double currentAngle = 0; - private final RelativeEncoder integratedDriveEncoder; private final RelativeEncoder integratedAngleEncoder; @@ -71,7 +67,7 @@ public class Module { * */ - public Module(int module, boolean inverted, Translation2d distFromCenter, Kinematics kinematics) { + public Module(int module, boolean inverted, Translation2d distFromCenter) { this.moduleNum = module; this.kP = Constants.MotorConstants.angleMotorPID[module][0]; @@ -79,7 +75,6 @@ public Module(int module, boolean inverted, Translation2d distFromCenter, Kinema this.kD = Constants.MotorConstants.angleMotorPID[module][2]; this.pid = new PIDController(kP, kI, kD); - m_kinematics = kinematics; /* Angle Motor Config */ angleMotor = new CANSparkMax(Constants.MotorConstants.angleMotorIDS[this.moduleNum], MotorType.kBrushless); @@ -100,9 +95,8 @@ public Module(int module, boolean inverted, Translation2d distFromCenter, Kinema configDriveMotor(); integratedDriveEncoder = driveMotor.getEncoder(); - // integratedDriveEncoder.setPositionConversionFactor(1/6.12); - driveMotor.getPIDController(); - + + integratedDriveEncoder.setPositionConversionFactor(1/6.12); pid.enableContinuousInput(0, Math.PI * 2); pid.setTolerance(0.0); @@ -112,28 +106,19 @@ public Module(int module, boolean inverted, Translation2d distFromCenter, Kinema System.out.printf("module %d, %f\n", moduleNum, angleOffset); this.invertDriveSpeed = (inverted)? -1 : 1; + // if(_CANCoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED) { // throw new RuntimeException("CANCoder on Module #" + Integer.valueOf(this.moduleNum).toString() + " is not green!"); // } + m_distFromCenter = distFromCenter; } public void setSpeedAndAngle(ModuleState targetState) { - double x = setAngle(targetState.getDir()); - double y = setDriveSpeed(targetState.getVel()); - /*if (++cnt % 50 == 0) { - System.out.printf("Set module %d angle to %f, speed to %f\n", this.moduleNum, x, y); - }*/ + setAngle(targetState.getDir()); + setDriveSpeed(targetState.getVel() / Constants.Swerve.maxVelocity); } - // public void resetAngleOffset() { - // System.out.printf("Reset module %d to %f\n", this.moduleNum, angleOffset); - // } - - // public void updateAngleOffset() { - // angleOffset = _CANCoder.getAbsolutePosition() * Math.PI / 180; - // } - //angle to normalize between 0 and 2PI RAD public double normalizeAngle(double angle) { double fixedAngle = angle; @@ -178,14 +163,18 @@ public double getDeltaDist() { } public double updateDeltaDist() { - double newWheelPos = integratedDriveEncoder.getPosition() * (1/6.12) * (Constants.wheelDia_m * Math.PI); + double newWheelPos = getTotalDist(); this.deltaDist = newWheelPos - oldWheelPos; oldWheelPos = newWheelPos; - double thisInverted = 1.0; - if ((this.moduleNum == 0) || (this.moduleNum == 1) || (this.moduleNum == 3)) { - thisInverted = -1.0; - } - return this.deltaDist * thisInverted / 2.08; + + return this.deltaDist * + ((this.moduleNum == 0) || (this.moduleNum == 1) || (this.moduleNum == 3) ? -1 : 1); + + //TODO: STORE THESE CONVERSIONS IN CONSTANTS + } + + public double getTotalDist() { + return integratedDriveEncoder.getPosition() * (Constants.wheelDia_m * Math.PI); } public double setAngle(double angle_in_rad) { @@ -223,6 +212,8 @@ public double setAngle(double angle_in_rad) { if (trigger) { invertMultiplierAvailable = true; trigger = false; + } else { + invertMultiplierAvailable = false; } } @@ -292,6 +283,9 @@ public double getDir() { return m_dir; } + public String toString() { + return String.format("Direction: %f, Velocity: %f", m_dir, m_vel); + } } } diff --git a/src/main/java/frc/robot/subsystems/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java index 6a7d74e..de7464f 100644 --- a/src/main/java/frc/robot/subsystems/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -29,7 +29,7 @@ public class RobotContainer { public static AHRS ahrs = new AHRS(SerialPort.Port.kMXP); - public static Kinematics kinematics = new Kinematics(ahrs, Constants.DriveTrainConstants.wheelLocations); + public static Kinematics kinematics = new Kinematics(Constants.DriveTrainConstants.wheelLocations); public static DriveBase driveBase = new DriveBase(kinematics); public static AutonLoader autonLoader = new AutonLoader(driveBase); public static TeleopDrive teleopDrive = new TeleopDrive(driveBase); From f1d71cef1f662295f286e7b0982cd7c695b30332 Mon Sep 17 00:00:00 2001 From: roboticsDev1584 Date: Fri, 12 May 2023 14:06:32 -0500 Subject: [PATCH 25/31] Odometry updates and test linear auton --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .gradle/7.5.1/checksums/md5-checksums.bin | Bin 27797 -> 27797 bytes .gradle/7.5.1/checksums/sha1-checksums.bin | Bin 36911 -> 36911 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 76065 -> 76065 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 35803 -> 61373 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../deploy/pathplanner/New Path.path | 74 ++++++++++++++++++ .../deploy/pathplanner/straight_line.path | 49 ++++++++++++ src/main/java/frc/robot/Constants.java | 16 ++-- .../java/frc/robot/commands/AutonLoader.java | 4 +- .../java/frc/robot/commands/TeleopDrive.java | 7 +- .../java/frc/robot/subsystems/DriveBase.java | 19 ++--- .../java/frc/robot/subsystems/Kinematics.java | 2 +- .../java/frc/robot/subsystems/Module.java | 16 ++-- 18 files changed, 156 insertions(+), 31 deletions(-) create mode 100644 src/main/deploy/pathplanner/deploy/pathplanner/New Path.path create mode 100644 src/main/deploy/pathplanner/straight_line.path diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 8132658ee8e2411b9d45e86fa3c83af0421affb0..1af18575c9e259a6aa89d508ee64e8e0973f55e8 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oMe1OPE%1aJTV literal 17 VcmZQ>zc8b1qOQ9v0~oL@1OPEH1V8`) diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index 3da1217fea652f4ca0f1cf0389e6a42c8619a9be..d4c8abf22ae3377bbedc7d0130290972bcdc39a6 100644 GIT binary patch delta 241 zcmbPwlX2=z#trvlS!SFz_M05-CAIl)EC&;iXY+7!ua_Q>p|3Z2KaBAa!eE*_KLISI zJNY7v(e4cv;hww`!qA!g+FNRJW8x~1S;r>(`+%fvZcc85FwOxPQj^d6h=4>7Ob!gu zWo-qro`8kth6sas7bjQyN=-fw<=veeoeVO>*mHAl@(orX%V6i`%*-~BYpnw&Pt5{J cS_e)(4rbW=nEW$KPXOqCKL#-H6$Dax05pMO!T3(0A>?d?f?J) diff --git a/.gradle/7.5.1/checksums/sha1-checksums.bin b/.gradle/7.5.1/checksums/sha1-checksums.bin index aa7be5df19b88de6849c1f6c75eaff2a28757cdb..de6b5188e5bd8ea63f5cbfef06acc4f9805c6825 100644 GIT binary patch delta 262 zcmZ3#fNA{#rVZs8EHh3U`)#hy$YKGqY#vVD>tzIF=<7``3^8PF1+z50!9q-v;~@;) z$p>@6BD#}LLqxbIGx`|u02S*nfPv+~$qVz6H=oOMWCWRdY_eh)*f5)$llgt6CV%#k z012N1azN(T8~}^P`+`NEfH_kkoQsn;LX_MEGNdMV=Sy$yDRgE9YBKhmeAZuTa(|5Y zW|s11L7<4i&dGv-Qj=>tB{uKvcmYyl9WZ%iAIJvlz{&T)44WU56??#L_nYk9{{#TO C@@b#| delta 262 zcmZ3#fNA{#rVZs8EQ>YG!!}oEWU&BQwsR)$^)do7jIK{E3^8PF1+uP9*7ODot)3hY zVO*VjFc&Oxb@FM5$kxe>K1Mu1#a9@>!1Cbag?Y)F&*eEXf=q3gtQZD1%y#l*eqX7{ zpM4}i!W}>k$Q;`$uxPw5SacqkGZn(=nYqGb>P&dC=su{!)|s zW5hSJls5|kMND!h3kFI}uI-fAytm^8NR7?2$t(LnHrPC$d>_oPT{~H^2P9%_IN7`Z F2>_LrU(5gi diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 8ee89d568cccf0c697242a1dd9cb0d96e53a112b..3703730fe24d53e09ccdbc7e15a05abca2b24fbf 100644 GIT binary patch delta 6329 zcmd5=d010d7SFv=5fDuR5{$^=4itPumSR8yhy@YB1yKPtgeXuTtZ}PV>Vg}HJmH9n z;x>+o)`ia?n_56|8Mm)fM^P4cDTuV{SUZmM^2lcN``YjMM<;*f-g~zDJNKS@-tRz; z*?}ChV@*fiwo-RvE zj|!1wWW+^{mrFB~WXT!nQPUR6(iAd3d4{u)5DKIGGt)EVNx&VC1>~Zjp?&Lb7Ocy1 z{SNg&EEWnsxh<&8X85>2;g+QY-<{Zs>=QqV?24O7G%<=`Z#>^-ZfD;4qW!*QzrCC| z!FggKzCcH`r8}ZYD;wgK4e=;%(lF0=+modU(j~G5!Z?K_MUk)|!+rF^)D$<#*J+Ep z!9b2m`#6-x2D1v(rzedBaHZHC=3GJ1aI6B^=qB?a)D)X@n(&9n!SQQI2-d-PE;uG3 zE;v>pQ?-(6h1}HI3p#@FEVx{Sto0VarW)nw#%mzT44XqxjaHkEBew=E(uLp-;xlNe zPDu#QL$M6n`eLp!?kvj2`l4oucqzE>sF2$YL$<$#MnY#TBGvNrIAsQogEk)Ka;|?u zJ_}CQYtf+O-Qj%%o(>Z&DEY=u$h*OrnRqr>T+nXXbqW?i$_1oQJEg|EVPk`K{agwz zfJz5st_;73u6?i-DjSP+TRW=$s{uY5Y_4jp=wluMS3g`(VJ?TfYx>fAbcWr5f_pYt zFTk2a(K(6XkvoD1|ww?P)O1BnP!|IrlyR(MX^mpI>-O8x5sW ztkb3uo(kjS+GwbCH`w(rl%W>VAUYFDQZN^)ZfnmDwX%ym`j`vf+|kS_DSRW@&~F4z zaqasHgUe?Db}h&XNua(7xqT(c%#bIij|!11WE13R3ngi>E(sEaJn|{sIbRxkls+JZsnm1iuTtD~W{;75ktw$RV{eC0b2fiX~>-Jfj zwTJ3f#N*V)9ITk-3AWb$hsHO5E4m2Ht!NnWjOjkDAmu*daHtN{ zJCZ~z1;j(C4-tnK5xz)S$GR+7P*Q%fW9KX-+D9T; zf6}hbgwDh(M&Z%s`Hv8nbG1C6=7+a$$@QxXH&_n$swI8v_z?OC^&`3%Tec06$7UvW zpItV`!njntx4g?@glO3o=Q4g9+D&g@h0K(A%h&~Mc#NhHzcJNMBbe0z8n*#YksooL zk!z}sm~lBjjdD+Os~9v{arc4S;a_qyAZmG$;0f4VGDCQlik7O#-GNE0RL=`2ojYH8S-g~~=j(yQJbCvt8bqlMRT z*ep(P+u_y0R=3&J?C@fN{ryN(=uefp3<=d74fR!(J&ldH*GokM^EVdwB|OM0-Dv*PdWI1jBy)oKFN-AvxEKYu|pm2lrDwfaWCtSC* zvDYhUftDT+>VpPbl@OIrOK$nLS^iX)*lus^eTrUfHz7IM!vyYe&l~)t1^agz<)(Fb zMcJ0oiwAljfa79pY0cu)rpoUo)mX+CH`NJZ`b`_F>{d5MC6MZiZ7KJHyl1=nZ<_dH z%NbVYRnxAw6|_XQF=Pplxs{7=&DNK>yH{Vdv2DNc=!?=SdiA?r(B(rl0R^i{R%XGO zpV8o+uk+8X&4S0&U#o-@Z$#G$r%7Mj8vTYP%CRb-b?yxXyLM#37ll+DZ*UIS&FgDb zX&Y9Xe~?~11HtWRkl5zF-MRCHcNzs(3w!e(E1Y)Gi%mu-M}3FXNjq#DuriUq`RBk~ zA|>bi@yb24T&oxSy&XC9uBS)jK$z!(tzyxydrg)XK0W9SaH=_|NanzL0fskm2FYYaO7?RD-jCTp8raa$moxtaKPC8k?UV6Fa-4kl+ zcVf<*_k!THC;LD5PY@eTBW>0?5N{ZqSOLVpKs;#cKw(5XQ~fZ4$&)Am@gm#<_+7}! zCUY_&oO$EY{Z!9#=RH4cTkwX)Xv;EHZ~vtUObNhVsz7fmlR8mEz%ai{kR$p4d$J%mX`NMveX`4}@0DPm?lvw%p;rUbqj@!2nhdSTP0Ih;7lAYWoQ7J$uph7Tg2koAJ;?TAPGhXMLx+bI3Foy{?P+sXH_e9DE9J6%re83D8YG2)Wz>-`zs z12MD(;%I$#fF^~Sg7BBR$$VJ84LQS|iCCZXQPDwL)I$1%M6TYPii=1kBQ6n~e;Vvf z#VD;ItuH(1o7J#EQIJ=R9O2$%twRk*F&CV7A}1&f)vl;qD=DI4Eb8{Fq-cRLY+dl~ zKyyTP;n+_-Ns|_v!f~1Ym|`jPXo)Pc?8gyM5ux=$>CjX~woZp8e>9ubuoSc5$t>)p z7E+Nt%8J=|lO`NWY9yRhU_Qu~;mOcA2cJ@l`ex!4U^x%FsmD6W+?j?nW3@%KV_qUVxx+{CSIEu3MiTcQZ~ioSL!3s&2IjMG?kiJQ?XI=KbiX* zy0q^wNG>0!jL(3j%e9P{<}4HucNx?mx)nBuODnJ)bA5kC4Xyz0Ey8>|?C3e-q9YUu zGvOV7@e#q5D%B_J(Yy<6RSWK?ge(blEe0*b-pm91D>rCW8n^?#e^Z+W&#T0@>A;=axl!UwZd+be1OG$}O&&pO#OU&?9#3$v*({fT1SIZJK1%b0_d=ixKkpEX0iSGGC zf>bD3gBLA`0D5tJKJhKP2ufUJNvxrp8za>OR@fLu;?OMx1nAZXmKx6{7)6)#nqcuf z5{(^=;4p08JN#vZl?h}qF6R?#4EtK+%H$B>_OBtEgK;BiGvunB3KxhOKZqc|ZU+wWxT! zoWi8h5Vs3En~Ci(X+>}Hl|a|@2{7-DW~WsQXO%p1nxJ41-owGWAc9~#*$q=^*Lv#_ zt7KWSGjGnt-UUB;PrWTScTP8#OtirA9`F@jf3R=1(MLnBU;UUEEjvM1!6AeI;$#?c zJf`%5km`0>DY9SG+}M03dvxv#{}9~K3oh_4wx{0bibCohP*laEEh$k)tPdRtwvE5F z{!1whpV-5XdZ>zf-~})B3_kb4XSlExeJ~L_?gJ&<)gub5m~^_Q)@|XanP(y@;SddS zTUva^f-CT>W?7uWx(6Vjmd=%V-nA}uIc_n}J;%X!fDS%mhsPhlDAKBjby|>`ur^ul z$~3wde^i+c%;*2F&yNQ6v{2h1wAM1cnFc?yv~w7yd7MA|3?r4J;Q6_ z3n(8t=EjZYr)?Kcdz~&5U)q5i0>KsfSx7XN&mjU_@fh3-W=39LC%S#=#qKGa_*lL; z?DhARuL`|)ilKC|JXxN)W>&f)OYRw;EX&9+qRhX0vetEs#WB~Wiy`h1xnxH-3w}O| zXGA$)kWBQqv>CttRo&*l-)l*SvUgLJ8JRL=W`=X{+RRj?Hb&gGAI5&D@Y~Wh$D+9L zxM|s?s?C3CDMY~&aI-Ju&pg-s)oxXFZs^B_!f&n~Wv9Hm%;5`K*5GJwL`D5>=%L_f z-PLuet=CAooox*V=rP7cK`=oyG9*#TF_LHi+!70T&raN zoQbl4yY>;Xjr>F00AvfC7jH3v+yX+Onz zmHz(o+IH`pBheY*eyw#J?$tY3IRFx~Hm*o|EUxe*q6B#HrA0y6<0FBH^$pP3bAT&n zgAKF6)t^3F*Zy<&w{c<3-m$)}t&Ax=_;!ZRVD|@VMO#wD0rpI#)&mHu!*zHe7!mq<8-vJ}uA>#;h z_cPa76zjxU%&?^g_;ws?Jj$>yXz!0~OHY~LuRUOj;s9duU;LAMf$5o*x1d_2r!)AdR&LuF2j3L(y zT*Pm64qllHqv{-u;q_17KZe;+ct#wkQV)1)gff6 z9%K?G%?CUE9i7ZR0T$ZBVA!S1#XgB#(FL6BJYS)dr^qrD@fqrSjz<~xE>y4=JNq11 z($rZK{au}m^Jf!#pnh4K-&cmRZ?Q!k;Tevk7g!yc!H66_d9Q&a9V-_SX( z*XMjPZkLt!a*laRE(G#GSEiZ*z!IJ zQP<9@$0>_Roo>Wv$^F_9CyN%+_19f6W~nywIgsOvrNmdOjPg*mjQG7Nk5=QRC=$Sh zK~_JiVxq|o9y>0@+*neto{9O8l(1j3&@+yFuKxPAfXuK zG87pZx*Ka&s(I3BX-*relu6_zw=dj(kN=5A&4UNr*<#h`be>9_Pr8hq`cF=+dLAhr zP{i!@QckV<^bx0CO51qGU}Q9F8Ac9*2!qxUJJG}V76;?bnpP{nh{(|RxU8;|ue*o5 N_8e4wkVC>R{s)WtW=sG8 diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 6110ec686c7a73603bb6b42347e41ccbfa13f33f..1a7cfa9b32e28167cb5cc46602bc7954d7e576cd 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~m0n0su2q1bqMi literal 17 VcmZRcbt>7YGeh$`0~m152LLmS1hxPG diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index eb050cd8dda8d42766693dcc34161ba933a67dd3..f7e4fc565b7f5227c8e3f65f4be64bfd16f34a12 100644 GIT binary patch delta 1249 zcmZ2@iDls>mJO|~EHh3U`%eC!smt0r_N$a%1-iZA_vl0YE*ee%ar;mLAqgs1-$ zW(=rzojIZT^}_d8Yb943pPM7M8?2hKa3@5y>;fQ-W`5A+glmUxe-5*qn5G>Q$2AM= zqSB3e5FH9pm^%K49x7%zu;uyPxQpGq#rJB!IwpMO2C8HQVzr&q^?4c9rhgUz2GHLX zB0Rt#RBNvz=IHRGm77+7T{(~Y< zyiaOcw@eFzt>1G&lOnqdP&L-m<5d}@Bo-L#ezD-X<14XiM{?dMa@s&;&8ANT$|{&< zygT8PJay?j_P(OGmND{BSrdr)OzP7=sxu1L7v|r|-6xhg_udhyB>rGKs3LQyBi>+o zN@~K(g`)jlS03zVC_Tsh@fTEw6;ww%rVc&FeKWrFEo?n0C;#T+Q=K(X9YE(YFt9K% zFf&cw*rCQEkijH6`EI$?`eA*3T5$4*>&8DAta~M(dPO0& z3B_XCX6?Ic;|X2Yg+hE1yX6B8PlDpZO##&_}Zh-prH5IMo0=*l2QVh$G{--aPr25s;nJ?N>Z+qZ!eUZ ztiMvk4#@F@NOLg)X*3H<{S1sEE}nW+Xud;t(VFpYf;Xun(GhHYDZ<3n+7g%%%g1GJw!x*N3 GtOo$njBVWj delta 1167 zcmZWnZA?>F817vPg9zAid$qe_TUTHuwIDRQY=ihMlK>X9xvQO9sh)#FbJo-6n%xgRoRsXx(ql{@rx)$&KF zydtp4;=xHg7HMI+omRS)t=C;mv9nnN_lo_y+Xc?x{%3?kXTbE&G$=Y~^oFJkT*U0F zab3>Py|=^bV%(?IV?28T+`ka^XAT@c@^Cw7?a!HNoYHbj{@+ujs1p?Dt$!26GJ2un zHRrUJ-&}s2YgCTQtZDPSV*ZJJGiFBMnUs#wVyR>_Bcl{5Tb*9>Z(eiML~XsViSxPy z_lK$jqn-NjIU#YI@=eX??RdXYtWi4-j?XVHo{iY=@i_8c(l1CuAzF~7O@ zF%N}?J|>+Mkpyx4Xj8V{%+glXf6JJbwR4q^0{B3&MIZAOQAE~)-I>I9U<_zJkz(b> zf#zJ%(J$^gJR8Q|N85a1em=}ObpmfYG9_W2dPl#rriDHG^33OFpR)soFfRwy;4J7) z3AULnU@Bt46kg?`uEjXH{t_L_cPu0x#1^PA)6h{wi}6n*TsegsJ)YWr4JMAT;4GH$ zzf0ivVwSt~@}jD5i~Kd$rJcEZ$0Q+5Wi_PG;sXn}pE(ox{mx2zcK-F2AN}RFqOf9` zD3;t_s5t4*uL!k YZDkpRPO#{mhN7kX@*pWDI)`KG2S$jOSpWb4 diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 4614835c7f83751075723e1abcecf1ee6e7838a4..b55e4a8d95609baf18eb8382019edab318737f55 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~pBq0{|>t1LObz literal 17 VcmZR6A~{9FsWEOA0~ko_001mD1HS+O diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index b15a6d31ab1b092ed8954fd5d8214640b5e37248..772d186fb049dff905f15eace2f925a5a7a14145 100644 GIT binary patch delta 1636 zcmcaTooVlV<_#(mf<4Jh3_!qN&cI;6!@$7Ezu8n`BMX0ofpz%08J|H?3=D4{PQELn z$UjT!$B{)plAvPdo4?8kFbd3FYT@`=sOCjR{_U+L>9hRTK?Q{;%W7u`JX?|w zapU@hc@V+3k&|n+1Nqf`oJxLZ{(^|TJ3sj-P~n>&a$DNj^q@ibK76yP&H*NYAAz1J zarvA6L$!-;zN@Fn$PM)d1A}Dc=81-_Z2ZzoQnzX=Plc%J{=1piLV#T$FzZlo_T!K? zsOas@qV|tCm<$9rJ9^A=t~Uk>O9HVV2n(F9J^A$Z^RQk&gVUN1R`fan1?5mx^jPoj zeSEAmz5d7lcWPB(pnyhGvGaUhnWV170-dcvme-aPf}#jb#rpS}Gmf1uYFqI9gjvP+ zBj3Q*KvNO}8f_`pxIfvW5vv0{nA!t`>=%rw9>!$Go}J_yVJV*g4%fGiS6cg{tk{ zY-<|9C?NkhWZ8lS7HHCZr@FbzWts(9 zttf#Cc28bv6CmLB+(y67Wc4eE;QQH|-`Y4b@_T6{K4kg~3SJ<1b8)h&-4AS$wUU*; zJjE`j?8I7#%D2^17J-M8R_=;4pDr)syvpT%o%Kne7PP=$`sG$8 z(=IN{*RG$>o)~k0LRRstnwBOzmN> zwl2Gugr;&&@Z^K_!iFv%{%cJ9_cnBM{B*8_sShx9RorpiB=a}I;KBEEQCqiN+B=!C zSh(KHR_!)dr(p21X3fp7C7)yJdb@4?$6JBrXU@-I(0rH|>W8V~Qtgt?{ugUnEnm24 zYP@L$pA~l#oB`w)C*P*m`fSomTR_N0S{pfWhKnttni0mqD24 z^nuz#t(dy5tIUf&rM&#j_lRkq-zav9O->9K4zA}d+Q{h4uktBK!gR_>ElgcC4HJ(_ z-+dl1^_QoG+RmIsm?}c@=N^rJvNTBHiHtF)%KDzkg;m1g({5=d^XP2I-Pt$qX`uK! sOkLrj72A(6oR&&ouw(Pvd6C;ORqQRl#~{hGF>oShO0QXBdo)M|0JxI=9RL6T delta 165 zcmdmcpZWH5rVT0*f)YAR3_!qN&cI;6!@$7!ceAO)MwZF{WF$80$+9zU_LXa3-27C2 zA=Bh}O75FQl>-^ M?;F0&{vJKf01`Ys)c^nh diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index f09f537708ce6baac6dceef14969c6d9f60640ae..6a9b6b0a5379c8ec2a749f2f1c8c1adf3326cccb 100644 GIT binary patch literal 17 VcmZSnIL9>cl}9%Z0~qiY001<<1Z@BS literal 17 VcmZSnIL9>cl}9%Z0~qkM0{}GC1cLwo diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 9aaed49a56f546cb6eebeb6d5826ff7823dcba5c..a8e1e492b32d14836b8feb0a198d1950ba9db7db 100644 GIT binary patch literal 8 PcmZQzVC)b`|K|n(25kaG literal 8 PcmZQzVC>*KYMu-L1`YxD diff --git a/src/main/deploy/pathplanner/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/deploy/pathplanner/New Path.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/straight_line.path b/src/main/deploy/pathplanner/straight_line.path new file mode 100644 index 0000000..ce982c1 --- /dev/null +++ b/src/main/deploy/pathplanner/straight_line.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 1.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.0, + "y": 1.486914226526235 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.0, + "y": 2.0 + }, + "prevControl": { + "x": 0.9999999999999999, + "y": 1.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 12be1cc..2d9e3af 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,11 +25,10 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; - public static final boolean fieldCentric = true; + public static final boolean fieldCentric = false; public static final double joystickDeadband = 0.2; } - public static final double wheelDia_m = 0.097; public static class MotorConstants { // 0 indexing @@ -48,8 +47,14 @@ public static class MotorConstants { public static class Swerve { public static final IdleMode angleNeutralMode = IdleMode.kBrake; public static final IdleMode driveNeutralMode = IdleMode.kBrake; - public static final double maxVelocity = 1; //3.8; // m/s - public static final double maxAngularVelocity = 1; //56.0; // rad/s + public static final double maxVelocity = 4; // m/s + public static final double maxAngularVelocity = 56.0; // rad/s + + public static final double maxSetVelocity = 1.5; + public static final double maxSetAngularVelocity = 2.0; + + public static final double maxAutonAcceleration = 0.5; + //velocity PID tuning for overall swerve public static final double velocitykP = 1.0; // 0.0001 public static final double velocitykI = 0.0; @@ -58,10 +63,11 @@ public static class Swerve { public static final double aVelocitykI = 0.0; public static final double aVelocitykD = 0.0; - public static final PathConstraints AUTON_CONSTRAINTS = new PathConstraints(maxVelocity, 2); // max velocity and acceleration during auton + public static final PathConstraints AUTON_CONSTRAINTS = new PathConstraints(maxSetVelocity, maxAutonAcceleration); // max velocity and acceleration during auton } public static class DriveTrainConstants { + public static final double wheelDia_m = 0.097; public static final double DRIVETRAIN_WIDTH = 0.635; // in meters public static final double TRACK_WIDTH = 0.470; // in meters public static final double WHEEL_BASE = 0.470; // in meters diff --git a/src/main/java/frc/robot/commands/AutonLoader.java b/src/main/java/frc/robot/commands/AutonLoader.java index 1db3dcc..f280015 100644 --- a/src/main/java/frc/robot/commands/AutonLoader.java +++ b/src/main/java/frc/robot/commands/AutonLoader.java @@ -20,6 +20,7 @@ public class AutonLoader { // private final AutonCommands m_autoncommands; private static SendableChooser chooser; private static List pathGroup = PathPlanner.loadPathGroup("trajectory", Constants.Swerve.AUTON_CONSTRAINTS); + private static List pathGroup2 = PathPlanner.loadPathGroup("straight_line", Constants.Swerve.AUTON_CONSTRAINTS); public AutonLoader(DriveBase driveBase) { @@ -30,7 +31,7 @@ public AutonLoader(DriveBase driveBase) { eventMap = new HashMap<>(); eventMap.put("event1", new PrintCommand("event 1 passed")); - eventMap.put("event2", new PrintCommand("event 2 passed")); + eventMap.put("event2", new PrintCommand("event 2 passed")); autoBuilder = new SwerveAutoBuilder( m_driveBase::getCurrentPose, @@ -42,6 +43,7 @@ public AutonLoader(DriveBase driveBase) { m_driveBase); chooser.addOption("trajectory", autoBuilder.fullAuto(pathGroup)); + chooser.addOption("straight line", autoBuilder.fullAuto(pathGroup2)); SmartDashboard.putData(chooser); } diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index f62a8e5..1725f26 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -51,10 +51,9 @@ public void initialize() { @Override public void execute() { - double xVel = ((RobotContainer.getLeftJoyX()) * 0.45 * Constants.Swerve.maxVelocity) + (prev_xVel * 0.55); //* Constants.Swerve.maxVelocity; - double yVel = ((RobotContainer.getLeftJoyY()) * 0.45 * Constants.Swerve.maxVelocity) + (prev_yVel * 0.55); //* Constants.Swerve.maxVelocity; - double omega = ((RobotContainer.getRightJoyX()) * 0.45 * Constants.Swerve.maxAngularVelocity) + (prev_omega * 0.55); //* Constants.Swerve.maxAngularVelocity; - + double xVel = ((RobotContainer.getLeftJoyX()) * 0.45 * Constants.Swerve.maxSetVelocity) + (prev_xVel * 0.55); //* Constants.Swerve.maxVelocity; + double yVel = ((RobotContainer.getLeftJoyY()) * 0.45 * Constants.Swerve.maxSetVelocity) + (prev_yVel * 0.55); //* Constants.Swerve.maxVelocity; + double omega = ((RobotContainer.getRightJoyX()) * 0.45 * Constants.Swerve.maxSetAngularVelocity) + (prev_omega * 0.55); //* Constants.Swerve.maxAngularVelocity; prev_xVel = xVel; prev_yVel = yVel; diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 05ef9ef..8da3864 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -143,15 +143,15 @@ private void updateOdom() { SmartDashboard.putNumber("X", m_pose.getX()); SmartDashboard.putNumber("Y", m_pose.getY()); + SmartDashboard.putNumber("tX", simPose.getX()); + SmartDashboard.putNumber("tY", simPose.getY()); - if (cnt++ % 50 == 0) { - System.out.println(simPose); - - + /*if (cnt++ % 50 == 0) { + System.out.println(m_pose); for (int i = 0; i < 4; i++) - System.out.printf("m: %d, %s\n", i, targetModuleStates[i]); + System.out.printf("m: %d, %f, %f\n", i, moduleGroup[i].getDeltaDist(), moduleGroup[i].getAngleInRadians()); System.out.println(); - } + }*/ } public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { @@ -161,9 +161,10 @@ public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { double angle = Math.atan2(yVel, xVel); double vel = Math.sqrt((xVel * xVel) + (yVel * yVel)); - // TODO: Ensure that angles from AHRS are along the same coordinate plan as odom - //angle -= simPose.getRotation().getRadians(); // for simulation - angle -= RobotContainer.ahrs.getRotation2d().getRadians(); + if (Constants.OperatorConstants.fieldCentric) { + angle -= RobotContainer.ahrs.getRotation2d().getRadians(); + //angle -= simPose.getRotation().getRadians(); // modify for when not field centric + } chassisSpeeds = new ChassisSpeeds(vel * Math.cos(angle), vel * Math.sin(angle), chassisSpeeds.omegaRadiansPerSecond); targetModuleStates = m_kinematics.getComputedModuleStates(chassisSpeeds); diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index c94fc58..f77f35c 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -72,7 +72,7 @@ public Twist2d toTwistTest(Module.ModuleState[] moduleStates) { // completely fo SimpleMatrix xyVels = new SimpleMatrix(8, 1); for (int i = 0; i < 4; i++) { - double vel = moduleStates[i].getVel() * 0.02 * Constants.Swerve.maxVelocity; + double vel = moduleStates[i].getVel() * 0.02 * Constants.Swerve.maxSetVelocity; double dir = moduleStates[i].getDir(); xyVels.set((i * 2), 0, vel * Math.cos(dir)); diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index fcb3de4..51441ac 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -96,7 +96,7 @@ public Module(int module, boolean inverted, Translation2d distFromCenter) { integratedDriveEncoder = driveMotor.getEncoder(); - integratedDriveEncoder.setPositionConversionFactor(1/6.12); + // integratedDriveEncoder.setPositionConversionFactor(1/6.12); pid.enableContinuousInput(0, Math.PI * 2); pid.setTolerance(0.0); @@ -155,11 +155,7 @@ public double getAngularVelocity() { //may need to multiply by the gear ratio to } public double getDeltaDist() { - double thisInverted = 1.0; - if ((this.moduleNum == 0) || (this.moduleNum == 1) || (this.moduleNum == 3)) { - thisInverted = -1.0; - } - return this.deltaDist * thisInverted; + return Math.abs(this.deltaDist); } public double updateDeltaDist() { @@ -167,14 +163,12 @@ public double updateDeltaDist() { this.deltaDist = newWheelPos - oldWheelPos; oldWheelPos = newWheelPos; - return this.deltaDist * - ((this.moduleNum == 0) || (this.moduleNum == 1) || (this.moduleNum == 3) ? -1 : 1); - - //TODO: STORE THESE CONVERSIONS IN CONSTANTS + return Math.abs(this.deltaDist); + // TODO: Make deltas negative with 180 } public double getTotalDist() { - return integratedDriveEncoder.getPosition() * (Constants.wheelDia_m * Math.PI); + return integratedDriveEncoder.getPosition() / 6.12 * (Math.PI * Constants.DriveTrainConstants.wheelDia_m); } public double setAngle(double angle_in_rad) { From ebffea640e11c36060587c66d3d189db537552e3 Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Mon, 15 May 2023 21:42:58 -0500 Subject: [PATCH 26/31] made prototype for 180 degree turn optimization --- .../java/frc/robot/subsystems/DriveBase.java | 3 ++ .../java/frc/robot/subsystems/Kinematics.java | 13 +++++ .../java/frc/robot/subsystems/Module.java | 54 +++---------------- 3 files changed, 24 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 8da3864..88017bf 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -168,6 +168,9 @@ public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { chassisSpeeds = new ChassisSpeeds(vel * Math.cos(angle), vel * Math.sin(angle), chassisSpeeds.omegaRadiansPerSecond); targetModuleStates = m_kinematics.getComputedModuleStates(chassisSpeeds); + for (int i = 0; i < 4; i++) + targetModuleStates[i] = Kinematics.optimize(moduleGroup[i].getCurrentState(), targetModuleStates[i]); + } public void resetDrive() { diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index f77f35c..39703ee 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -11,6 +11,9 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; public class Kinematics { /** Creates a new Compute. */ @@ -98,7 +101,17 @@ public Twist2d toTwistTest(Module.ModuleState[] moduleStates) { // completely fo return new Twist2d(deltaX, deltaY, deltaOmega); } + public static Module.ModuleState optimize(Module.ModuleState cur, Module.ModuleState set) { + Module.ModuleState ret; + if (Math.abs(cur.getDir() - set.getDir()) > Math.PI/2) { + ret = new Module.ModuleState(set.getDir() - Math.PI, -set.getVel()); + } else { + ret = set; + } + + return ret; + } public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeeds) { Module.ModuleState[] moduleStates = new Module.ModuleState[4]; diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 51441ac..7d23b6c 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -44,7 +44,7 @@ public class Module { private int invertDriveSpeed = 1; // global, always applied to drive speed if motor inverted private int invertMultiplier = 1; // inverts drive speed if 180 degree code tripped - private boolean invertMultiplierAvailable = true; + private Module.ModuleState currentState; private boolean trigger = true; private double angleOffset = 0; @@ -111,10 +111,12 @@ public Module(int module, boolean inverted, Translation2d distFromCenter) { // throw new RuntimeException("CANCoder on Module #" + Integer.valueOf(this.moduleNum).toString() + " is not green!"); // } + currentState = new Module.ModuleState(0, 0); m_distFromCenter = distFromCenter; } public void setSpeedAndAngle(ModuleState targetState) { + currentState = targetState; setAngle(targetState.getDir()); setDriveSpeed(targetState.getVel() / Constants.Swerve.maxVelocity); } @@ -172,45 +174,10 @@ public double getTotalDist() { } public double setAngle(double angle_in_rad) { - //code to make the angle motor turn the least amount possible and drive direction if necessary - double targetAngle = angle_in_rad; - // double currentAngle = getAngleInRadians(); - // double normalDifference = currentAngle - targetAngle; - // double difference180 = currentAngle - normalizeAngle(targetAngle - Math.PI); - - // //if going to targetAngle + 180 degrees is not less than the distance of going just to targetAngle - // //then turn normally and also do not invert the motor direction - - // //what it does not do: does not go right or go forwards - // if (Math.abs(normalDifference) > Math.abs(difference180)) { - // targetAngle = normalizeAngle(targetAngle - Math.PI); - // // System.out.println("module "+moduleNum+" target: "+targetAngle); - // if (invertMultiplierAvailable) { - // invertMultiplier *= -1; - // invertMultiplierAvailable = false; - // } - // } - - // System.out.printf("target %f, actual %f\n", targetAngle, this.getAngleInRadians()); + double targetAngle = angle_in_rad; pid.setSetpoint(targetAngle); // angles are in TRUE BEARING ( angles are negated ) - String name = "Mod" + String.valueOf(this.moduleNum); - SmartDashboard.putNumber(name, this.getAngleInRadians()); - - if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180))) { - this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -1.0, 1.0)); - trigger = true; - } else { - this.angleMotor.set(0.0); - if (trigger) { - invertMultiplierAvailable = true; - trigger = false; - } else { - invertMultiplierAvailable = false; - } - } - return angle_in_rad; } @@ -243,16 +210,7 @@ private void configAngleMotor() { private void configDriveMotor() { driveMotor.restoreFactoryDefaults(); - // CANSparkMaxUtil.setCANSparkMaxBusUsage(driveMotor, Usage.kVelocityOnly); - // driveMotor.setSmartCurrentLimit(Constants.Swerve.driveContinuousCurrentLimit); - // driveMotor.setInverted(Constants.Swerve.driveInvert); driveMotor.setIdleMode(Constants.Swerve.driveNeutralMode); - // driveEncoder.setVelocityConversionFactor(Constants.Swerve.driveConversionVelocityFactor); - // driveController.setP(Constants.Swerve.angleKP); - // driveController.setI(Constants.Swerve.angleKI); - // driveController.setD(Constants.Swerve.angleKD); - // driveController.setFF(Constants.Swerve.angleKFF); - // driveMotor.enableVoltageCompensation(Constants.Swerve.voltageComp); driveMotor.burnFlash(); } @@ -260,6 +218,10 @@ public Translation2d getDistanceFromCenter() { return m_distFromCenter; } + public Module.ModuleState getCurrentState() { + return currentState; + } + public static class ModuleState { private final double m_vel; private final double m_dir; From 2f439dbd9ce191ce08ef4d353b53df48a64fde31 Mon Sep 17 00:00:00 2001 From: marvalarva2929 Date: Mon, 15 May 2023 16:45:53 -0500 Subject: [PATCH 27/31] Changed wheelDeltas to be positive or negative --- src/main/java/frc/robot/subsystems/Module.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 7d23b6c..3db3e3e 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -157,7 +157,7 @@ public double getAngularVelocity() { //may need to multiply by the gear ratio to } public double getDeltaDist() { - return Math.abs(this.deltaDist); + return this.deltaDist; } public double updateDeltaDist() { @@ -165,7 +165,7 @@ public double updateDeltaDist() { this.deltaDist = newWheelPos - oldWheelPos; oldWheelPos = newWheelPos; - return Math.abs(this.deltaDist); + return this.deltaDist; // TODO: Make deltas negative with 180 } From 9d6a5e996eb7803454fac09a5c1d99c99507a946 Mon Sep 17 00:00:00 2001 From: roboticsDev1584 Date: Tue, 16 May 2023 11:04:50 -0500 Subject: [PATCH 28/31] Fixed 180 code and delta dist --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 76065 -> 76065 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 61373 -> 61747 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes .../java/frc/robot/subsystems/DriveBase.java | 10 +++++----- .../java/frc/robot/subsystems/Kinematics.java | 4 +++- .../java/frc/robot/subsystems/Module.java | 11 +++++++++-- 11 files changed, 17 insertions(+), 8 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 1af18575c9e259a6aa89d508ee64e8e0973f55e8..ff61bd30eaa02ec869317e784436e16d8b3c78bc 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oMe1pqN+1atrZ literal 17 VcmZQ>zc8b1qOQ9v0~oMe1OPE%1aJTV diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index 3703730fe24d53e09ccdbc7e15a05abca2b24fbf..a36dfb797ead5ee5b23ec2b21fddd2c6bb5320d4 100644 GIT binary patch delta 977 zcmZ49%e%ancZ0u&Qej19W@ce!VQE%MK|xMxL0Lg!o@sWKNqQy&1Tbzc^pIv`J#eUV zWz*(~o?qppaBJDTAVZ3g^$Zd3Q0M(-fNDnP>Rq zuILH(8H_2LK3+P5Vpw8q--{FivCrG85?L=?h*kYOv0n1!A6k!D!Fgc<0cHrs;w&8Le0wz|4S`jHVpT zAg191mg)Ysj7rnjzho5P_;VX1^oDD)K)dSnH$V~ApCFOxjIS7Vx&A>UIT;sB-{`<3 zKRx^vqY!Ho*u1<~jA|UMVDp;Trhl|$lw<7xOYDEesKh!C?2>0dri6wdhaE;B@7{ffoVP+86`MQ#8 z-@%<0d7HPeG_E{x2q>hmK2Eufd6xU%_GfzOd?zjeg)T-dn(FKVZQaZ_++^b&Fyt>822;EajdOcIx~%b_fzi|mYW{0={O3uZb~f+ zBLjm4*LH&sj6#gtbKf&Yxe`on%+i|!H+*KCe$SkVcl%2-CPrQX^uPrsDYfYeE=P43wlm$&Gumz){xew_KRIh0w#7aUC?>u}u$o%_uZ|i7Qh*M&M5VpCUfFA(ef5 zpc_-I6o%iYPfTQT-5z&_v72rCx^$*x{1Q^rBbXjFN*OHsQSjp2r@IE97J%Y-!<6kg I?p#-iz%flMsuNRfg&0SeMg^^V^-y+GanjpKQrVW}N8e%By|l z94k&7Qr3B=oE^%qlH(8VB1e+i^83)`x@<3h<(%(d&-Zzr&wcLm+*4!Y)!2A-=_wI8 zszQx1J2NvrGjpd-ouf~XXe4>&Eru*5>(6uMrAz?44yU z)#}#MZathxjmTCRR68|ViO#IdH5;_s6XFW;a^sa-^9z43fGB#h4$9axuMyhC-<>%)sjy_^=li;nx^AwhtEIE(WgXg9J{KAI^CdfYK8o0$uI{f^(Ij`neQw zU>fT8F?a*x2MEB{7I8c2OHVLis0Ess;dbS55IoBC|Zy@@d2qL^?a_B z&iV)&<^Qc4yVbd}?kk{|QjACU&BcI6j11RTsYn#Q6tj{egN3vHO8M&+LJ=)E>R5`E zk|Xnl+CRMuM610(qBC-=_^)uOVKtxHqY0u{CSygyzivn6N=!~?v_WYgU8}{;juBu{ d(&_i3P^H9#hWLe-gl*UTEvzl0jRlzO(JwizlVkt@ diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 1a7cfa9b32e28167cb5cc46602bc7954d7e576cd..ea16f85fa5f39f29cd90153e40feda5895a8353f 100644 GIT binary patch literal 17 VcmZRcbt>7YGeh$`0~l~E0{}CX1ik7YGeh$`0~m0n0su2q1bqMi diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index f7e4fc565b7f5227c8e3f65f4be64bfd16f34a12..617c76b8f247aa263c7ffe1ddf6e704ccd0e00e7 100644 GIT binary patch delta 626 zcmZ2@iDls>mJNrSS!SFzdb0R<^8^-^Z^G-RpB84Us~4_1WVl;+<*ggPvhQwJ7iC~z zVC+zpwU7rgfIuz`Naz^@2~8kY1Yv>dn@u6#63WdoUWqKOx-#oDSci=B6`&*#DD43f zXgUlHQ{Qu5j#zRqC{|ZA@9$o)4khzoph{LCmOnRLpO;Z>`)3ix1^hg!N_WSiXNz0p$j8iE{-{AzE$ z2>!3>5&3t-0OE3=zYwjW|A91`UrX*NX#V^X`daDc1kD)3M^z9Vz9JAE{68^ugv@{S zrez1uWLKVvP4*#1-=R9#fhs2ptrAxdmOK$4z?K{8=szRg&3C5&RGtT7qV(s<8yBjw zb^zV4IQjNMsmb~)Ma+QiSA|G(F=ATCv9@aI%rySpPrYAQZhE|?18Sh_bZHGn9|fuD z5loL7r3{w+D0p%1(_N^S1B$@r6`nGf-mbwI0Suu;hspn&q^AGXU=$Gm@|+;{F-!s3 F2LLUes1yJI delta 626 zcmZ2@iDls>mJNrSSr%)W{aJjxc>;@|;!FSOr-d2o>Ro3}Xnwu${nc8@)yC)M$n9oe zVC)cNEZoTe2C@r)l%6q=&;(*d5Ecl!oN(>X?ayJh6VtR~;<#pkbO@Ag)B{QafkG6J zK-2L*^iVO&fi2JP#$D{@ExuO+)-mBLH&7)j5UcH+uFuP;w*9jR;{tvjK_w{xpdb(k zbWLV#5uUzDg;AIVC~7eMqza=Rh-EqbHDT~`$%W<5O~$}*ci5y~=|ei+I!pZ*of zvYM{0#;6Cf#Rg&vGt=aa9cnBB8BC&+@0LqV{@NkJA}HWlHhFKfRDIT_AJ*rm1t)*F zZv2D6x>o`mI!aQa5UoP7KpM@j*1o$op3rq&D8whRTR!0MB&ZHh@BsmzJ&-`tA@@k( z;IV19ZcCk;#v$~qOdG009;kA%&?<3-)ts))IpPyvd-NF;^d8#?i6JE^C6IXx3^ETV zZ(OL#+5z;C>*U)Dr6%jI6fpyGJR#Ctj6fRA!csp2qlk;A9u=DJ&|P$BRyx$eis{lC zj6MoXTIT8}wWiNBDxLmUymGA+L{`oRY+m6Bz3J^5j1eq?s+q2n|2IiZ|Es|$A^_yM LL+oRi0t1LObz diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 772d186fb049dff905f15eace2f925a5a7a14145..ffef9c9147395f14ff9e9f32563d393683804caa 100644 GIT binary patch delta 600 zcmdmcpLz2m<_#tijKZ5uB{CQV+NW>WqOpEo4+9vy-@AFLWTp_m#Nr+|!Tr7v!4Hg^ zziMYN3w&|CzC^|mru4(v&7t})8TluMMjFkn-wV}$f3m4zG2_k6x%+w;H%~Qw-N7g@ zxp9vq|FLU__f)B+K~#Md+&pp5O(uTkC5wJpOg;*=MR{}L{u@jJQr{nT$mS-RKt*h}drS#s3P(jzt zJ5LF+^UGe7u(->}0Tt}reDLBTW+usr8x=risv0SoANnn>zc<-b(e&HBQ zUF+PxK4MzCbIFATnewMDOzxk&Fj|=Z`_m7+KFc(Z`mx{PNjvj(^2BW6`gI3yc^{qn zUm!5A?nqvRKBh^0EN9<|)mkr?j=uK9>f+X`m@3YSJ=EI2r6u#0{lkk^iqBqRs#s8@ z|NGUB(1pnx{!9+~QSX3IApr}B`JH)&PwtAIaG$}LvgzZcbCWlE3k%zv7cJr_am|U~ M4r%P2Z~&wY0OGsMbN~PV delta 117 zcmV-*0E+*!;sd?k1F$q00S~h^7-#{rwHaX&vo0)O1GA+s;Q^DLF=_#Mvn9S=0h5}) zkF%9C>xcmflYzV$v$DL<0<)UG&jPchH3$K-iNk;dv;8{Z0kZcl}9%Z0~qkW0RS~S1nB?( literal 17 VcmZSnIL9>cl}9%Z0~qiY001<<1Z@BS diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index a8e1e492b32d14836b8feb0a198d1950ba9db7db..003323daa44da71585dc0f17781856f44b8be1e0 100644 GIT binary patch literal 8 PcmZQzVC+z3O1=sJ1nU9m literal 8 PcmZQzVC)b`|K|n(25kaG diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 88017bf..a6d3b15 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -146,12 +146,12 @@ private void updateOdom() { SmartDashboard.putNumber("tX", simPose.getX()); SmartDashboard.putNumber("tY", simPose.getY()); - /*if (cnt++ % 50 == 0) { + if (cnt++ % 50 == 0) { System.out.println(m_pose); for (int i = 0; i < 4; i++) System.out.printf("m: %d, %f, %f\n", i, moduleGroup[i].getDeltaDist(), moduleGroup[i].getAngleInRadians()); System.out.println(); - }*/ + } } public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { @@ -168,9 +168,9 @@ public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { chassisSpeeds = new ChassisSpeeds(vel * Math.cos(angle), vel * Math.sin(angle), chassisSpeeds.omegaRadiansPerSecond); targetModuleStates = m_kinematics.getComputedModuleStates(chassisSpeeds); - for (int i = 0; i < 4; i++) - targetModuleStates[i] = Kinematics.optimize(moduleGroup[i].getCurrentState(), targetModuleStates[i]); - + + for (int index = 0; index < 4; index++) + targetModuleStates[index] = Kinematics.optimize(moduleGroup[index].getCurrentState(), targetModuleStates[index]); } public void resetDrive() { diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 39703ee..90803ed 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -105,7 +105,9 @@ public static Module.ModuleState optimize(Module.ModuleState cur, Module.ModuleS Module.ModuleState ret; if (Math.abs(cur.getDir() - set.getDir()) > Math.PI/2) { - ret = new Module.ModuleState(set.getDir() - Math.PI, -set.getVel()); + double newAngle = set.getDir() - Math.PI; + newAngle = (newAngle < 0)? newAngle + Math.PI * 2.0 : newAngle; + ret = new Module.ModuleState(-set.getVel(), newAngle); } else { ret = set; } diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 3db3e3e..62f6eb8 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -162,11 +162,12 @@ public double getDeltaDist() { public double updateDeltaDist() { double newWheelPos = getTotalDist(); - this.deltaDist = newWheelPos - oldWheelPos; + this.deltaDist = (newWheelPos - oldWheelPos) * ( + this.moduleNum == 0 || this.moduleNum == 1 ? -1 : 1 + ); oldWheelPos = newWheelPos; return this.deltaDist; - // TODO: Make deltas negative with 180 } public double getTotalDist() { @@ -178,6 +179,12 @@ public double setAngle(double angle_in_rad) { double targetAngle = angle_in_rad; pid.setSetpoint(targetAngle); // angles are in TRUE BEARING ( angles are negated ) + if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180.0))) { + this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -1.0, 1.0)); + } else { + this.angleMotor.set(0.0); + } + return angle_in_rad; } From 21055b9ffc9fc827aaf28d3163949f4b8038ed07 Mon Sep 17 00:00:00 2001 From: Joshua Vigel <56195553+marvalarva2929@users.noreply.github.com> Date: Wed, 17 May 2023 14:22:10 -0500 Subject: [PATCH 29/31] Minor fix --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index cb43e14..1bb1020 100644 --- a/README.md +++ b/README.md @@ -4,4 +4,4 @@ Team 5417's code for the swerve bot, _Vira_! [![CI](https://github.com/frc5417/CustomSwerve/actions/workflows/main.yml/badge.svg)](https://github.com/frc5417/CustomSwerve/actions/workflows/main.yml) -- This branch is an attempt by Joshua Vigel to refactor the code to more easily readable and compliable to Java standards. +- This branch is an attempt by Joshua Vigel to refactor the code to be more easily readable and compliable to Java standards. From c69dc807a3a185a2eb24a2d5b146512bfaee2dce Mon Sep 17 00:00:00 2001 From: roboticsDev1584 Date: Thu, 18 May 2023 10:55:09 -0500 Subject: [PATCH 30/31] Fixed module 4 angle motor issues --- .gradle/7.5.1/checksums/checksums.lock | Bin 17 -> 17 bytes .../executionHistory/executionHistory.bin | Bin 214695 -> 214695 bytes .../executionHistory/executionHistory.lock | Bin 17 -> 17 bytes .gradle/7.5.1/fileHashes/fileHashes.bin | Bin 76065 -> 76065 bytes .gradle/7.5.1/fileHashes/fileHashes.lock | Bin 17 -> 17 bytes .../7.5.1/fileHashes/resourceHashesCache.bin | Bin 61747 -> 62495 bytes .../buildOutputCleanup.lock | Bin 17 -> 17 bytes .gradle/file-system.probe | Bin 8 -> 8 bytes simgui-ds.json | 2 +- src/main/java/frc/robot/Constants.java | 4 ++-- .../java/frc/robot/subsystems/DriveBase.java | 6 ++++-- .../java/frc/robot/subsystems/Module.java | 4 ++-- 12 files changed, 9 insertions(+), 7 deletions(-) diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index ff61bd30eaa02ec869317e784436e16d8b3c78bc..9b14f58fdf98521bcf4e95406e5ef00ad6bb79d1 100644 GIT binary patch literal 17 VcmZQ>zc8b1qOQ9v0~oN}1pqN^1bhGh literal 17 VcmZQ>zc8b1qOQ9v0~oMe1pqN+1atrZ diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index a36dfb797ead5ee5b23ec2b21fddd2c6bb5320d4..48c62ec0dc29e6b65c69de3a0e0e0918fd8a05a9 100644 GIT binary patch delta 10397 zcmb7Kdt4P&9(QK$732yMfhZ~96E)O%+&hl|0Uvy7QIW8%Z6lkxGb5%hYH8?hb(OTo znpzaU)bde2(=>JC<}0*PSH-lpY&TO#d?A*va@(-lGv{*fV!Iz_#6Q00cYfdB>zp$) z=Lf4_j9&d>^t%Ojch|>GC=sWYjZ>#-PfQ!HKR%sTOZ4d_>Wp!1n*VK?eD@uct17td z`j!RvTubi!e;HermLSFVEyWMU&0Em3WO1ozXpc@ToiX-NO&84Z<4p5$ZS1tMlcdSb zwr{JL^wR;jI*tBa#VbvT$uAVYvHITcW**e!(xbnByK8;p%QdyLxP5LH)us(Kb<5VK zH@Dq7;HqG^5^4bxjNV4w-Y53uz{z7A zvEe$zHc%O^6Ba6Hpv1^}gl2JdE-1*Ny2Hi>DmL;M7F~#{h<&zK(!PFUcK3ZFuZNmZ zn>0#&Ci*Kpx!y$qP&sr*=yINli#i@+w4;{6rW`s2?ngq`H%JH*&r@lUr||Twws1L% zN``GnJ z(^n!ii#riPy*Q#&pE|)9;GdvA_E?+PK?~k4NHbXr0pqf4rKgcL*L!qM?)DBt(}^f zKYU(8n)~p9n!E*3laVO%)A1js)aLJ;e5JT|;@$EtB$}~*_SEkmi^+&Np4E`=N;+GS z{9NDQQx~XeD!F=7|J;1~z0_`{U$OtX@0rpD>~8Us)>e|Eg6t(KjtWk^NIf|0mOF|m z+*0`NRSHf>bab#mq^Cy4-n=Rmqh9VGT%1i8xMFXvXGQMEy@RPkX@>qysVaWe6DtBn zM6fWAelqDd*}zLGJuLWe4fWTy!3|^SRUHz~c#r)W7?wRGFnYqok=%-|`zx)rIv5=P z1btuco}b_Ev1VfXDOZRjPtMq^_*q&WM}F{R3j|kNu|J~eRNE+Pn1NBlT_8K@hq#Gyrl4) zB+HuU^C>(OzDYjEt&U*=_9qSO-sX-y6ze5(sE@w0xc_;j6+VL|ctYo~k z1e_{~rf#x+Nfy1VE{UQh%Z9|83VgbOEI*FOhI#=U-q>pOZE%J|$-AwpWs8%zsa3UX zaPr~1XeI)-ZfaFB`^52^oo1E~OJb16s4dPtV&aZ1#3Oz%a9&g3e2U=bIacS;3&yXR zqG@Wpz_PL=NKhE0^I`Pg$QHLxau3iaObCAl*>kn@J?E}qWEDA1w&S}OE?1Ef+9nl; zHo9&70?2-!Y(x7b*Cbu54?VmW+{QcuuWcoP;$guyvVv$h6E1Bd9`JDW_OLx%22DHA zaMTWB0G&B3>)di;@z4Y7cM>Z^#bbqWvgNg2R#{E=y}?2w57l}$64YJcMvzJ*B*v_GW(^rqV3S+!N*l%RErOC(IF+ zb!-W~9J2P2S(NR(mWcpV{GFIb69KqqKe3FfGB1gVfh+sNxMhH4>kg1Sh@J;V9t<;u z^*j(!L+oApg#I%;a)^$G%{9axk#!0MM21li_X#m|Lw4wsFjFg97;|9Y5fTehE{D&K zgmKH^ub&bcj`Zu#kTUlfiGawOYZdzi{S?gp2Qh$$%ICxY8qS2{pA#+`&TXk=8NAbt zi3af~;bO~{%!Qr`XK(B(OD`5)|B_fCda+dV3BZDj|D^kc*+#?mH_x|&qOV#xLrU!+ z>KKW3!PoR`*nEtr;Nirt!)8~79H5EkTGWEV^;eqsTZdUeKbvhkZCuBLjbp$ajUwHW5V49L3%chr?MIS4Pn(kL6UgJ2C zQ#D0_!)Z(b)Yp?bhwd6SIF6juo+2-ITlSH90USR~DrlFJ4^z*O<*?65J4?rf3TU6R z_pEb2W#?M_ETKCjb3R!y(JPfTB#l!=!IZp`mt!SO=OmMb>I>*gwR1z*JI@~FKttFY z&mN`Zykjq&8y$Kngzb%FE29^loxlpA^a42%HYrn@*U_GX(Nt~d99{YaeKt$FZb%Zk zGVqGe)C|M)q92wMBvIyNK?ijpIw$_`NU7{|I$k8Tvd^iy=-kh^OHTb1LG)#^D0X8j zvYtQ9?{~YL-@~{o0Z0_(NNB!CR;gqM4eXQ%uzq{C>mll{m>~ zsFG~>CDcjKA;2;B!(XnEirJ-5=R&(VFo20kF}#B6Rar@A{kmeZx=&>_O_WrD7X|b# zfT#aW&j>|iSN$QX`K>dm0j-8t(nT-22JnVo@Cp*k={hg7Uez!ZzoM&fk!Mf`>pvt1 z+Z^(S#UI5YCvm1B@R})0hUhgko;OuqkTt=FUSb^fk^mi==)th)C*sN(b41G%BRWNu z4MX$`KGuu+YP`z&Spz)=6j7HHnb%Z!qlwha@?l5)A*%U{2Intmyw~eREj)=ecn-Di z43;%SU6yqT^(Q2tex|2~!m+FVN3tgPgjlUXpXrk|pMu(hoPZu&J{2_yQM1jbh$eLX z6?GMN{z3|6m!sb(QL1XFs-OuRYbvs+`E(9#Vcw8sMc2Hhsv55bnBrGc8PbEa8ecXj zC1D;!zn52JMKWYR+DRNQpbr;T^Rm2;HGMiOLbk+=f#<1KK5UVwhS2#Sd(o@0B8MIx zy3Bb|R|0(;8H(srB_Hos;JU;V!xfr%vCrAaFzuVuvE8*D z?U>Y19qe<;qnvUILG+M>?D`oEjh~)7mb|A%eKx%SMx)(OalG54sQ~(d%Mrwz8G0Q>~!CjfXabyB5cym`Ka69op zWFGLwx6-SNXVO~gYIR^D;q?Tfftn&KLzB395}Oz{(cBr?^Bb85p6nFnwj8+5VZ#8N z>`biSpip-s{Njy<9vF28Q6b%kXlUPsSRt|~_^?Zu-W>Q#*D#fGV3Wx#cJy^J$rN`P zDpSZT?lR=35-wK8y#=ZpQ6aMM`?1v`fGOQu(QrqgXAeTdEr8?U!{=aO&sGd_osWQ# zz5eHftFkvShoe9Q4+MWBi-E~$gm3HiiEiu7zwol9jxW*&`#5reIQj5+-|ManuVVUz9a}q>!Vxh0PKT62kkiRppd)L?%V_vEoz%?kg{1%%^(T?p=j0D? z$|pzEzMrwSk z&*Vz#bD(OeA#ga8ZVMM==1%vwbI@M|K^KMT>0bD(1&1k2ANOJmrzlLC`%Mg&C`=Fc z;%6*{y$aLay%EE26{fd)6Na7cVtTdTZb2@vY%YcVnr1i0bc8W?F>#)CmI*F!DS=u9 zPyI-DhF6i){Vq1V{w^lny$!?TcQO6l?_<~@gBj@FiQ$k8W`KJahSMy#1H-i$DE7Z$ z_^%8m-Lu((T;P(vg8OpQL6pSAOysr+!zGz0 zi6DmiGf~19G5jGDC48>jO1NtlN_Z~La%dLPorhs*7D~9XIpO&f^$JjN$j6o}lv*{` zIFQBM*=|orqb?V?K}Ol6;U}!nFB|FpjNyY8ET3(K`$9I;&;1I9Tah$sjRmT^)Q`~+$Of_*E0#8LtjkU z+>QBme$GQR2SX}5-lLJut|O=qsqA6aaSFvS4B6d^;XF%WJBC|_ zA-f-7Sa0cupeGN(6Id$bp{@E2hGX&&?JR~bp0>^1evQ+3xB>yNY6T>LNh CwlQV^ delta 10358 zcmbVSd3;pW_2<1eLqaAALm)tqCTw91@b(1)By80N2q{>t1@*moZ%kMNN()ppfwJT$ zOCXp7XhRebDI!8@rYy=*QMBw;vc0s~BxevFSG<8}*!K7)Eri`%*3&%Nyzb!N;C}XF~&lEHW|6{5qJ^T>k*}1!9 zT~+BL|4L2yKN(dk^O53iSMmLVg{2*)zA|M(Am1*WrUVt|+2@?+EKzdCIDE2d%dOY9J~+h7p|<%n-Ahb@n@KhOj1qe!Q{!Y&kY*)xNl_fg3_JLYgnPk@rCTOOabd3aD2xz=bGgG z<Y zsIjvUnkk(0KyDwV9c;Y7_+x+k$`xIVEA!7yZ>8j4+S@|CZ?#I~)6$*$}SwVNzi$5_u}zpw52F zv~StDNo;oGju&qS5W_Lc|Z==9x3U|9fn;NErXQ`|Aa1Fy}%tdIXa3Y3TFm$3l zWt`I^FwT7T*#`cmDSL%LB0#t8f_MKdV?G-oU8u+)=OpAMe*A{(3)t^1Z>-O$OqBa|!Y(Ixv@$=hr&-HjF zaqkf%I{d`z%WgM4(Qo~d+hf~z+5QR=wM?$I(zOgBKGbra_U^3~WvTPJgkHGF>|j!3 zH%wo%JlJ^K*_`vn=F@M@+k@RLo#9~`G|=Nh{1qmN35~zZJTd5jJBoX#D({`^44lx| zgwO?*of7N6e^ttcFX&p!6w7-3vA<5$q7iZ zrdf)m2K9g-E4;27ni>q)x~d6Kxsn|LSu5Gps3=d;YiCumlcLfd!PG$88EB*LtY8z; zY@3&PMY3g46%AD~MbpxPc2E^`UXp_W$a|fA8m_%Y;-}?|4TnUlJ{q1_#kP%>ld_sk ztm7o6NtUV@jw)HYXaq#b=5^k*WI@vmS#=B(-d@8#0-IMy_@U%Hxh7gaW1Veq102lSw(!Di-i>vqRzNI<{T3oVkBu8%N7YUmv}nqwAyflLu4ZB1=PCgFMLCK$eD< zGZGGMAg7X+^HjKGMLoF0G~N<|jw#xrZHT-n1}xh&Y(WqLLQt_}6Ar&kR-aZq7Y=SD zt53@r0{QQdYSD9AY$Da7<>bJn1TF@)Y$7F#k~nsAw3+2VMI!PTR~5ZSOx#{YJQm(W zCrJq?ykb~(z)-}1X#`9`wQWa}bv`I*ya0J2HU~!jne1?Cj<<(BZd~o{LrsE}??&wi zhEBoaJ)aW_c?4d+1mUkTqGMw9^!r@^8Rk-ga; zM5xrtGWSIIz%n14-V@;n%bM2(KMVSNL}pQD3+pC|plmNOk0y%Xk&lUGT$P2bxJ0=2 zaRhfNVA=XlNFGGb2gCjrVG8T{AZ8!2cjW;4dw6<3n*f{l5qm_|G8_;Y#zE2nV(Ol3 z|A7cotLqqZpyweH3sRm1M-D}BXTeK{2@OYj>8v546>!Vb{Hyn42aPO)AnJi(?% z1VCgl(Bo@TZ5$|>@W8>ZiL3i2=A0x>$%GFw{&~N$&#lwhh&mz58TBJbtp8Q#*t*FJ zqG&0>fGG0rN0BBOqHdYIW2$hl6PF98Pm?;MFK6^6NWqE9$=7Gdk2;loq@D}M&yot# za&lnGIkFt|oKD}cN#O$0b3XhgdOuUo6F>OE*Hl@SjGz*fP4x9+*}5*vLcm=k+eGgN z*l`hkqNZG^_j^ah`2@WVYd3`Z15U-s|2BF*DK*jh$%Ab*b^Dnzwmg`4vF=<@awbQY z{z&HQ@OeV(pU}pzbw{yn3!RgI7<5qIORy}N7Xqpnv<+PYvnM(*flH)dQIMKnCKZc< zRDJpX;i=$C)SgB_!d0?3)ZvYAZ=>2@dur0Jpx|1>bwgM3UqRY+avxl|gZ8x`1Wi#; z4a<=v)70%CIy8PWWUjJ9*4HylZ0qFf$$!6p985u zM@N@|!3&xgP|%$$3XY(glBl7ltIR`@z&!@f|BHB_r7-6scD2e|n&TLT9k3lqmo$Ut z1G=rqe9#J@B5LsBzuD<^w_rjW8vRjB?e7rI*_O~NTjQ-@K-L4MsEZCdIq2MIrfzAx z4wpp^^{{S|ET}o;Hy0BuN0dwlU6rEk7lOjR6F2nKW^DBFe>6yT4yNX6XyoI-mPQ~P~`Pm0V3RMa1m z9Lvy6Lsu-*ka%5|93`k}imU>2o1GT1A~gEHAuEDknTq2$l4jeotP4D^1q5DDBvlUz ziekt-4?nw$x{B}JA$6kV*bzY~iX)>3eZV$U8~uzahJflLpaO}iqDZFoauGNBDg z5!WVM1A5M^xM(?fp!!HYw0=fHjgRDm&oV`)EfN6Q%k{S!!Rw0>|KtbMp{^c>WW zs^!qONOE9UV^V8+&aJ39zl4o`Qa4&ZxiG2;sT=CZRMzeyw^j3D5}CkHKir!sYFg)@ zt{EdYtEYFe8P_Rn#iil4%B#sl1GPrhgeP$e#or=gqP9P>=O;2B%xD?mb`}UJ5yK)l zl|roGKv9n)^7)O1J{bNGQ6W8v1c+}~0;QHwT{oS%k_g;8V#gc62P2WQuzY zp~77RaltQ zp)29wgF*HDJleJ1p{3%??iMX45A=+><)C~@j)dR>_4G-AQoxn}_aAnbRX` z(une&BqM4^wXb9R5kvKe8p9JGw4BGGy+EqwUiTC)oM*##70!a{@d1lP;sS?7@&1?+ z_JP%D9|Zm3wnY568=B@jAF5?S!TC^qy2C_;knk_$gTCJPD_m_p*ayXD^=SB~N?afg z)pzpmBT7c`m0Zr8!G8z@$8@fnH?;Wbjv|ORxDMVgF;on$v-b*ymce!M-o*#$K!`NP2di*XI3PqLk8T3~)y9BN|+by=!@Iq>p5BtBu2kyH_3$poFzCXYSYdf@6#IG% zKkm(C_!hZPC_3Y1%HcH`Jus6qxuixPhNbhpU}kbnVL&FA?ESphwK_Tzxjl#B!c3IJ z5)8LwqJ+0%cs3Iyd}pq!o6rX(ylS2cb8M}#Tw_RheFLaG{?~mXn3`h4z+vQ6P z7rXF0hI?Fv(nYTB_b#l!Fns_@dJBe+x$q+lO9r6GD)5f1cHssLPY*yVzhI_oA#R|% z2{04}qD`7YGeh$`0~l~O0{}C01e^c> literal 17 VcmZRcbt>7YGeh$`0~l~E0{}CX1ik(M@o&44eu7)=skL|0)iL>hpZlo2!n!+rIzs*8Q(?kDfP|g~BxjJ)iiH*2rK^mp`qv zp!YTs;P?inB@Q7e@oh@FgEoKKT8xH55?H}RR-K!*&~sFyuXJn1Y9&5qtA>wtBmOW^ zc^@UKP4^E`FYyz!OFTp4J%+n7g-bH_8CDmWqDjs9AGilb1JjQd%nhdek*45On$=!p-5Zvu)<-szYv^8;q}lFFf2omAw~pA~j9I$X_c;k!Dm&{1p)sm15csV_wPzd@o}g=_T+jhPbx!y|s~);%n>b)beL`5Areg_jP8aLxIXahcQ2n@OZ1${J&Lz}-P<4$3Hq}|j z-JaGT@4C=%q}8l_MdXBrFmGpPRBZ)ux-Z{7x7=1!y`}B3XIdVv94-;5-L*V5GE;}n k3c||y)3@%!ScTDNY-oZmA9f&9L9_z)c1E~b2G}$B9~IBh{r~^~ delta 1516 zcmZ9Me@xV69LMh=F3Dfq?fboy6YdxHJ+2at8#CmONNm=e-XxsmcXfBLeoW`AV7Zpr zjB!xhwmLAg;zEQ`$+mV~8O0zKd7M{Rz$?>(=5p3nRJ z`Fx)5?dcfF?ik5_=@dz6m5yg;TWtK9{|Z}5%Gl2x?Dbo(qkMSrp#4g0^5Q^El)dTu z$>cA1)kTUetkrNvp$k(A^C{WK_Y;S)>2^}QfTbo*a<}btnCf)*KFyLRK0puK=J*MI zl~@}@!4m0zCpIW7L!-hdZEF$r%Q2yXE3ss$RIR{Tg(Y}F;c}!EMldeXd=Jh%Au~Iv zzm?mX&tnG*T5X$0dE3`{+i$muwiEbPY7%>~z%3_e!95C7cv@i}Iu!QPuKi-z5!jc> zV;SN8g>>qWP9q0|{{(&DfJnT8*I6PzplN)iFoQXTHxSJi2}we$+IVVmF;XmOwVw=e z`>k-giB8Y7asOl(86{edy9%T}inzkNv0vdToK;vuhYyMZCJc{M#qlshUaOARa`IYu z$3sN1$I+{Dx8U;s==Dm620Zv5(z0Jfc2QuDbsBDz z`DDQ+tn*3h7Q862wg8<91Nc#*d6G`0_(y{|L$M)Zw;(AfeFJR< zar;(m`K^ETW2qx=&1-GAT=6KoI@lHBNN0q1RuR55NQk+mXYr@>$U;#_xY=QbNUc@X zLr*{A_Jy4}Rk@+btIoMyE&A-aQgN58pAlxRIa~`_W`URvf01Poh|Qz9@4J0kUVpXY z*6o|BHoTU+Ki1x7?P=y21>F};#$4Y56+u=S=}EhoM6$l)h* zdneXis_6ddc(XKIBNUbG79tcxwF@rDzHh j!`{??C^L$#Fz>o<6`gz8jhQgf9Y&yTwQ#kyTYdZsom~{$ diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index 5fdcddaf43c5ee36299257e1a96e3b2dfdb31483..725f2caed1d2f12db8874646ad21ca910244789b 100644 GIT binary patch literal 17 VcmZR6A~{9FsWEOA0~jdS001md1JnQj literal 17 VcmZR6A~{9FsWEOA0~pA@1pqAj1cCqn diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index ffef9c9147395f14ff9e9f32563d393683804caa..11197ede1e497d5f29d779ad88221ddd8ad186f6 100644 GIT binary patch delta 1259 zcmdn|hG^NyBE&Lq8Y=6z9yBJ^euU2~_?6%|8#HVAe1@|7F2p^-d+IXy|_^FwehndfPJ& zK8X0Im6O+61#IR#k;%y4w(J|P*49N(4R<$Zo-AbK&*Gfd?Qy9es=9sh#ZwyueBQj% ze|(bjI#kekbLDAIR(?TtuPya#E)c=be<#a2X9%QM3)$~4Sq%-}kGm(=0tK^--Y5Oh zJ%0vjQTgP%&JQ<7UE0XZB(-s)VuI-A-Z#rwCQpsx4?b{YQ()E7x1Vy&ZkxP(1@fSr zF_4f1VnGlV@R5;<+qV4T^;17RmmIo(JQ!0&@AB^ zd;8cf>DTgpUm1Va^&Y0KH1oe2Npimrf6GcYjt@!#1s0mkV*ARb)jeJ%^4-#R9@~X) zZI~*GoN89^^{a61+H_9Qc*B8ego=8Y^CvYty1lcYac1T3)0MtbGVBN?Fcp{bv*(D` zeGaV>y429Y=06ux#kP_~`}odOiTIez{m@_9l8mWhQiSj8l&H=xKVr+VI?7v@VXEL>zQ!=~LS&QqL&nX*WTo z0-A5wcp5~fsd)(JYGmv=8@J^%rV5GRt95IdO>XIi6}Hct?NN-WV$!$#UoV%fnRvwk560kfVWZvwL=B@hUc zO)aRi4K6qVlU*;VvmP+n0scl}9%Z0~qk#1^_iI1la%p literal 17 VcmZSnIL9>cl}9%Z0~qkW0RS~S1nB?( diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index 003323daa44da71585dc0f17781856f44b8be1e0..b986757b6398461c9cd6fe56aa5171ffcd54b6fb 100644 GIT binary patch literal 8 PcmZQzVC>LO{@Vut2O|Qf literal 8 PcmZQzVC+z3O1=sJ1nU9m diff --git a/simgui-ds.json b/simgui-ds.json index 14bdc93..5ff7183 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -27,7 +27,7 @@ "incKey": 265 } ], - "axisCount": 0, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2d9e3af..a9b8cc1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,7 +25,7 @@ public final class Constants { public static class OperatorConstants { public static final int kDriverControllerPort = 0; - public static final boolean fieldCentric = false; + public static final boolean fieldCentric = true; public static final double joystickDeadband = 0.2; } @@ -50,7 +50,7 @@ public static class Swerve { public static final double maxVelocity = 4; // m/s public static final double maxAngularVelocity = 56.0; // rad/s - public static final double maxSetVelocity = 1.5; + public static final double maxSetVelocity = 1.5; //1.5 public static final double maxSetAngularVelocity = 2.0; public static final double maxAutonAcceleration = 0.5; diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index a6d3b15..40286a4 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; import com.fasterxml.jackson.databind.module.SimpleAbstractTypeResolver; +import com.revrobotics.CANSparkMax; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -7,6 +8,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import frc.robot.Constants; @@ -149,7 +151,7 @@ private void updateOdom() { if (cnt++ % 50 == 0) { System.out.println(m_pose); for (int i = 0; i < 4; i++) - System.out.printf("m: %d, %f, %f\n", i, moduleGroup[i].getDeltaDist(), moduleGroup[i].getAngleInRadians()); + System.out.printf("m: %d, %f, %f\n", i, targetModuleStates[i].getDir(), targetModuleStates[i].getVel()); System.out.println(); } } @@ -191,7 +193,7 @@ public void periodic() { for (int i = 0; i < 4; i++) { moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); } - + updateOdom(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index 62f6eb8..1302f54 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -97,6 +97,7 @@ public Module(int module, boolean inverted, Translation2d distFromCenter) { integratedDriveEncoder = driveMotor.getEncoder(); // integratedDriveEncoder.setPositionConversionFactor(1/6.12); + //driveMotor.getPIDController(); pid.enableContinuousInput(0, Math.PI * 2); pid.setTolerance(0.0); @@ -175,9 +176,8 @@ public double getTotalDist() { } public double setAngle(double angle_in_rad) { - double targetAngle = angle_in_rad; - pid.setSetpoint(targetAngle); // angles are in TRUE BEARING ( angles are negated ) + pid.setSetpoint(targetAngle); if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180.0))) { this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -1.0, 1.0)); From 2b0cacb717ff026f7ad375dd561b79c6f65d7ea5 Mon Sep 17 00:00:00 2001 From: Eagle Robotics Date: Fri, 19 May 2023 09:44:10 -0500 Subject: [PATCH 31/31] Update README.md --- README.md | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 1bb1020..7170eca 100644 --- a/README.md +++ b/README.md @@ -4,4 +4,16 @@ Team 5417's code for the swerve bot, _Vira_! [![CI](https://github.com/frc5417/CustomSwerve/actions/workflows/main.yml/badge.svg)](https://github.com/frc5417/CustomSwerve/actions/workflows/main.yml) -- This branch is an attempt by Joshua Vigel to refactor the code to be more easily readable and compliable to Java standards. +This is our custom algorithm for swerve. Here are the developers: + +Krishna Shah +Jordan Escobedo +Rupak Bhattacharya +Pranav Gattineni +Brennan Coil +Lucas Hatcher +Joshua Vigel +Matthew Caldarola +Jeevan Adhya Vinoth Babu +Ethan Wallraven +Boden Smith