From 605f740b6707e946f79b44925fa9dc9dc3675a0d Mon Sep 17 00:00:00 2001 From: G_Zod01 Date: Sat, 6 Apr 2024 09:51:34 +0200 Subject: [PATCH] he --- .ftc2024_autonome_api.java.un~ | Bin 0 -> 5938 bytes .ftc_new.java.un~ | Bin 0 -> 25789 bytes ftc2024_autonome_api.java | 14 +- ftc2024_autonome_api.java~ | Bin 0 -> 7221 bytes ftc_new.java | 964 ++++++++++++++++----------------- ftc_new.java~ | 563 +++++++++++++++++++ 6 files changed, 1058 insertions(+), 483 deletions(-) create mode 100644 .ftc2024_autonome_api.java.un~ create mode 100644 .ftc_new.java.un~ create mode 100644 ftc2024_autonome_api.java~ create mode 100644 ftc_new.java~ diff --git a/.ftc2024_autonome_api.java.un~ b/.ftc2024_autonome_api.java.un~ new file mode 100644 index 0000000000000000000000000000000000000000..b8fdabdce4475b672dcfc7fb9362e436204ff4ac GIT binary patch literal 5938 zcmeHLJ1+!L6rNow8XcvEkWD-aQD`-&ksxIDA?#up8=~>t20=VJ*0b@9N09gzI`IQM z8d0f4IOon?8ON^7+#FNwNygkc_PgJ?_uT!?nQiIq-)bJHHy$*9;{AT6u=X*q_F2E) zIyN1BNnS?sjmkjt<>Kbu%hj%?Y1eSs%?}T`w|$`>$*h;7=934X!xOa zF@iszVBh~qIkR`h9e{M)$aK6F!Mx{iUC8+{B_+ja%rt^VyrM@pMMaWz_#K;6*B0NbZV8zoRTv?0KT)o6nR>V_6|x$GV9Ek}Vo?GyebfUBfj^C_hbRlQZSd`&CCe1}qm95qe133Fw!P zoq&o8I{_6-b^=-h8Z#%^mOke>rlpO20kmYZ6VOhCdxvO)#kl|?tf(Q_m7~CBg%x+h z69Zv=7TtITVg2Jo*;{I5UFDvi$}0E#R8_g>r=rR|Kh@L*N1Ez5y;M@U!>5YMJwFvx z=Ix<+${4+VWh{_=cJn{Xh!sy z_1g$0y)(5|(KGAtgs*LAcr3h&1)J#z(xQo2dSHN3jpl7Ey}+1yU$$Z~z%zt_fO#9s zATZ+32ExR?jKgr!diFS$31G$IBTU4y0K+Be@OEDoffXx>FcHfJ3^%32+gOsox-FTl X^Kt}FC(`3>EXTllnu?`n>gn(csjBDW literal 0 HcmV?d00001 diff --git a/.ftc_new.java.un~ b/.ftc_new.java.un~ new file mode 100644 index 0000000000000000000000000000000000000000..b0464be339570b883872cfab0ef0f2f58d0c2893 GIT binary patch literal 25789 zcmeG^U2h~wQgbH`IB5@-({UVdr^_+yZnwwXwtL3AGv4vccsxC`X8ncjndM-GRqig^ z#Z*`IR#n^W8_fg10EBpkSN3>035f?JbkdD@1_^!v2q9jPcmf0wkr`Q;m09h!&4)N$ zNp@9N#aBi~L`Ftt)r)BG$Ip`|=|A51;M>0ccD*`NR2#$Ubl z2cP}R_y6rbesJ&ZAALG_aO1`oZ{M(wFThXy!-KEs_tD$`{^lp-orAxT&#nIqOgH}% z4o$`a`|*#dG(akskX9GHU~K4)AFA&7*Ka7mI|jmg!=U#vI1WXc9k-97EH9!YA4XZ2 zw~vZmy9k3pFYSjPDth;$!7$AV(Mt#I&qhHE=RrG557VNTW??%#4U-~H(_uRu4gjsa z5hY=eZ4WmUjrqy=qaupi-8dNLVShgw5Zf(<;up~1Jubp53F3AVoJPk%5v57{JHgpb zRP;`E(>PvDj^i-LhRjYDf=IS^u~7)Ud|CecVH^&^B0ERy3mSY9Wc{-MH1{kRgu|eZ zD2tQD(iw7&TqQC6k}AB?uM2&|h7Yddkw!}18-rYt;d?}n!zjSl_TUB&(?#!UwOJc-Ck zuSIAa=G$36%upfg*O3Sa?S@BT7A8IF!DrVXW+)8hdU6d?J82$aJh(2M7h#5aN=4F3hMZUIB?;7H}vsVKN#B5(9%S zJ#OM-Ji2>(tLsm9SGU$ao)g`T^{v%8v9-Osxw0h`hFNqP z6rs=oM`Qzpta%vILqx1%ia{FXd76ZyoXq2<2c|m~@c_aNa7@V{Yk)Ef=$HJI4pRz_ z-}XPvQt8dumT#CK6i@^VBhqBfKduMGzrgy@ubm2nzv27vPW=W1QdOv zZS)GF;qjA*IzJ0nD|jVl#YP~G(xiuGEQaYQ6Gs7z(xM;cU=~BrDTorrFv5v^_~!Fo zbQFCy3UekUkknH}>d`RBf`RC#V0WP?#L5E!=Oi5*BEd8%1n@WwM}^`Mzh`lHJU_Oz&5DB@o%}01$!$n4e$!y41m>#i_SjcCN@AJ8a4@aB&#(a$_;JF_j1*5n? z4&+tlvlF`k)1Srw**2o+VS!8|Eh1PcuDGT7$2vs)1=rl{}dn!h2wCiFGRCfy|=g z6SNcEyh$R&;gJPvijKC#(rc0OD0vwr$0mJ8%Yz_+NiF3~h8PYQ&678RQ3CNEqC+;K zYzDdN;8dXR;v}#Q+IkWcL1ShQvW-y=S>Vh%B<)cUN8b(mGn6Ul=>Sj z-qd}mpedD>ohmaipk@%dOb4Mbx>aQgUN1{`tfh&IEDz&TDaabPs3dZgUS9W;xs0YC z@2-9)Ldf z!d2S!tSSZ)xq7SgGyzhOsTw3x71^N2EDPMcx)|Ck6_v{dD>U5qSrVF&dKyCWRqc!f zCEKvTn2-u80k{^jv;O4iS{fc5MLjHQ<&CBG0!23DsLd4cxIbamDXS>?vgj=Q@cZ~% zKt_rqStwviDY9Tl#o=Hi@)UDRJ=g=8vy%wG8fwZ63hkv(%4jt2FSM5^3)V@N!4vgP zvAKazn6$lRw1|roAMicT=!yEqq4O*NL1FYiN;8ocL8h!08>^F-3S+MJG!{QtkgCyB zwF=~Zkim${nr*Ze7Zie=yn}cZ1g{nBKHoan-#*yc-do>a-`>)lu|1)Ax^tjOAc6oH zptXD#^MHzMO(Lsft4Cvu?4h+b%AmAYHdGR$;uFeP5*rQsP%x5J5LB_Am|{0PO*2S^ zvO+%A-OhZm&$a41N|CycOsGoJWFIZFwoJ&W^{u5*=9<#V#d^L9Db6W0NY3dkt3iGk zU5;f1f3c03=>e>x^j;p&3`Wb2keKEBAoaB71w!#LKP&ogK0hKagPIJM^B%>J$BN>$ z$e{)l2jUdG53X=>2l?}H2$xwJ6?jAO5sPA0*H@S_=FVCUO1z;$L-;&3`WXhrGnJw~ zHSa*3rAD;QYl;TAmJ{VvE=R#ZR9(MPjo{)AwFPu))fIrgZYw&RsA>rso}@5NV~?Cds;qkJchYmR5$J_c?}VO~{nf~V9;}=}y(r7x ze2!HTc%zf! z6hV!DUIY;8jzpLUoD7GtfKeqF6)E7~Vq$o@Jq+p|9O8qjr&t>1`ZM-tIZQc5QmNN=Od$pRrdtP$4|(D55t=r33H zP)413YXRW`U7nmhYQeHmp2ASE{V@Prae)y4ShuOM2z>&52oxFtga|TW>J%!k*5T{+ zc3P6uhk$?R;(x+C&9mk|yHa#!8*&sv(;_2WSy^a4TwHG1rb!ZFQF#l17I4h_HIpP? z18FJRrAPxcCz>Edp!PIMAc8^|C3WfwH}k_YV9%Ss`5LR>u}t1!D1g*mU!%S2`5S}d zxdaE_Ea5RSAx<6pyCx03CibwU2dlvG0Ot!%j*m2JaWjPrRj@i-T|H%S5vQXS!yvgB4SaE<4nvG8}VNgQXZocOn8qS%3`OlO^tp)L73vkY%dq8PT6YWx5+ ze{ZrJ7GV$<QuDQfS?yp2SxF5Ghsme614|OKE85Szkfm@2q?6n*Lp%e|x_FJb>-Yg` z1jjyV&XS%f zAJf%V58YDVUb? zctK;Yti%gC=Q2vPiXGfvtVFb8XS#|&%1EsCAy>$!$W+LJ(I6mSg29ojViF;%K^GC4 zWD{TGCYWbvg)U5*_dT<2Ly+J!fE=nfDrV-&yVR0ti5coKh?RGVSe?YbZ%WG7p!ZT1 zbf7XxDDO8PNRNEz_Jsj7+dw^o_v&HZc=!%IYc3nN(AIyCz2X*&B4A#}hCyo|(h{B%4(@;}&_)N{eyaW?B+u4+DQa zZ6PV@+VzMfGS5=iQcU@5RPfi4YOr( ze5}8A=Ae{64<%Q0C@&`NT0+bg)=cU!z0-A?(j&GGGf>4dg3ci~|Qhd{qhr zwA0Fqp{oOxk^o^`tXQXJ2a?&MSrtmE4)Y4h1E}G_{?J!|;iQnN=rO+{ND$|_5}TiU z#rV2u-r%;lbw$<&toZ|u5)|onu(H^YoaYzSN@o>Rnz#g<FG+TR_#WN1$T#>O4vY z5Lo1e9oD#+*xX4XTCFS9e-Ewt@1az!)9Y$xZtPFl8-`&YtPbYcXj#tG#Ogv>D^nGI z>O2P?TjwF)QuOtfL4?Bt(^E^&c#})W=uEn%P4!nxiwKJH8T_yYB0H!lR#qHIq*^W# z+F0`@M|aYw*D`ZP>-`Xgs2-+3di~tENlGWuUVS(gZB1mc#bwEz`d#Zww$GV#>F1!& z`jR`e=7v=&vaM8Zwt=-X0NXNKi~{l2KC|#*Y(DZ;rXWP#TWE^=D`E*KZkiPN+N5Ue zv$=p;O(z@TnCsh6bD^GajKW~(gLg_oHvLNGBY8MDHV-cjZEWpv3XBT z$1%=NP2tcM%9X-CY*amf-KXZ#)M(fgziq505)AETHKFi^TIzQ?##VsN_uju`M>70l zQ+~YONMVE74ZMi6O)6{{}J&NScIwpd0AW*x9d5zCYl3lu73Y-)Ny(J@N zM;nlt*{`z9=DB31=isMuzz=iF$B(sNM-c$whTNNqP zPT4%E4aeejOo0(?BkZrGF;plaHJigFb@{KeB=LpCt2ol7!Q3M+6fLflRoL*XhlX0NFfU>Q8nK3W4Z&RGC!?AmR3LG0j57c+m zK<(vY3+CWB{e$hnp%$iYxPoe+`?5sz`dSh&rIU-WdRuuN3Ge*fZf#{WA@OdBMd@2! zd_#1w*$O_%rtv?*R#y+3f33||_qTd!*|JPc%jr=gTyT`@D@cxX$jR24V{cv@n~Q@}YheE$Y!qJGy_St?e6L;E+Me33ytM3In3`4td)+*I zHhM0jrvr<4s)jODd(qG)F|2qk#R|-B^~4F}ESDxu7f8N#Ro6<97@w1bvXViBD7;U~ z;+=vK60C7XIi;<`uzP2R%4-tCNzsspE3gFq4SZNV9D%O(eo7r}TlA~Z-mA)tzs*Ko zjci?Y+&fpYl1StadV377;m9nJlL@CgESsfZjg?#J5Z+myeys&ds2riEhje1O&ml<; zav&*)*K;gE^jL8TQ2mxNY739o0$k6kCF5OMHh%?Ayi-|A3TeewxeO`bq2T@(48fU+ zfFZ2*dRf{#!S7DN%MJ7%x)jU!Z&|{ac4}-IkM*VFkF~+r&6ev3+D>>0g5Ef6k2`wA z<`R$>$Ckjs@$N6R`BZYaJJJ}uh&D4nqIVRuIo9gwe=_Uy1|RSad&OCKHM)(nbxP-k z=CoALNNH}mD3#~TXzM3etIzkhH&^%9fBkt^&amFr2CFVW6em|hn=h;DW!S-LgPy<# zeIpck2jD8GIpg>0%PWZ1i!Fy<5`prnA&65b#b$a0;lhSDy8&P3*WbkY7Q6!G&oI}%uh8}54>$18)Em+hd_@v?Eeyz1Kfl;t(g zruMGq>9!WGN6T*I)U>=_;B!%{1c~bKxF}{Q6%Q?wmW%v?kd1{guVh{+Z2x2-O!q9M zu-5RnR}Jq}Qm}KbF^G9KopX2l`Nrxq*eW(Jc7wqHKWnrTjp1#fevsslT)?YDSMpo0 z*O-=9vE$a#f4}E?cUIUoTajW>NpTh5^>VYVx`x}2y;oZ&uH(O@f58R(#=}~qz~>(C zl`DjXFW?#>CJNq31jAWh>+$-mm$&V+mBmRIP?@(gFGAs&H^IS}&fe+`F{Zb!eYCo_ z*KKWe*PmT&IK8Y}E~sfma!2Y4f+L;wpSRO`w4vm5}eZ!Kd2b;{vcClSibc6hy_B#o>)5U&vGBeRT~S z^$*MqRo83cEpDQ>;s6T1po!2z)kIJjeXvy=!Y5HNllV5_R{jA+V_xUPTZF!tsmvdo zvZO-BfyG%qRL{)L`oBp$s|(*SFqCWG6NfMB!*$-_Yl@VU*_$b6$=_LfN44<~hWvT> z@&ZD~>6zeT9KE0;E4p`g>BwcrLK!u-jT!}gLJgv@{U&q8>b?Ll)=l( z9)YIgW#f2x)wTCWf6VgQ{#Q(s!4QDQm*}RvEihEetpGy?aPZ$ zbP!l>XJ0l|?uBNzg)tA>0hmz2-c2R1?_7wz!y}5`NGf8WVfdjh#UO26i8u;qI9@IB z{plrV1t8@~c@YV1z0~N{r`8DQQEJos3O7-+ioafJKm@*SBR`K`5l}}d`F@%8{Q{Gg Z`O&B^vs%#m$n)3@zwmnLcmL^o{{>wVYd!z~ literal 0 HcmV?d00001 diff --git a/ftc2024_autonome_api.java b/ftc2024_autonome_api.java index c37f0d6..0ae9749 100644 --- a/ftc2024_autonome_api.java +++ b/ftc2024_autonome_api.java @@ -82,12 +82,15 @@ public class Ftc2024_autonome_api extends LinearOpMode { switch (autonomous_mode) { default: robot.boxElv(); + robot.harvest(1); robot.forward(0.5); + robot.harvest(0); robot.rotate((-90)); robot.forward(1.5); robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case B2D: robot.boxElv(); @@ -97,6 +100,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case R4D: robot.boxElv(); @@ -106,6 +110,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case R2D: robot.boxElv(); @@ -115,6 +120,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case B4N: robot.boxElv(); @@ -132,6 +138,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case B2N: robot.boxElv(); @@ -140,6 +147,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(); robot.forward(1); robot.harvest(0); + robot.forward(1); robot.rotate(180); robot.forward(1); robot.rotate(-90); @@ -149,6 +157,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case R4N: robot.boxElv(); @@ -166,6 +175,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; case R2N: robot.boxElv(); @@ -174,6 +184,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(); robot.forward(1); robot.harvest(0); + robot.forward(1); robot.rotate(180); robot.forward(1); robot.rotate(90); @@ -183,8 +194,9 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.harvest(-1); robot.backward(1); robot.harvest(0); + robot.forward(1); break; } } } -} \ No newline at end of file +} diff --git a/ftc2024_autonome_api.java~ b/ftc2024_autonome_api.java~ new file mode 100644 index 0000000000000000000000000000000000000000..8d90a84b27e12a74c148ba0dc8b36f333ba90e48 GIT binary patch literal 7221 zcmeHL+iu%N5bd)8|HD8Jg`7H+t-3(lAVB0;MvTUm!PpJj7hy$?q(zpy%&%$V+GD1}DI=JR;96Ow zyR+2(9hI)G?@2rtULBT-p)+zt>1N!s`ka8w~Gwly&W^`Hs%b zI2i~YKp>bgaA^bA$bFU>g?$!tKq_e&8mP7${CRi)^vShRCJ{JX*N*uk_xQYhaeRJ8 zzwH~k-W>aM@6)|c_x{;UhVfSosz8C<1`15ybv!v+*)Wd)h-me?UcKI(kp?Ad!iWV{ zYLbNBECJMJ6JoWInukPy72hU`hoBRgkz4bq zh_w?D5xjH;q;z)8VsC`DneX7GIXs5OKtzqB+z6+ncT&=IrK*=Z>=`L;jN|geV14u4 z>Gc@hS!p?niLY#`pM@kd-!o`0=nP7ai_+b4#q+zrcKvtVn$P*RPW5-x>Q+rrpW~QV zS?LmD>4|slcYaBM=KE7dR~S#ZF3l#1n$L9aapDoq1pkE1Ih1q9eI!o70o*dpBBT?; ztPsjXGvRe?zCo=}U#IH5RU>uD0xn1MNZ#KbFI!t671y9wm44^&YT|RO za`$vo3?H`bVob?nmlEQVbi7#3LTbg_SU!ZNN=L6N)1juEkXJRDa$dBa%@y1|JX@r6 z2>CEQxUGR$8N&xO8o`8#gf3JcF*Q8cF;XgL7ECav(Eq#y>PFIjpoG$WGp1?Oji>Nk zmW{rkF|p5Oh%;IWQwr&mpj3-6i}#f26|gUfTpMY!2V^9C3k{v!Y7CbU($rKfcnN1T zRzn*MBG7ygqa}_|5+WvPOml*^OeG1lBt?h z{6EmhK2&CnVm)96-T3$Tyc7m(4fw+&+1{H-($M_nBS{sxBC{^TS+NwE*`I=>_m8I= z#{H>AdX@WpVcc3hd0%g<#)5BmtJ&{2ixps7Iv_Xg^v5l7Wv6W3HHULY!ETMWS!~C^ z6~^sR{k8YL{Rw!y_Gbt3^{*(#u^YY);z!f3c h9|ENBPxrn6pE3h}EkJ-(Z+7`t^^TMCf9XO${{p30bR+-( literal 0 HcmV?d00001 diff --git a/ftc_new.java b/ftc_new.java index 25b0002..5948748 100644 --- a/ftc_new.java +++ b/ftc_new.java @@ -26,7 +26,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity; @TeleOp(name = "WeRobot: FTC2024 NEW! Carlike", group = "WeRobot") public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { public enum RobotMode { - ESSAIFRANCK, ELINA, NORMAL, TANK; + ESSAIFRANCK, ELINA, NORMAL, TANK; } private DcMotorEx rm; @@ -48,516 +48,516 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { * @return double une_exponentielle_très_spéciale_de_t */ private double helloexp(double t) { - return (Math.exp(5 * t) - 1) / (Math.exp(5) - 1); + return (Math.exp(5 * t) - 1) / (Math.exp(5) - 1); } public void nextMode() { - RobotMode toNextMode; - switch (this.mode) { - case ESSAIFRANCK: - toNextMode = RobotMode.ELINA; - break; - case ELINA: - toNextMode = RobotMode.NORMAL; - break; - case NORMAL: - toNextMode = RobotMode.TANK; - break; - default: - toNextMode = RobotMode.ESSAIFRANCK; - break; - } - this.mode = toNextMode; + RobotMode toNextMode; + switch (this.mode) { + case ESSAIFRANCK: + toNextMode = RobotMode.ELINA; + break; + case ELINA: + toNextMode = RobotMode.NORMAL; + break; + case NORMAL: + toNextMode = RobotMode.TANK; + break; + default: + toNextMode = RobotMode.ESSAIFRANCK; + break; + } + this.mode = toNextMode; } public boolean isBetween(double elem, double mini, double maxi){ - return Math.abs(elem - (((maxi-mini)/2)+mini))<=(maxi-mini)/2; + return Math.abs(elem - (((maxi-mini)/2)+mini))<=(maxi-mini)/2; } // La fonction du thread principal @Override public void runOpMode() throws InterruptedException { - double boxRot = 0; - int signeBR; + double boxRot = 0; + int signeBR; - float x; - double y; + float x; + double y; - double t; - double t2; - double t3; + double t; + double t2; + double t3; - boolean already_b = false; - boolean already_a = false; - boolean already_x = false; - boolean already_y = false; - boolean already_up = false; - boolean already_down = false; - boolean already_ps = false; - - boolean already_paddown = false; - boolean already_padup = false; - boolean already_padright = false; - boolean already_padleft = false; - + boolean already_b = false; + boolean already_a = false; + boolean already_x = false; + boolean already_y = false; + boolean already_up = false; + boolean already_down = false; + boolean already_ps = false; - boolean sinking = false; - boolean manualMode = false; - boolean firstLaunch = true; - - telemetry.addData("Status", "Initialized"); - telemetry.addData("Version", "5.123"); - - lm = hardwareMap.get(DcMotorEx.class, "blm"); - - rm = hardwareMap.get(DcMotorEx.class, "brm"); - rm.setDirection(DcMotor.Direction.REVERSE); - - lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - - moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); - moissoneuse.setDirection(DcMotor.Direction.REVERSE); - - lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); - lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); - rmelevator.setDirection(DcMotor.Direction.REVERSE); - rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); - rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - box = hardwareMap.get(DcMotorEx.class, "boxRot"); - box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - - avion = hardwareMap.get(Servo.class, "avion"); - // box.setPositionPIDFCoefficients(5.0); - - // rotation positions: 20° pos initiale par rapport au sol - // while (runtime.seconds()<0.5){ - // rotation.setPower(0.5); - // } - - telemetry.addData("Mode", "waiting for start"); - - // rotation.setVelocity(1700); - // rotation.setTargetPosition(-1000); - // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - telemetry.addData("rotation target Pos", rotation.getTargetPosition()); - telemetry.addData("rotation Pos", rotation.getCurrentPosition()); - telemetry.update(); - - waitForStart(); - rotation.setVelocity(200); - rotation.setTargetPosition(0); - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - while (opModeIsActive()) { - - x = gamepad1.left_stick_x; - y = gamepad1.left_stick_y; - - /* définition de {@link t} sur la valeur du trigger droit du gamepad 1 */ - t = gamepad1.right_trigger; - - /* - * définition de {@link t2} par utilisation de la fonction {@link helloexp} sur - * {@link t} - */ - t2 = helloexp(t); - - /* - * définition de {@link t3} par utilisation de la fonction {@link helloexp} sur - * la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link - * x} au carré plus {@link y} au carré) - */ - t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2))); - - telemetry.addData("Status", "Running"); - - - if (gamepad1.right_bumper && gamepad1.left_bumper){ - avion.setDirection(Servo.Direction.REVERSE); - // telemetry.addData("servoPos",avion.getPosition()); - // avion.setPosition(0); - avion.setPosition(0.75); - // continue; - } - // Choix mode conduite / actif en manuel et auto - if (gamepad1.a && !already_a) { - nextMode(); - already_a = true; - } - if (!gamepad1.a && already_a) { - already_a = false; - } - double lpower = 0.0; - double rpower = 0.0; - double vmean; - double a; - double b; - - switch (mode) { - - case NORMAL: - double ysign = Math.signum(y); - double xsign = Math.signum(x); - lpower = ysign * t + (-xsign - 2 * x) * t; - rpower = ysign * t + (xsign - 2 * x) * t; - break; + boolean already_paddown = false; + boolean already_padup = false; + boolean already_padright = false; + boolean already_padleft = false; - case TANK: - lpower = -y; - rpower = -gamepad1.right_stick_y; - break; + boolean sinking = false; + boolean manualMode = false; + boolean firstLaunch = true; - case ESSAIFRANCK: - // Code ci-dessous OK - // lpower = (1 + x); //(a / vmean) * t2; - // rpower = 2-lpower;//(b / vmean) * t2; - // lpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); // sigNum(0) - // rpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); - // Fin code OK - - int ltargetPos, rtargetPos; - ltargetPos = rtargetPos = 0; - - int lmpos = lm.getCurrentPosition(); - int rmpos = rm.getCurrentPosition(); - int step = 100; - // double rapp = step/theta; - double signe = Math.signum(-y); - signe = (signe == 0)?1.0:signe; - if(Math.abs(x)<=0.1 && Math.abs(y)<=0.1){ - ltargetPos = rtargetPos = 0; - }else if (Math.abs(x)<=0.1){ - ltargetPos = rtargetPos = 100; - }else if (isBetween(x,0.1,0.9)){ - rtargetPos = step; - ltargetPos = 2*step; - }else if (x>=0.9){ - ltargetPos = step; - rtargetPos = -step; - }else if (x<=-0.9){ - ltargetPos = -step; - rtargetPos = step; - }else if (isBetween(x, -0.9,-0.1)){ - rtargetPos = 2*step; - ltargetPos = step; - } - ltargetPos*=signe; - rtargetPos*=signe; - - // if (x>0.1){ - // theta = Math.abs(Math.PI/2 - Math.atan2(-y,x)); - // theta = (theta > Math.PI/2)?(Math.PI - theta):theta; - // ltargetPos = (int) (Math.floor(theta*37.0 + step)*signe); - // rtargetPos = (int) (Math.floor(step)*signe); - // } - // else if (x < -0.1){ - // theta = Math.abs(Math.abs(Math.atan2(-y,x)) - Math.PI/2); - // rtargetPos = (int) (Math.floor(theta*37.0 + step)*signe); - // ltargetPos = (int) (Math.floor(step)*signe); - // } - // else { - // rtargetPos = step*((int) Math.signum(-y)); - // ltargetPos = step*((int) Math.signum(-y)); - // } - - telemetry.addData("ltargetPos avant cut",ltargetPos); - telemetry.addData("rtargetPos avant cut",rtargetPos); - - // ltargetPos = ((Math.abs(ltargetPos)> step)?(int) Math.signum(ltargetPos)*(step + Math.abs(ltargetPos)%step ):ltargetPos); - // rtargetPos = ((Math.abs(rtargetPos)> step)?(int) Math.signum(rtargetPos)*(step + Math.abs(rtargetPos)%step):rtargetPos); - - lm.setTargetPosition(lmpos + ltargetPos); - rm.setTargetPosition(rmpos + rtargetPos); - lm.setVelocity(2800.0*t2); - rm.setVelocity(2800.0*t2); - - - // telemetry.addData("rapp",rapp); - telemetry.addData("ltargetPos",ltargetPos); - telemetry.addData("rtargetPos",rtargetPos); - - lm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rm.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - - - break; + telemetry.addData("Status", "Initialized"); + telemetry.addData("Version", "5.123"); - case ELINA: - a = (-y + x) / Math.pow(2, 1 / 2); - b = (-y - x) / Math.pow(2, 1 / 2); - vmean = (Math.abs(a) + Math.abs(b)) / 2; - lpower = (a / vmean) * t3; - rpower = (b / vmean) * t3; - break; + lm = hardwareMap.get(DcMotorEx.class, "blm"); - } + rm = hardwareMap.get(DcMotorEx.class, "brm"); + rm.setDirection(DcMotor.Direction.REVERSE); - if (gamepad1.left_trigger > 0.1) { - lpower /= 3; - rpower /= 3; - } + lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - // CODE OK - // lm.setPower(lpower/1.5); - // rm.setPower(rpower/1.5); - // Fin code OK + moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); + moissoneuse.setDirection(DcMotor.Direction.REVERSE); - // activation moissonneuse -- actif en manuel et auto - if (gamepad1.b && !already_b) { - double moissoneuseSpeed = 1.0; - if (gamepad1.right_bumper) { - moissoneuseSpeed = -1.0; - } - already_b = !already_b; - if (moissoneuse.getPower() == moissoneuseSpeed) { - moissoneuse.setPower(0); - } else { - moissoneuse.setPower(moissoneuseSpeed); - } - } - if (!gamepad1.b && already_b) { - already_b = false; - } + lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); + lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - // activation elevateur - if (sinking && Math.abs(lmelevator.getCurrentPosition() - 90) <= 5 - && Math.abs(rmelevator.getCurrentPosition() - 90) <= 5) { - lmelevator.setVelocity(100); - rmelevator.setVelocity(100); - lmelevator.setTargetPosition(0); - rmelevator.setTargetPosition(0); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - if ((gamepad1.dpad_up && !already_up) ^ (gamepad1.dpad_down && !already_down)) { - lmelevator.setVelocity(600); - rmelevator.setVelocity(600); - Long targetPosLong = (Long) Math.round(288 * 3.4); - int targetPos = targetPosLong.intValue(); - if (gamepad1.dpad_down) { - targetPos = 90; - already_down = true; - sinking = true; - } else { - already_up = true; - sinking = false; - } - lmelevator.setTargetPosition(targetPos); - rmelevator.setTargetPosition(targetPos); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); + rmelevator.setDirection(DcMotor.Direction.REVERSE); + rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - } else if (!gamepad1.dpad_up && already_up) { - already_up = false; - } else if (!gamepad1.dpad_down && already_down) { - already_down = false; - } + rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); + rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - if (gamepad1.ps && !already_ps) { - manualMode = !manualMode; - already_ps = true; - } else if (!gamepad1.ps && already_ps) { - already_ps = false; - } + box = hardwareMap.get(DcMotorEx.class, "boxRot"); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - // activation rotation - if (manualMode) { - gamepad1.setLedColor(1.0, 0.0, 0.0,255); - // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - box.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - telemetry.addData("mode","MANUEL"); - // Elevator manual mode - if (gamepad1.dpad_up) { - // rmelevator.setPower(0.3); - // lmelevator.setPower(0.3); - lmelevator.setVelocity(400); - rmelevator.setVelocity(400); - int lpos = rmelevator.getCurrentPosition(); - int rpos = lmelevator.getCurrentPosition(); - lmelevator.setTargetPosition(lpos + 15); - rmelevator.setTargetPosition(rpos + 15); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } else if (gamepad1.dpad_down) { - // lmelevator.setPower(-0.3); - // rmelevator.setPower(-0.3); - lmelevator.setVelocity(400); - rmelevator.setVelocity(400); - int lpos = rmelevator.getCurrentPosition(); - int rpos = lmelevator.getCurrentPosition(); - lmelevator.setTargetPosition(lpos - 15); - rmelevator.setTargetPosition(rpos - 15); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } else if (gamepad1.y) { - double power = -0.3; - if (gamepad1.right_bumper) { - power = -power; - } - rotation.setPower(power); - } else { - // lmelevator.setPower(0); - // rmelevator.setPower(0); - rotation.setPower(0); - // lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - // rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - } - // Box manual mode - if (gamepad1.dpad_left) { - box.setPower(0.3); - box.setPower(0.3); - } else if (gamepad1.dpad_right) { - box.setPower(-0.3); - box.setPower(-0.3); - } - else { - box.setPower(0); - box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - } - // Accrochage final - if (gamepad1.x){ - // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - lmelevator.setVelocity(600); - rmelevator.setVelocity(600); - lmelevator.setTargetPosition(40); - rmelevator.setTargetPosition(40); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } + avion = hardwareMap.get(Servo.class, "avion"); + // box.setPositionPIDFCoefficients(5.0); - } else { - gamepad1.setLedColor(0.0, 0.0, 0.0,10); - telemetry.addData("mode","AUTOMATIQUE"); - - if (!gamepad1.dpad_right && already_padright) { - already_padright = false; - } - if (gamepad1.dpad_right && !already_padright) { - already_padright = true; - // POSITION INITIALE - rotation.setVelocity(600); - rotation.setTargetPosition(0); - - lmelevator.setVelocity(600); - lmelevator.setTargetPosition(0); - rmelevator.setVelocity(600); - rmelevator.setTargetPosition(0); - - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (!gamepad1.dpad_left && already_padleft) { - already_padleft = false; - } - if (gamepad1.dpad_left && !already_padleft) { - already_padleft = true; - // POSITION ROULAGE / Rammase Pixel dans boite - rotation.setVelocity(600); - rotation.setTargetPosition(-50); - - lmelevator.setVelocity(600); - lmelevator.setTargetPosition(150); - rmelevator.setVelocity(600); - rmelevator.setTargetPosition(150); - - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - + // rotation positions: 20° pos initiale par rapport au sol + // while (runtime.seconds()<0.5){ + // rotation.setPower(0.5); + // } - if (!gamepad1.dpad_up && already_padup) { - already_y = false; - } - if (gamepad1.dpad_up && !already_padup) { - already_padup = true; - // POSITION CHASSE-NEIGE - rotation.setVelocity(600); - rotation.setTargetPosition(110); - - lmelevator.setVelocity(600); - lmelevator.setTargetPosition(555); - rmelevator.setVelocity(600); - rmelevator.setTargetPosition(555); - - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - - // int pos = rotation.getCurrentPosition(); - // rotation.setVelocity(200); - // if (gamepad1.right_bumper) { - // // rotation.setTargetPosition(pos - 25); - // rotation.setTargetPosition(-100); // vertical si pos origine = 0 - // } else if (gamepad1.left_bumper) { - // // rotation.setTargetPosition(pos + 25); - // rotation.setTargetPosition(1000); // position basse - // } else { - // rotation.setTargetPosition(0); - // } - // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - if (!gamepad1.dpad_down && already_paddown) { - already_a = false; - } - if (gamepad1.dpad_down && !already_paddown) { - already_a = true; - // POSITION BASSE - rotation.setVelocity(600); - rotation.setTargetPosition(800); - - lmelevator.setVelocity(600); - lmelevator.setTargetPosition(0); - rmelevator.setVelocity(600); - rmelevator.setTargetPosition(0); - - rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - } + telemetry.addData("Mode", "waiting for start"); - + // rotation.setVelocity(1700); + // rotation.setTargetPosition(-1000); + // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - //telemetry.addData("x", x); - //telemetry.addData("y", y); - //telemetry.addData("mode", mode); - //telemetry.addData("lpow", lpower); - //telemetry.addData("rpow", rpower); - //telemetry.addData("ltrigg", t); - //telemetry.addData("t2", t2); - //telemetry.addData("rotation power", boxRot); - //telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition()); - //telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition()); - //telemetry.addData("Position rotation", rotation.getCurrentPosition()); - //telemetry.addData("Position box", box.getCurrentPosition()); - //telemetry.addData("box velocity", rotation.getVelocity()); - //telemetry.update(); - } + telemetry.addData("rotation target Pos", rotation.getTargetPosition()); + telemetry.addData("rotation Pos", rotation.getCurrentPosition()); + telemetry.update(); + + waitForStart(); + rotation.setVelocity(200); + rotation.setTargetPosition(0); + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + while (opModeIsActive()) { + + x = gamepad1.left_stick_x; + y = gamepad1.left_stick_y; + + /* définition de {@link t} sur la valeur du trigger droit du gamepad 1 */ + t = gamepad1.right_trigger; + + /* + * définition de {@link t2} par utilisation de la fonction {@link helloexp} sur + * {@link t} + */ + t2 = helloexp(t); + + /* + * définition de {@link t3} par utilisation de la fonction {@link helloexp} sur + * la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link + * x} au carré plus {@link y} au carré) + */ + t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2))); + + telemetry.addData("Status", "Running"); + + + if (gamepad1.right_bumper && gamepad1.left_bumper){ + avion.setDirection(Servo.Direction.REVERSE); + // telemetry.addData("servoPos",avion.getPosition()); + // avion.setPosition(0); + avion.setPosition(0.75); + // continue; + } + // Choix mode conduite / actif en manuel et auto + if (gamepad1.a && !already_a) { + nextMode(); + already_a = true; + } + if (!gamepad1.a && already_a) { + already_a = false; + } + double lpower = 0.0; + double rpower = 0.0; + double vmean; + double a; + double b; + + switch (mode) { + + case NORMAL: + double ysign = Math.signum(y); + double xsign = Math.signum(x); + lpower = ysign * t + (-xsign - 2 * x) * t; + rpower = ysign * t + (xsign - 2 * x) * t; + break; + + + case TANK: + lpower = -y; + rpower = -gamepad1.right_stick_y; + break; + + case ESSAIFRANCK: + // Code ci-dessous OK + // lpower = (1 + x); //(a / vmean) * t2; + // rpower = 2-lpower;//(b / vmean) * t2; + // lpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); // sigNum(0) + // rpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); + // Fin code OK + + int ltargetPos, rtargetPos; + ltargetPos = rtargetPos = 0; + + int lmpos = lm.getCurrentPosition(); + int rmpos = rm.getCurrentPosition(); + int step = 100; + // double rapp = step/theta; + double signe = Math.signum(-y); + signe = (signe == 0)?1.0:signe; + if(Math.abs(x)<=0.1 && Math.abs(y)<=0.1){ + ltargetPos = rtargetPos = 0; + }else if (Math.abs(x)<=0.1){ + ltargetPos = rtargetPos = 100; + }else if (isBetween(x,0.1,0.9)){ + rtargetPos = step; + ltargetPos = 2*step; + }else if (x>=0.9){ + ltargetPos = step; + rtargetPos = -step; + }else if (x<=-0.9){ + ltargetPos = -step; + rtargetPos = step; + }else if (isBetween(x, -0.9,-0.1)){ + rtargetPos = 2*step; + ltargetPos = step; + } + ltargetPos*=signe; + rtargetPos*=signe; + + // if (x>0.1){ + // theta = Math.abs(Math.PI/2 - Math.atan2(-y,x)); + // theta = (theta > Math.PI/2)?(Math.PI - theta):theta; + // ltargetPos = (int) (Math.floor(theta*37.0 + step)*signe); + // rtargetPos = (int) (Math.floor(step)*signe); + // } + // else if (x < -0.1){ + // theta = Math.abs(Math.abs(Math.atan2(-y,x)) - Math.PI/2); + // rtargetPos = (int) (Math.floor(theta*37.0 + step)*signe); + // ltargetPos = (int) (Math.floor(step)*signe); + // } + // else { + // rtargetPos = step*((int) Math.signum(-y)); + // ltargetPos = step*((int) Math.signum(-y)); + // } + + telemetry.addData("ltargetPos avant cut",ltargetPos); + telemetry.addData("rtargetPos avant cut",rtargetPos); + + // ltargetPos = ((Math.abs(ltargetPos)> step)?(int) Math.signum(ltargetPos)*(step + Math.abs(ltargetPos)%step ):ltargetPos); + // rtargetPos = ((Math.abs(rtargetPos)> step)?(int) Math.signum(rtargetPos)*(step + Math.abs(rtargetPos)%step):rtargetPos); + + lm.setTargetPosition(lmpos + ltargetPos); + rm.setTargetPosition(rmpos + rtargetPos); + lm.setVelocity(2800.0*t2); + rm.setVelocity(2800.0*t2); + + + // telemetry.addData("rapp",rapp); + telemetry.addData("ltargetPos",ltargetPos); + telemetry.addData("rtargetPos",rtargetPos); + + lm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + + + break; + + case ELINA: + a = (-y + x) / Math.pow(2, 1 / 2); + b = (-y - x) / Math.pow(2, 1 / 2); + vmean = (Math.abs(a) + Math.abs(b)) / 2; + lpower = (a / vmean) * t3; + rpower = (b / vmean) * t3; + break; + + } + + if (gamepad1.left_trigger > 0.1) { + lpower /= 3; + rpower /= 3; + } + + // CODE OK + // lm.setPower(lpower/1.5); + // rm.setPower(rpower/1.5); + // Fin code OK + + // activation moissonneuse -- actif en manuel et auto + if (gamepad1.b && !already_b) { + double moissoneuseSpeed = 1.0; + if (gamepad1.right_bumper) { + moissoneuseSpeed = -1.0; + } + already_b = !already_b; + if (moissoneuse.getPower() == moissoneuseSpeed) { + moissoneuse.setPower(0); + } else { + moissoneuse.setPower(moissoneuseSpeed); + } + } + if (!gamepad1.b && already_b) { + already_b = false; + } + + // activation elevateur + if (sinking && Math.abs(lmelevator.getCurrentPosition() - 90) <= 5 + && Math.abs(rmelevator.getCurrentPosition() - 90) <= 5) { + lmelevator.setVelocity(100); + rmelevator.setVelocity(100); + lmelevator.setTargetPosition(0); + rmelevator.setTargetPosition(0); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + if ((gamepad1.dpad_up && !already_up) ^ (gamepad1.dpad_down && !already_down)) { + lmelevator.setVelocity(600); + rmelevator.setVelocity(600); + Long targetPosLong = (Long) Math.round(288 * 3.4); + int targetPos = targetPosLong.intValue(); + if (gamepad1.dpad_down) { + targetPos = 90; + already_down = true; + sinking = true; + } else { + already_up = true; + sinking = false; + } + lmelevator.setTargetPosition(targetPos); + rmelevator.setTargetPosition(targetPos); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + } else if (!gamepad1.dpad_up && already_up) { + already_up = false; + } else if (!gamepad1.dpad_down && already_down) { + already_down = false; + } + + if (gamepad1.ps && !already_ps) { + manualMode = !manualMode; + already_ps = true; + } else if (!gamepad1.ps && already_ps) { + already_ps = false; + } + + // activation rotation + if (manualMode) { + gamepad1.setLedColor(1.0, 0.0, 0.0,255); + // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + box.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + telemetry.addData("mode","MANUEL"); + // Elevator manual mode + if (gamepad1.dpad_up) { + // rmelevator.setPower(0.3); + // lmelevator.setPower(0.3); + lmelevator.setVelocity(400); + rmelevator.setVelocity(400); + int lpos = rmelevator.getCurrentPosition(); + int rpos = lmelevator.getCurrentPosition(); + lmelevator.setTargetPosition(lpos + 15); + rmelevator.setTargetPosition(rpos + 15); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } else if (gamepad1.dpad_down) { + // lmelevator.setPower(-0.3); + // rmelevator.setPower(-0.3); + lmelevator.setVelocity(400); + rmelevator.setVelocity(400); + int lpos = rmelevator.getCurrentPosition(); + int rpos = lmelevator.getCurrentPosition(); + lmelevator.setTargetPosition(lpos - 15); + rmelevator.setTargetPosition(rpos - 15); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } else if (gamepad1.y) { + double power = -0.3; + if (gamepad1.right_bumper) { + power = -power; + } + rotation.setPower(power); + } else { + // lmelevator.setPower(0); + // rmelevator.setPower(0); + rotation.setPower(0); + // lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + // rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + // Box manual mode + if (gamepad1.dpad_left) { + box.setPower(0.3); + box.setPower(0.3); + } else if (gamepad1.dpad_right) { + box.setPower(-0.3); + box.setPower(-0.3); + } + else { + box.setPower(0); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + // Accrochage final + if (gamepad1.x){ + // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + lmelevator.setVelocity(600); + rmelevator.setVelocity(600); + lmelevator.setTargetPosition(40); + rmelevator.setTargetPosition(40); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + } else { + gamepad1.setLedColor(0.0, 0.0, 0.0,10); + telemetry.addData("mode","AUTOMATIQUE"); + + if (!gamepad1.dpad_right && already_padright) { + already_padright = false; + } + if (gamepad1.dpad_right && !already_padright) { + already_padright = true; + // POSITION INITIALE + rotation.setVelocity(600); + rotation.setTargetPosition(0); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(0); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(0); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (!gamepad1.dpad_left && already_padleft) { + already_padleft = false; + } + if (gamepad1.dpad_left && !already_padleft) { + already_padleft = true; + // POSITION ROULAGE / Rammase Pixel dans boite + rotation.setVelocity(600); + rotation.setTargetPosition(-50); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(150); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(150); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + + if (!gamepad1.dpad_up && already_padup) { + already_y = false; + } + if (gamepad1.dpad_up && !already_padup) { + already_padup = true; + // POSITION CHASSE-NEIGE + rotation.setVelocity(600); + rotation.setTargetPosition(110); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(555); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(555); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + + // int pos = rotation.getCurrentPosition(); + // rotation.setVelocity(200); + // if (gamepad1.right_bumper) { + // // rotation.setTargetPosition(pos - 25); + // rotation.setTargetPosition(-100); // vertical si pos origine = 0 + // } else if (gamepad1.left_bumper) { + // // rotation.setTargetPosition(pos + 25); + // rotation.setTargetPosition(1000); // position basse + // } else { + // rotation.setTargetPosition(0); + // } + // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (!gamepad1.dpad_down && already_paddown) { + already_a = false; + } + if (gamepad1.dpad_down && !already_paddown) { + already_a = true; + // POSITION BASSE + rotation.setVelocity(600); + rotation.setTargetPosition(800); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(0); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(0); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + } + + + + //telemetry.addData("x", x); + //telemetry.addData("y", y); + //telemetry.addData("mode", mode); + //telemetry.addData("lpow", lpower); + //telemetry.addData("rpow", rpower); + //telemetry.addData("ltrigg", t); + //telemetry.addData("t2", t2); + //telemetry.addData("rotation power", boxRot); + //telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition()); + //telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition()); + //telemetry.addData("Position rotation", rotation.getCurrentPosition()); + //telemetry.addData("Position box", box.getCurrentPosition()); + //telemetry.addData("box velocity", rotation.getVelocity()); + //telemetry.update(); + } } -} \ No newline at end of file +} diff --git a/ftc_new.java~ b/ftc_new.java~ new file mode 100644 index 0000000..25b0002 --- /dev/null +++ b/ftc_new.java~ @@ -0,0 +1,563 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import com.qualcomm.robotcore.robot.Robot; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.robot.Robot; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; + +@TeleOp(name = "WeRobot: FTC2024 NEW! Carlike", group = "WeRobot") +public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { + public enum RobotMode { + ESSAIFRANCK, ELINA, NORMAL, TANK; + } + + private DcMotorEx rm; + private DcMotorEx lm; + private DcMotor moissoneuse; + private DcMotorEx lmelevator; + private DcMotorEx rmelevator; + private DcMotorEx box; + private DcMotorEx rotation; + private Servo avion; + private ElapsedTime runtime = new ElapsedTime(); + private RobotMode mode = RobotMode.ESSAIFRANCK; + + /* + * La fonction pour faire des exponentielles spécifiques + * + * @param double t => le nombre dont on veut faire l'exponentielle + * + * @return double une_exponentielle_très_spéciale_de_t + */ + private double helloexp(double t) { + return (Math.exp(5 * t) - 1) / (Math.exp(5) - 1); + } + + public void nextMode() { + RobotMode toNextMode; + switch (this.mode) { + case ESSAIFRANCK: + toNextMode = RobotMode.ELINA; + break; + case ELINA: + toNextMode = RobotMode.NORMAL; + break; + case NORMAL: + toNextMode = RobotMode.TANK; + break; + default: + toNextMode = RobotMode.ESSAIFRANCK; + break; + } + this.mode = toNextMode; + } + public boolean isBetween(double elem, double mini, double maxi){ + return Math.abs(elem - (((maxi-mini)/2)+mini))<=(maxi-mini)/2; + } + // La fonction du thread principal + @Override + public void runOpMode() throws InterruptedException { + + double boxRot = 0; + int signeBR; + + float x; + double y; + + double t; + double t2; + double t3; + + boolean already_b = false; + boolean already_a = false; + boolean already_x = false; + boolean already_y = false; + boolean already_up = false; + boolean already_down = false; + boolean already_ps = false; + + boolean already_paddown = false; + boolean already_padup = false; + boolean already_padright = false; + boolean already_padleft = false; + + + boolean sinking = false; + boolean manualMode = false; + boolean firstLaunch = true; + + telemetry.addData("Status", "Initialized"); + telemetry.addData("Version", "5.123"); + + lm = hardwareMap.get(DcMotorEx.class, "blm"); + + rm = hardwareMap.get(DcMotorEx.class, "brm"); + rm.setDirection(DcMotor.Direction.REVERSE); + + lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse"); + moissoneuse.setDirection(DcMotor.Direction.REVERSE); + + lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); + lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); + rmelevator.setDirection(DcMotor.Direction.REVERSE); + rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); + rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + box = hardwareMap.get(DcMotorEx.class, "boxRot"); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + box.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + avion = hardwareMap.get(Servo.class, "avion"); + // box.setPositionPIDFCoefficients(5.0); + + // rotation positions: 20° pos initiale par rapport au sol + // while (runtime.seconds()<0.5){ + // rotation.setPower(0.5); + // } + + telemetry.addData("Mode", "waiting for start"); + + // rotation.setVelocity(1700); + // rotation.setTargetPosition(-1000); + // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + telemetry.addData("rotation target Pos", rotation.getTargetPosition()); + telemetry.addData("rotation Pos", rotation.getCurrentPosition()); + telemetry.update(); + + waitForStart(); + rotation.setVelocity(200); + rotation.setTargetPosition(0); + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + while (opModeIsActive()) { + + x = gamepad1.left_stick_x; + y = gamepad1.left_stick_y; + + /* définition de {@link t} sur la valeur du trigger droit du gamepad 1 */ + t = gamepad1.right_trigger; + + /* + * définition de {@link t2} par utilisation de la fonction {@link helloexp} sur + * {@link t} + */ + t2 = helloexp(t); + + /* + * définition de {@link t3} par utilisation de la fonction {@link helloexp} sur + * la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link + * x} au carré plus {@link y} au carré) + */ + t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2))); + + telemetry.addData("Status", "Running"); + + + if (gamepad1.right_bumper && gamepad1.left_bumper){ + avion.setDirection(Servo.Direction.REVERSE); + // telemetry.addData("servoPos",avion.getPosition()); + // avion.setPosition(0); + avion.setPosition(0.75); + // continue; + } + // Choix mode conduite / actif en manuel et auto + if (gamepad1.a && !already_a) { + nextMode(); + already_a = true; + } + if (!gamepad1.a && already_a) { + already_a = false; + } + double lpower = 0.0; + double rpower = 0.0; + double vmean; + double a; + double b; + + switch (mode) { + + case NORMAL: + double ysign = Math.signum(y); + double xsign = Math.signum(x); + lpower = ysign * t + (-xsign - 2 * x) * t; + rpower = ysign * t + (xsign - 2 * x) * t; + break; + + + case TANK: + lpower = -y; + rpower = -gamepad1.right_stick_y; + break; + + case ESSAIFRANCK: + // Code ci-dessous OK + // lpower = (1 + x); //(a / vmean) * t2; + // rpower = 2-lpower;//(b / vmean) * t2; + // lpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); // sigNum(0) + // rpower*=t2*((Math.signum(y)==0)?1:-Math.signum(y)); + // Fin code OK + + int ltargetPos, rtargetPos; + ltargetPos = rtargetPos = 0; + + int lmpos = lm.getCurrentPosition(); + int rmpos = rm.getCurrentPosition(); + int step = 100; + // double rapp = step/theta; + double signe = Math.signum(-y); + signe = (signe == 0)?1.0:signe; + if(Math.abs(x)<=0.1 && Math.abs(y)<=0.1){ + ltargetPos = rtargetPos = 0; + }else if (Math.abs(x)<=0.1){ + ltargetPos = rtargetPos = 100; + }else if (isBetween(x,0.1,0.9)){ + rtargetPos = step; + ltargetPos = 2*step; + }else if (x>=0.9){ + ltargetPos = step; + rtargetPos = -step; + }else if (x<=-0.9){ + ltargetPos = -step; + rtargetPos = step; + }else if (isBetween(x, -0.9,-0.1)){ + rtargetPos = 2*step; + ltargetPos = step; + } + ltargetPos*=signe; + rtargetPos*=signe; + + // if (x>0.1){ + // theta = Math.abs(Math.PI/2 - Math.atan2(-y,x)); + // theta = (theta > Math.PI/2)?(Math.PI - theta):theta; + // ltargetPos = (int) (Math.floor(theta*37.0 + step)*signe); + // rtargetPos = (int) (Math.floor(step)*signe); + // } + // else if (x < -0.1){ + // theta = Math.abs(Math.abs(Math.atan2(-y,x)) - Math.PI/2); + // rtargetPos = (int) (Math.floor(theta*37.0 + step)*signe); + // ltargetPos = (int) (Math.floor(step)*signe); + // } + // else { + // rtargetPos = step*((int) Math.signum(-y)); + // ltargetPos = step*((int) Math.signum(-y)); + // } + + telemetry.addData("ltargetPos avant cut",ltargetPos); + telemetry.addData("rtargetPos avant cut",rtargetPos); + + // ltargetPos = ((Math.abs(ltargetPos)> step)?(int) Math.signum(ltargetPos)*(step + Math.abs(ltargetPos)%step ):ltargetPos); + // rtargetPos = ((Math.abs(rtargetPos)> step)?(int) Math.signum(rtargetPos)*(step + Math.abs(rtargetPos)%step):rtargetPos); + + lm.setTargetPosition(lmpos + ltargetPos); + rm.setTargetPosition(rmpos + rtargetPos); + lm.setVelocity(2800.0*t2); + rm.setVelocity(2800.0*t2); + + + // telemetry.addData("rapp",rapp); + telemetry.addData("ltargetPos",ltargetPos); + telemetry.addData("rtargetPos",rtargetPos); + + lm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + + + break; + + case ELINA: + a = (-y + x) / Math.pow(2, 1 / 2); + b = (-y - x) / Math.pow(2, 1 / 2); + vmean = (Math.abs(a) + Math.abs(b)) / 2; + lpower = (a / vmean) * t3; + rpower = (b / vmean) * t3; + break; + + } + + if (gamepad1.left_trigger > 0.1) { + lpower /= 3; + rpower /= 3; + } + + // CODE OK + // lm.setPower(lpower/1.5); + // rm.setPower(rpower/1.5); + // Fin code OK + + // activation moissonneuse -- actif en manuel et auto + if (gamepad1.b && !already_b) { + double moissoneuseSpeed = 1.0; + if (gamepad1.right_bumper) { + moissoneuseSpeed = -1.0; + } + already_b = !already_b; + if (moissoneuse.getPower() == moissoneuseSpeed) { + moissoneuse.setPower(0); + } else { + moissoneuse.setPower(moissoneuseSpeed); + } + } + if (!gamepad1.b && already_b) { + already_b = false; + } + + // activation elevateur + if (sinking && Math.abs(lmelevator.getCurrentPosition() - 90) <= 5 + && Math.abs(rmelevator.getCurrentPosition() - 90) <= 5) { + lmelevator.setVelocity(100); + rmelevator.setVelocity(100); + lmelevator.setTargetPosition(0); + rmelevator.setTargetPosition(0); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + if ((gamepad1.dpad_up && !already_up) ^ (gamepad1.dpad_down && !already_down)) { + lmelevator.setVelocity(600); + rmelevator.setVelocity(600); + Long targetPosLong = (Long) Math.round(288 * 3.4); + int targetPos = targetPosLong.intValue(); + if (gamepad1.dpad_down) { + targetPos = 90; + already_down = true; + sinking = true; + } else { + already_up = true; + sinking = false; + } + lmelevator.setTargetPosition(targetPos); + rmelevator.setTargetPosition(targetPos); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + } else if (!gamepad1.dpad_up && already_up) { + already_up = false; + } else if (!gamepad1.dpad_down && already_down) { + already_down = false; + } + + if (gamepad1.ps && !already_ps) { + manualMode = !manualMode; + already_ps = true; + } else if (!gamepad1.ps && already_ps) { + already_ps = false; + } + + // activation rotation + if (manualMode) { + gamepad1.setLedColor(1.0, 0.0, 0.0,255); + // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + box.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + telemetry.addData("mode","MANUEL"); + // Elevator manual mode + if (gamepad1.dpad_up) { + // rmelevator.setPower(0.3); + // lmelevator.setPower(0.3); + lmelevator.setVelocity(400); + rmelevator.setVelocity(400); + int lpos = rmelevator.getCurrentPosition(); + int rpos = lmelevator.getCurrentPosition(); + lmelevator.setTargetPosition(lpos + 15); + rmelevator.setTargetPosition(rpos + 15); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } else if (gamepad1.dpad_down) { + // lmelevator.setPower(-0.3); + // rmelevator.setPower(-0.3); + lmelevator.setVelocity(400); + rmelevator.setVelocity(400); + int lpos = rmelevator.getCurrentPosition(); + int rpos = lmelevator.getCurrentPosition(); + lmelevator.setTargetPosition(lpos - 15); + rmelevator.setTargetPosition(rpos - 15); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } else if (gamepad1.y) { + double power = -0.3; + if (gamepad1.right_bumper) { + power = -power; + } + rotation.setPower(power); + } else { + // lmelevator.setPower(0); + // rmelevator.setPower(0); + rotation.setPower(0); + // lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + // rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + // Box manual mode + if (gamepad1.dpad_left) { + box.setPower(0.3); + box.setPower(0.3); + } else if (gamepad1.dpad_right) { + box.setPower(-0.3); + box.setPower(-0.3); + } + else { + box.setPower(0); + box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + } + // Accrochage final + if (gamepad1.x){ + // lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + lmelevator.setVelocity(600); + rmelevator.setVelocity(600); + lmelevator.setTargetPosition(40); + rmelevator.setTargetPosition(40); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + } else { + gamepad1.setLedColor(0.0, 0.0, 0.0,10); + telemetry.addData("mode","AUTOMATIQUE"); + + if (!gamepad1.dpad_right && already_padright) { + already_padright = false; + } + if (gamepad1.dpad_right && !already_padright) { + already_padright = true; + // POSITION INITIALE + rotation.setVelocity(600); + rotation.setTargetPosition(0); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(0); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(0); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (!gamepad1.dpad_left && already_padleft) { + already_padleft = false; + } + if (gamepad1.dpad_left && !already_padleft) { + already_padleft = true; + // POSITION ROULAGE / Rammase Pixel dans boite + rotation.setVelocity(600); + rotation.setTargetPosition(-50); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(150); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(150); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + + if (!gamepad1.dpad_up && already_padup) { + already_y = false; + } + if (gamepad1.dpad_up && !already_padup) { + already_padup = true; + // POSITION CHASSE-NEIGE + rotation.setVelocity(600); + rotation.setTargetPosition(110); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(555); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(555); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + + // int pos = rotation.getCurrentPosition(); + // rotation.setVelocity(200); + // if (gamepad1.right_bumper) { + // // rotation.setTargetPosition(pos - 25); + // rotation.setTargetPosition(-100); // vertical si pos origine = 0 + // } else if (gamepad1.left_bumper) { + // // rotation.setTargetPosition(pos + 25); + // rotation.setTargetPosition(1000); // position basse + // } else { + // rotation.setTargetPosition(0); + // } + // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + + if (!gamepad1.dpad_down && already_paddown) { + already_a = false; + } + if (gamepad1.dpad_down && !already_paddown) { + already_a = true; + // POSITION BASSE + rotation.setVelocity(600); + rotation.setTargetPosition(800); + + lmelevator.setVelocity(600); + lmelevator.setTargetPosition(0); + rmelevator.setVelocity(600); + rmelevator.setTargetPosition(0); + + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + } + + + + //telemetry.addData("x", x); + //telemetry.addData("y", y); + //telemetry.addData("mode", mode); + //telemetry.addData("lpow", lpower); + //telemetry.addData("rpow", rpower); + //telemetry.addData("ltrigg", t); + //telemetry.addData("t2", t2); + //telemetry.addData("rotation power", boxRot); + //telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition()); + //telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition()); + //telemetry.addData("Position rotation", rotation.getCurrentPosition()); + //telemetry.addData("Position box", box.getCurrentPosition()); + //telemetry.addData("box velocity", rotation.getVelocity()); + //telemetry.update(); + } + } + +} \ No newline at end of file