From 5aa3b7dba9563c770b308b05d4c0c106b089ef40 Mon Sep 17 00:00:00 2001 From: tazule Date: Sun, 15 Oct 2017 15:34:49 -0700 Subject: [PATCH 01/38] Hope --- TeamCode/build.gradle | 6 +- .../ftc/teamcode/BasicOpMode_Linear.java | 108 ++++++++++++++++++ .../ftc/teamcode/LITERALY_JUST_THE_ARM.java | 30 +++++ libs/Jama-1.0.3.jar | Bin 0 -> 35981 bytes 4 files changed, 141 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LITERALY_JUST_THE_ARM.java create mode 100644 libs/Jama-1.0.3.jar diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 301ff649463..d88d17bd1aa 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -7,9 +7,9 @@ // the build definitions, you can place those customizations in this file, but // please think carefully as to whether such customizations are really necessary // before doing so. - - // Custom definitions may go here - // Include common definitions from above. apply from: '../build.common.gradle' +dependencies { + compile files('C:/Users/jxfio/OneDrive/Documents/GitHub/ftc_app/libs/Jama-1.0.3.jar') +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java new file mode 100644 index 00000000000..ccde802702f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java @@ -0,0 +1,108 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +import Jama.Matrix; +import java.lang.Math; + +import static java.lang.Math.cos; +import static java.lang.Math.sin; + +@TeleOp(name="Drivebase", group="Linear Opmode") +public class BasicOpMode_Linear extends LinearOpMode { + + // Declare OpMode members. + ElapsedTime timer = new ElapsedTime(); + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor ADrive; + private DcMotor BDrive; + private DcMotor CDrive; + private IntegratingGyroscope gyro; + private ModernRoboticsI2cGyro MRI2CGyro; + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + ADrive = hardwareMap.get(DcMotor.class, "A"); + BDrive = hardwareMap.get(DcMotor.class, "B"); + CDrive = hardwareMap.get(DcMotor.class, "C"); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + MRI2CGyro.calibrate(); + + // Wait until the gyro calibration is complete + timer.reset(); + while (!isStopRequested() && MRI2CGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + waitForStart(); + runtime.reset(); + telemetry.log().add("Press A & B to reset heading"); + // run until the end of the match (driver presses STOP) + boolean curResState = false; + boolean LastResState = false; + double temp; + double previoustime = getRuntime(); + double theta = 0; + double θpreverr = 0; + double θkp = .02; + double θki = .0000001; + double θkd = .00001; + while (opModeIsActive()) { + curResState = gamepad1.a && gamepad1.b; + if(LastResState && !curResState){ + MRI2CGyro.resetZAxisIntegrator(); + } + LastResState = curResState; + double APwr; + double BPwr; + double CPwr; + double angle = MRI2CGyro.getIntegratedZValue(); + double drivey = -gamepad1.left_stick_y; + double drivex = gamepad1.left_stick_x; + double turn = gamepad1.right_stick_x; + double time = getRuntime(); + theta += .18*.5*(time-previoustime)*turn; + double θerr = angle-theta; + double θderiv = (θerr-θpreverr)/(time-previoustime); + double θinteg = .5*(time-previoustime)*(θerr+θpreverr); + double θmotor = θkp*θerr+θki*θinteg+θkd*θderiv; + APwr= drivex*cos((angle)*Math.PI/180)+drivey*sin((angle)*Math.PI/180)+θmotor; + BPwr= drivex*cos((120+angle)*Math.PI/180)+drivey*sin((120+angle)*Math.PI/180)+θmotor; + CPwr= drivex*cos((angle-120)*Math.PI/180)+drivey*sin((angle-120)*Math.PI/180)+θmotor; + if(Math.max(Math.abs(APwr), Math.max(Math.abs(BPwr), Math.abs(CPwr)))>1) { + temp = 1 / Math.max(Math.abs(APwr), Math.max(Math.abs(BPwr), Math.abs(CPwr))); + }else{ + temp=1; + } + APwr = APwr*temp; + BPwr = BPwr*temp; + CPwr = CPwr*temp; + // Send calculated power to wheels + ADrive.setPower(Range.clip(APwr, -1, 1)); + BDrive.setPower(Range.clip(BPwr, -1, 1)); + CDrive.setPower(Range.clip(CPwr, -1, 1)); + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Data", "A (%.2f), B (%.2f), C (%.2f) angle (%.2f)", APwr, BPwr, CPwr, angle); + telemetry.addData("MEH too lazy to name this", "temp (%.2f) drvey (%.2f) drivey, drivex (%.2f)", temp, drivey, drivex); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LITERALY_JUST_THE_ARM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LITERALY_JUST_THE_ARM.java new file mode 100644 index 00000000000..b8934fba2b3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LITERALY_JUST_THE_ARM.java @@ -0,0 +1,30 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.Range; + +/** + * Created by jxfio on 9/30/2017. + */ +@TeleOp(name="Literaly just the arm", group="Linear Opmode") +public class LITERALY_JUST_THE_ARM extends LinearOpMode { + public DcMotor arm1 = null; + @Override + + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + arm1 = hardwareMap.get(DcMotor.class, "arm"); + waitForStart(); + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + arm1.setPower(Range.clip(gamepad2.left_stick_y,-1,1)); + } + } +} diff --git a/libs/Jama-1.0.3.jar b/libs/Jama-1.0.3.jar new file mode 100644 index 0000000000000000000000000000000000000000..c8007594cb5106ff6e699e1f532dbca046759c91 GIT binary patch literal 35981 zcmaI6V~{Sex9-`tcH8!D+qP}neA~8d+qP}nw(aiSyJ!FB&bc!+b?!+jRjJC8O7i7d zsr6e5(x70dKv4hsr?)DbPMp(wA8cHb4^MN%gp;HPIOYTG?KClE|p-= z=@(eTI2n`{U}>jR?`Gr}Cm8DJX~n1LzuiyCt}Gv&9-M&x?~B-TCW4FqJLW&*|Gmh^ z|L+xq|5l|8Z4K##E$nSfovl4YOpWbr9qgSg0hac5bjCJ@&dx5XvJNPwn7*wWGpBWy zRJxK01_BGsB38s;lkyFuf55c*!78E3r8`4QBwLRm%OQJ)ANC_qNT&0L`j>a?EGK}1J=Cf?;8<)tI>>d zFm@y=p`d&82yuqD{1I^)B*SN@TcO+)M${1X9t!Y;sarkb9}7@~2b0wrbVm@-xXC1j zkafZk%RbajlM?dNS1yM!_x9rFHCd@k^>A6KRhOsRU*ikw7 zB?G;hG})|!;Gz-kR;4|E!u6`s&DmaJrQ(%W(}@a<*J*IPu1bu>PW*kVC5m~~m!6JD zK7)H&y|#>|bX}P{zN;@IUmNi{43^jJ#hc$6t^rkABg!%@Hi8@AcD>oEb;mJyrIcMP zF5A@DD-RRR32jm+YAe-_an^F@WzRrr#3=oC*U$!-B*4!GmO#=-Fa~uRd@-ifui*6r zbrA8Qa)Ew&M8A9l2Bi`z`pVN_zcY*$D9=Xc)h=8+n`t*vp)SAVHAg%vc;-;Ee^2hl{<$$>7%_tUFdcU&RVRG@0Y1p%9shz8#jf?4jfR3w;AkUkK;kpq(p?Iz#-5`K1c|_DTI_@BAm;? zq`?fNL6U@lfrSt2E-5b}qDaB4_|@gVBiC*{4;yxGJ9lXsYxCARFFk_=dM|zL5s;q8 zAa<{AJ3BkODmy=4x*hft_(24ybS{Gg9WuRVQtOt!9U!I?S~?op=6Lb1`QP%&*3qDI z>GjP&4`?{F6V)cvTfAxG>#!qgT;jfl5B@B9$(e4a*}wJLFo2Ww&dw#nek*{z_+kP@ z9ajy2mMHZbMMO=lA%9pU*O4QVlFlf7aE*b^;9EQh{03orP!2${u&~rzSl3wF+*TNh z@A$J!Jl`#;Q*mU@7MNb9mDG`HVWy@oYdiF47AoU(2@l1qb(x7%nKoT(ZD%RLlA}h1 zRZ194PM)cyRh6Y>u1>G1cIHG5o>AA*QQMkKhK}ZpxH&9B>xQ1YIj`b2**FUBGSgIv zL1QRkVP@?j>#g;ov|{#>(fX22HhpfUioWuo5y~V%9k5T8IqWC3Tj-ogChH-rEPSQ0 zT5EBcd^vhquvF=heiloYJJVzYTUBRm`stwfAP5G0Wk-g6ur`q^1}qV$1YN=Kc_ zOyr0bv+@FYggMUfGd2Lc_8yC%5!i5B)()on9e{qavL`%9>MrGf^Yc!q|F0 zB}rgG!-uXebkRW5dI))BoTpawcPe@D37V=3s=7ocgxbiIvah$Z^9xJ_Wp0{IW<&(p z6m}LlE6XZt3uVg2X!@A;FzPhl)-Pt9&jvqKU?E0siis72B~nhq%Ht5l6Na*5R;3e% zGTefFjENA1lZMJ`*s*{SCUL6F1u3REKH*W|_T1xE1j5#c$L&N&Ap0OF4ALv1)|So1 zZ4FayBk?!h30M7vTeD61sAm^;Velc{4yjx< zoGO_`#FhzD3^;nzQ7P54%v!6^E*AAVl|a3#ezzoVIrb-G(FiGJ=TAVSdrS#cXdgva z({^bq%694+;OdGkaP7HOQ?EoJ;UcC9Xlcf@r-T!(QFDQ2Jqg^UWG-}=D3;72Zy zUSa1$vwd+%KhwJNx9ws&=t1U%nM-%5eibz=CU9yGcr0rV3;|^e)dZA8PKuD2fiF!= z@^s1#@U2T4?fUbkRP_R_n^K$+4lBQz6%WR3GlUXn>XjO<{X;s7a+RcpuOd22;54Rc zv7p)W?@I$3J^c-Qlf$?~Qb$O`4Wo`j)<9Dtl41xZ4NGilRVJHA1)5qh@kX_wLm`@5 zcx@@GgrEvhgiF`3wWoVG4Gu)K-M0- zdUD{j3ZdDAS-D4E%`vawD8Swc;Qawb9~`TP>J5ZA4<0!W;CS^XvvA6QJliMSZ3q>F zjXnMDLQXYAqZwnf7^-Ub#jEazmuUi@9ojnZ-bSdpSJa59bNH@vAo7K>zDH??>N0@; z)vOFeS76hgTHgp1DN@Lz%Qvp?1AUV?Br{+SGD`j<{)rDSLuyx?wfoGowt&hbI zz`3AmkJvqY^wE$f7@~dls}JZ6jUfiZ5uslvC9cLClk6gLPK{p*ir~{ ziyC!6*8p>iuI}({_1?1%^A_Iap%#0F)e9ck>0gfm)7c*bNk|CBK23K%BN}a^GvON! zwE+Izu`3krElzn2mZm$U$ZE38XakF){NRlVRuF&mVMA+a&q;l1*;45s^wDQB)Jol>lvgc3 z0U^)!?kd#(fH)%cG7{V1Y@nW7YsJb<#4&Q8>;Yf5Bb+-QqGq9hA z39YeV-nyKLvy4gu`MOjSX?ujLQ`1&6K*h|Rsb9Oc&4^?&I?33GY$Zf*`(xIM6|K!X zlMkvy(s-qng2njaUJ}Ik5{#BGNnw&F1)ADV>bsjA=$fVJ$wZoEXhtfjH1rird1e0G zG{&MVn(JGV$Yu62Z9#?JH4xY|lRKRrSWc%r4n^W2_W2OKy`ne2vyfvgwmIG1ZaF&B z`*2L0!Hb;A%Yf(%mXuUGs*}NnY6*A6hI?OvE?+y6j~ynY{^0xrPvw=A;Mde#RUGqv z+&qQ1jBns)zQy8WgU%c6FN|p*)S}x;F{CFVE}_IqAg?O5IDTyRn_F@v39Sw< zPBrBt`T9(yIxj1>8--&Kgl90}O_8BvI%tej{|n{J%Fis$JCM1Sf)k&A>H&Z;wsgjE z$v3C?2Cf0>7cjN&(jIAiy>hRv0ck%(9@`Sv-!=V!uiS%3KNMCE>FUh!4hgd!WcVIl zbfZUe;DqTNO>&@&3!8WFaL<_wq;CN39nrME=g#FF;(Wlbfv9hc+dBZ}9YuSu-wo6| zBz@o&JI;TFp~kK)$Pw+*iQ&X^b3D@H0%=#8o}OHl)TPmmdHS1E?bT9~G05ML{h zvud2)+MfiVFk2F~m@it-${oe^1eM*4@v)$PEunA!h8L?qc?!mO5=BQ3r+h^j(V!eP z@KYSPQD)T69`y=~uEr3G#p8AE@_=-9Bt^y52JSQ<6yqHH;Mh!MJ_pJ!H?rHalXLRo zG#OW9ekn_*Gf!mZU~wG+bD*dI@(nJ(J^>O@%*PXfy@$jcVbzlJj<;HSbo;h!ATP5Gl#yI;&8eWigcjG|u&uj&efhPE=@#K#ZT8#do=B zY!unKqxn#q8#_!>smz|5hIwrOs;X57iy8o@R?H&C)aET$k^(-aDy1QE#*|;`s$Cth#YmPR_>HuaNdC8aw+{aT&NbJ-h>{IHJ>ys7K0NTSm#jyMc=kfAr zO(za&JUC}uPMN>$TVi6y5diZDxLx5UDJrqjzZcbynXUuQ9K2{Q`~J;JG^Wb4O zjSGRexFe%H*rCpr|NcL7j)ufzWfdI1Djku{jZip1xf7UclJ=#t;nwz1*MljBT(c44 z4&mNO%iR?6nPZ1#+rjPz#k`TS?!oEwPhR)xv3>$*-_cqR`EL_i}zpU#VOlbmU|B%_aAGE)*b zE>lI?e6g^dXp1%VcBU6K-MJSuL7efsIt|r9*ZST*~Na1MIH#u_)UBlq5P;3r8bq8gVuzfL=@ErXAaa5%Bail&vauX^Qo!?27Q!jSS&LcB&v23b*&?11~wwAAue zP%dGp19qdKS4XJa<>EaQ+9rDh}4I?3U|u=TM;F+NcQ{5Wy{wdsc#U)>YcL;&Uo zqxp2@K(MCclKIEbUd*8{k*QO)DpQQN_Cz&+M7R`;)O50wb}1B} zl$qKTJCjz$1O_b(1`SI{RI2>FV*YErZ)s}ZIh9B?Wy#iTgO$0ehUw4<(FD{vCtJ^C z=dwr}0w?=BhsG2}Esq2<@7K-29+=4|Q?g>ym}K_B`>t5Jd1wh$Fj)lGWyn_Y3MLgk zvrbZZ+hEuVUG{Qp7}6<`iSVrVl!6wf6MtM|pjFKy+mIWTm>#v|#fy!!KeF(W+ji$>P1>QG%PPRiQq{Z@*Z|S%iM&VeDcN==Bbv>j+Q3@;3xa3$Ck1k7){KpUKM zCx0e*?(J!Z`gq{=9l81Br129G)D{O_lXXyTXT7BePJWXjbRL z6ukMp@uYSM2s-7TR;dmyR_g7?#^d#<nxdUqk%|)j_Is>M_|$Za^nU5|yXlEZC|e6b ze2qmXx?86cOora%4)?5=o5V@JTAd1i5A{+P59EV|RA)`{MiiD)F}e>kgJB1vgp3+- z%gAKD>5EZ`*T{!ox+xmxfqB$tX}K9StFc*KFkaE?_`U0XB!VL+{X|S#II9y@VwBT3 zI$DzpZe9Zkc@nr}rUZpTMfIfmUaI6gFpAW3b9ZadA`OB3jPNVRlBPynP_S9aQ;|v~ zYAv9(^yRtP4((ixNLxBVarg+|7@c&jcDyx}cm=IaB0%$9Iavg?72~0q$0Z!zY+Y5} zR2>{w33j8^Vv?wpwR)Jwf*JN?lT6&$oT*}9ycAm+((WTqRyyR_(^?=i*v%8++0iBcqKDrJIi=91cq<`bUL7q zVY1n0OSb`>R$_}O3rRjL0Uar+kl9JX@*AsEKV8rpsbZ!=$5bogIfoC@cn}L|V|G#l zUpoPAg^bYDE;d@_(SUnU4hsL;ZmfxD-OSBqbD1exu2ZKmgNfrLY;iv~&9~hTj=+_R zOCM#H%9e?sSBavhJ4ntmXnp(!Ro|yP;jHEl^4We0L6dVw+;W0l8IR7S@wOQUZ;D;s zu=r&8=k2V~3g6nEjM>h`_+%OPYd8%W<*t;`6NiV|pnTcRu7UbcZ{}@xhJ*i(QzZJH3WG!Svu)0m2VZOgpIBK~R0_)D>16K0#2-JBQ5x z#lEXIa_fQeJu)A>{UP(c+cvoVf%83lFA)D>)qG5yp}aR9>^(egu)PuVU$n4;EqC1B zpt8fRcFesYHE#m-BS?2v-|#I5cD``02gg4Yd@%$^Q}58u-jQDS%zt3`$L=0roeugW z$LhxWe-!xtWX2BITafxnpSMZVN9v?>lt@&my@cKw;sg5aNm`yYyF}%pz{SVzY@w*DJ!z!d|K5 z6?6t*xU|QUB0F(Mt`%ga>)~#+Adpgdcy+Le%WXp=t=zEiD{nn zVXSfe$)t0rD`CCGIVXOD=v?>VZ|Nc^mENvfGQDlPaC)=*O!?A(wthUfPUqos(Y4MxG99nZ(d8j+?|Gzgcz zl{!bfaCZ)J1l&BUF6i!rI=8wKTRrTHy}Gw7yWYq~djw^7+b)3byqcH3e(_%KNXL2b zmyPx8EfVzTJ{QZ+EAz4Q-oP#~_wqeg*M+-&KrA`;COmiFq0*TzMD2|w4~VR_EIarWF@6(at@|D3m@SLiWFupH&Y{zJ(Z7^?#RMxH)y`W>@l0-cw-qNvllWSsx-;T!$XaZ+%ps z>*#Xu3xRrs{lbq^UM?FWSwb=r@g2Pe@EJ@_nIMjSc20I0f?|d}R*3HgwuiY6;GSmM z?X`1^z)@d75FF!HaGPC+D#m{+H@xmhVL(pHopTpX_rl+0k@c0{25G+y$fR|@(K+>| zzhci>r5?E_r0vu(^hF6@@p-5+i&-@wq5}pgyV!ojF<0Pg^Rh@D!@n6YSJ<_mC_Fi> zq<6Rs1?~#3?b?qOUT$zmyli^>xL%XtBIdYJ>6yCDLj(2qe?AHT146jJ`SIL>5I)3p zz?!wC_BqbKo&Y-va6Yl+NjAxDLxV%{#q=vciLUQWDmIa#Uk2vvDu0B+9yjSci!G-y zvtIgSN*_a#LBFF#QK-`{g1t?%vD>=;fSts#a1lxS_KD*!qJDj`zZ|wkq2X#gA{`_6 zA`vf1z2ofwQ~FH@J$gVqT0lar_)a&0QmQ(qSa1{ayBM053G2+A6~DemZ&d{$g^&p! z-zeLJch5$aJQ|=E(}n7(y=lHl9cCHmNV!Uea(r-#=!R|(XG_zJypbao`RDoKF?_`X zpHsCAi6>Xyj@Y~)q-c%H|2*FeNxkABnKvAo2g$1lrk02L8Ue$hvoa_x4^TEBnT305 z(4Y^D==_b`vK~Qh7i1YEUjW+@0l`0cgtogwuDLI?2wEGFS|^zenj4{hm#Y>-#xOun z<1|7O6XkwINjp$HRKH|1#;?CitJ+(0-#0-t2lQ*lblyV8j(y;#XLcVs&R?FIQ)>sP zZ-l5PY#Of8`_(7ed9JgR4p7a4&moyI0Byt#`g4-aBDNhrA>q2zG1+qB*-4Vr9=%mC z>>NZpQE{%!O(Kx>gzPaRNMCX#1Ah1YR{p#Om&K;3c71jPU~#Bo*;|=bF{!nNHKLsL zKEv9T=PhxrF}{id=%HKu?1%0VTvVwQAI~shRIl=rd2MjL?I)2Yk6+T(A$W)H29VzB zxoylz+A*X$c;vPbQAP;%Q6GS^N6>c(@qesD+&IbcD3IJFiH(s06WoOH`Y;s*zH1T4FAA~kWAEYNauj1b z6%t}iKI210lp}I{6iVng7z-4XZxEwc`EYru{%m=jaq`1q79*3aeNj`kR7sH};|3YF z1i6^d;%R>YO?|GE8Byhq1`8$vW`mz#|04(DlySV0e10CJoG4cWG3Sh7<*>{#u;<=q z8JT?IY>AQ3M`z`RX_AEsQYurDj`Qbh1At!7Z+S>rnS0^{yutzz>Rfp-X2A5G%+HHE z4Mr)x-xE3tdG{+`ku!3(l#axi!~?s0u*@|wB}D^&PFaMkI-EpS#7VNEgLBm>5omTuOf+3~d4_$%>Zbbwr#!Xzr)0XlKTf9pM(9FrmSheKkA$ z=+K~cyRNuc?{215FrNB2ecZ^lr#_wHONcWw!c&qLGB7`2OY&W0Ps%T+v0gQc9c5;m zXeR&;_=ML^@TL}Dm1S#1YbI^%eK}OPJlzp!3iabRzvW`RV+M%<0h*_0;Fe$dk71q~2qY zDVs}hqf1Ea6e6N35-n?N{Pn5&K;}4oi*Ksl{}{%O8qIL7qVv)iMdON`4*w$(9;mGq zTvBsnz#Qk0FX9SxRwS~jKY#~*{#$?z#E{qZ$anHuvAM?q`K>6!92VNz+s%1g&rUPc z!m=kcv1*=lxo$9i-Vx1AdMb)R&q+T-nfoy1otPRrTkq~(C_+Q)OB=RBH*x}ZF|e}) zg&kwWgVsVLlvZJ@mSKjelZF>blY#QBO!lo{@Fd?<(BRnB}=M$$TpL7PME@+~uO$Z0dey-6#+&m}cMJy2pl(2Of+ zJRtbGCuyuMz;Em7{0nk52LR?dO-IF5S_e8(rM4`ISp;O*$u( zfSb#Xfo{)1wwDLlB+55l?-aixU5)<29TesfJ|Cfsb#%rR&t!;8dM6%yhmkwL;)%8= z4E&8b@CNM{%=jnzjh!)|jWI%dTd>U$`-ku^y;XY<)xlshhk!d2BjZ2}^U%aMb>bn@ z>F`tV+Wc?5K@^LkFnzHZY~2-^=B#t1ewfO;LR@mL5o)OsnC>ddU)I-_X){AO9#p}l z?N@8{@S#2>#R3F6LM%1?8f1-uLB4ZA#6oEP#wY}C0Z55QB-cdrWByqa_z=;|_GH1_ zEkp3G?8_6$>;Tq!k-5S7Ty3OA+n2LQ=(>PEa;nz_uYZxq?$Tf2qJvTe0tCAz(%mD0eL-Uq z{6k+&?ah z@%j}4dRwRxQ94@h@Sq-P0oz7|cL{&BAs+caKX`wpy2t2xM{2)|VDb@Sy+GFQ`Nu2j zzw?TJun*|G_ClhzYAaW&UUkEI?(FOzhwc0k*R|Um3I0yHcxA!9-%VCRlDV1uxHjQg zTNFV;!lvTWYi!!m&%jG>tgR3P|JXsshZOfzER-zI?wO@JCa{VL0i;L`GLJ0SZeBt(O}<2_g2D*ML2k3 z3$dzR{wqC399<^#rB*ACeTDr9oT_e4Vq}lT8>YNu0^Jq0wU(#^R{ISxa*XLOeU?2P zrnR(n+9vy?XX+8S;!yJ{Q_a>#SQEaND2mnyf@>_I_=p``_!lGHt3%iA2nJN&@LOHN z-)M^ww1v>S2-j|z_sa8O`eENcT449`gJ=UYlv*J{S(>S59MmcsH}ASo$GNsl_M8M7J_J;?)RsekU>whN(PG;&;T(^0KzVxjK@k$n2^cN zsNY!Il)|vBYS~({LMx!PvJL6Qp0~KJ;#TX@Xye<`Qmk%mwQG?Y=)U8Bn@yYu^zkv} zJj-?d>oxo4_v82XDUVy6zuVkD{8FS}&exMT?E0A$gMW=`{DmIySrX%GO7WZ5v`GKz zxf!o#&f$RuWA5U)JI2?zjK?!_>&LhZ)z zbFrgBlbymgCbu=K*dP_tMnVJzK57dm>{ssI(hjV1{iHH@RwG%MML60*`;E1x{Ke-4 zYTIW7I$*@%$3!Wp!$NfO_J*VfYYHu2nx%7!~BiX!Hoj zo}`&0YH$rMKq;Ua9wYy_`pK>nTvTiJoFp2EgABFK8Wd8YKiwILQj`9UHA!pO24lex z9qQRhU=<*M!|cHhCerpFXj#C^Ao&necP{6~G^7@E^#GTklbK2443?ip z!CjsOfkcCNLBp-aq-y zPBZGms_M7Sdni=%5ToTD3YI{%8~^*DT8TWr&dYXNU38^P7&sic8? z8hA2<%f_D2(T#kIrjiZVU45Auh_ztnfF;`}k5{9MF^$e^WAoCvI*$wmmz9&E5n{&N zYyGV)7h{hI#YIB3iXI7g4gF^9=m?vD5Y7qYk=z(5m^Co@k_b$7XX(bHIiB#;-Q4!1 zY{Yv2lehBMl^dF8>rf{FCZ{;P+kf}p6+kVp@cwYnZwZ)@AWq)@-avsBWstV)Xh;xV z$YkAi_N7UN8pV#;KVX7H8OVviCV`_3kpDo5acXxxb{lgS$Iyeoe+GT1e%^xhYO{kM z4Gd_(><82QQd$4Ca{!2Pa|QECCre-7SU@{IA49HyBJ?p{Pi<%u^)kjAAR|J^>lhVa zt-H<-zvs00?ZQG3Hg$ZYb=%nkoQD;P&0e;`s$f%`1d=Gj>lKm*FIS5h9t zCz{56&h#0a|HwkgsVhy2wB6Ck-r#vPXkkx^H0RIOTjM6(mOX+J2_}R$&>qw)pnh36 zNmMBsBRq{Bj-@Pj)DXR29#mU)1yDRgB0y^vbXw zc!9~Ui1r9))6N=7&p6J1KcfgHEzU-z2<`l>Kj?viEQ)Tvx!gsDA9GpQM~X3`lV_VF zI*oG1GYYV93J=ktIV{wqIaT$&M3N4c$B@P|%CHznbqGHZfExn3{x$-~NRcEJ*iP*a zz0kwhL5~i0g*5IKs1mLYzKc|KKxxQ~Vl`sy*6&@+f(o%_+ZTx)#XaQ7^`LbzW0-H` z8qy`Nivx3e72v?Ur_t*fMu~BkxjWF+x-y&Aq_Rw{@xGW@6r!0b(^{*|wFo>B`o-*F zi3MY%&htw|K)t93$r%B@Bh;P+A>t?;mbYgjAtst(h+eA*&v#Wxb1#&plGQ7pvBu1d zUY-e>YE~k4j>6z--HYVquT*izPLG5<^1!S?HRm#xltk&VqY{mCc@9lISQyrqxl$p= zx817(Uuf!RVGl3E*rz)~|B_^hi^pJy;N@@vA}MaF_6#IN8R}3pEHH`9Q+Aa5EVN~6 zk~x~Ev$3GR#TJ@meTB2Dh;e|Z?qAHHt%VQ)l)Hfin^#n@qJva-?Q&-1P3+72r(fAY zc%xk8jUloX{0q@VP_}`3X-^sSTmF5f)C62t5wP<@FDu z;Jc_H2~+Gb#ptqRz3RW4L%1Y_V}$YD=tAOrG~FgTlZ61#(3|96g8;f*M~I?pL3xyl zcWISFUYS8{R}3n>HQi#*DILUw(rwXHVegnCX+=5G-qo5BCEH}-D~I3u@*~ZHav02I zUC>4Ua9ojW*{XJSYNHPzRLkB)OZTgM{L5HKJ!eU)!8J}57xT{(KJ4TkexieiF8wlT zCls@Ov}$s*#C`d$%+cq>rd~M#`PR1=Dxvqn74rq7!7LaD zNF6qLTTvNOaXXL&I$1gTocR($rZhL%FtHM32are(ivWOZz?OYrW~3Th8hgJU8KuH~ zMeET4zgfb(+Rpq~wZ6eoc zeXQO_R`aGeyNqhZsE)(Qmunn^QtXYQB43VKNh~g9A6T$H z#9&ngShIXtodT;5n%EaMZE@Wa^(TbPmy7$|+J4EJWauoZPkc7HFa``V*A1?SU!|~M zSUMo_aaeNEaF=S@0O6)^r*s!Fv$lPKn_&&MY4(c3%PZrG;iR_ml0-Zx6)*xpzir>o zIy@+OM?<$zZ0~8RY3+VaP!h(c^#A_8Z|$ z6A)fW*L(mDhdIyB}geBI*UxYSz&`*fWH4TjG1{5>Q8#QD*`i}~T8B->Qu~>9v%B&cJ zG8O5?y<+Z_vM9@53IplT`6j4^hQe4lTlU^DvmlHxE}u(uD%R6vM|@#M)+KJL)9v$} z2!Bt1K)fo(z*(iZXC_8lDU6;qtE{PV)vTB~cqu}(>DLRTHVdg%%M;NSp=o|Dqugew zP@cw1v&{bstImq_B3>C90>6j7wWwxGo*^WDa#Xw&5a2FOJZx?6lL=qKt&R(C8>320*jgza^T}7O^KQk@NHhw zF!tkq>ct3|Gw>yuMO);PWs8RVSp;{@mpM_X7r1Texw}|tk!WD?H0moiHG}qek80T&~aArb;Ml#c}H4E*u=ajMLY#H7w zFjRmbhF#@vkIDyzCN)9gKw<7_F=}c>`a8&KPa=7ERsMAf3(k$FBmB%S{ZcVUR$6rZ zgh;Ll&qS3aJ9CqAciP4TQXNX}-*e%V`ZjHhe1jTG)(%M(nX zM|fB9y^o@S9ux<}z#URMux-BnZ^r#y@b)ly$Tu9U>&sAPAUf`#ZOHC2-qW3Y znAkao1B6x7?Y$pm2Lhl#NZ>c3ejlWN)9oVvFUoyBi1xU+8@N4OR%qo99&TOa)8**w z<7dhAf3-^&%EzdErBsKOj}hkOz%L-pY2jz`Nx|O~^H`L1tVw&N-KG_weSG$@Zt^BcsTCq8Dw5|>OOm~%uviQ2guE)4JXn6RZTkPP z?U)SmK#@30e7CQ;rAIc^*H(kAs!9e6c(q2GKx|@#H@Gw!CLp%-E-Q!qa?MM6C#dD& zSYDi@X{0<6&=V|)#OVI)wOr`Pk&?yk;SS-$IPZb@%u+t*hrAYxoHqjzCs`w@f9px=YKLQS)b$-;0Sw6fuL5z4yX;774+(5(kK2+7*p3NMw4S{ zu=#ao+U#s2mIiXi#IOk>mKL;t*1ruSrtZIRVgPut#OyiIH~)`-9%#^&q4QU?9Y|-4 zK{H;o9t(BR*WaFKJDjWqx&0fEA|eJ)Xv`gmX+wri_-H*4v7$0Vz)#=od20jK4s>u= z|G6E5C)WH8MA5$~g}CW6_m7@fkume9$No16W)E_-9=KQ^)U-ZGkv>?FdccCU+|aq( zkIR1~o_I)lh+=)DlFcV-c_3Bhk}zIu zny*qY7OSaFJ@{xRGA$(+BHNrd2zg(0; zJuz7@^2>E6CiKxUG_67K+Q-V)v~k~b9@jS!En+9h<$7Ny#o-H}&yP}dE%=@(&5UhVJ8-{WVTZjvb^3w3OYJHbUb}Jba{5_1e+?2RwM~_C{BKFAzuijIvW|5oFn!}ip z(05UsTnn^xeknb}gBQrtG_)nY^l^#a{4OEuVYoL3xLDb*qxlBgQ@XcNqsGLqFKOl#W8?_SfTW z5iv6pAuB7L#28a8$>SQ?ihN{e2zfak3)TyOmC~{`iMWoa9ri$>J?*yWdCiQ{Gr~NOMdmo7R zTA*HD0Qn_EGe&t{+I!}^vKD!-pg?P>?_C1FsV?RFSk1It3ik?%2`k9pcZI%9kHr(k zosd1hYU@vUW%H1V?&)Pwrm&{oz3RuV{PbI)dt6-PsBh+uZ@Se-6A6O&^nJq+z>FYA zwb0T>MZd7)>)b{&SGlmlhh^;Q_kvF9jL&ksUjO{J*rZ|~e2;yg^)}qvy0qR4dS(pa zcK&Tz)X$qnzE3TYGj=}WwPA!4jL92@{5i2@~w^r3z_+8QXfRTV!j3 z8CcwckC#sBzQe08%02b{iz4R}^)GfGng#*v*1WRR(Qi8b62DHHe4c#|AickYXyu3r z(mv_bE8-ThpOmyp1V!k}RVzi!_{Lewc?%=)u$A&k65k$=D3uB3$Hc>FSI_?b=IFih z9TfRW2)q)AhD3ZkdqK{RWqBUo%fsBBMEa{3AIlFc>*6zfDUS57102$ymQ*rrHe{pP zFxL{Q2ylP#%#WQ+=?U6?Z^CLLE=_*@UTC?5s<|y>d&=+>^X6YQ+0}zb>ziVT6$m0p!X`83fYtCCe3V{zfds_hi zgEQ&jv#i(tD~4}#*x{XUtRM@d#m*La1&DlueUy|yUf#*Tq3!xK0!#YBem|3Dk?k3seK}RX^LssOtU|~qW>!hR%;Pf8qT&;xM)e0|t4A_x zrOlwLjjFI$I8W$X^F_B)>l?>M*eA|TX|HT+8Sxz9h2S~bGxBe_uY`R?P$AqFps_R@ z3o0ljlbo+qvv|im;Tu);{JCB(?l|@*o+q&F5!xx|I1hz@QxZjV1lmgNToKCJ;Ta9+ zFD4$Vu5XA3j2x>n)jHdpxwpBDscIHym6Vqi{vBM3HPfC5saIslnt}$@1L) zN`4QD3oG@a0|7-d{!hv8{|&JfmHr!uZL9JRVxRnj*tlC`j(8NZ&YLU^>O)X5OT`1g zjiqTemWzwQ#U&KY&&aKuuLHcp5750VsFqm}_%cZlS>!d%^T0bLODdMpw6fd~rl7;J zAYfT6mY1*A(ckIs+zJ+N-g}O?6jfw?cW%3PJ^HzAb$|bw0`b0E0bSjf0D-J4MYWtK zwbeYf%I>yKZ6SC(L*AQui;nr5+$E3Y#}e_of-8jW084Eb7{?I|0avH)0Y{G?$>9}o zjRLOG$~Wv`EX3mwpu;WyhJDPzxX&c#NIYrsu^iWA=ZxbvJ9%tP;A5sXl7P4b$+786 zn7~4!WE7r+e4&8!-ng#PJu5f(07w~g&he#DonC{sj3B{zg5)NV%L!VVR(B7di&MI| z88e8=?7HJ61 zHwJ4VlUAdjDIJ2UFlIQL7LsSgHDA2}o$2%PnLwyETCU<(m+0jXq3W{leT-YFF? zrpm5-na1!%R9Af5mPy?j7_@0!SVxSIO06S3Vs-C$A zVI$QQ*)kN9y?#9V5NFjyoYZWLBB_Z;#?wvpwhNl@+O8$zSc0Kv_y&;5Hph%KTXza- zeUvYKmJy_>vW@o;8@nz1Pd-C+H<8j^Tii-pl5L!`cYNIC4z^*Hn|pZo-A0w+Q+p1! z+7J$kP%7>w`amZUdfi#Q7&k0x+hC?p>@g3k$$Zm9DGr<`nXlYG)-&Nx1;6?1>UZzD zyE@oDQQHa1r!j_d3Zz`zr+fhKo6WZ`3N0*Jjm5o4^vz6YtlA=JqOX&#k%rADRS>$* z`~fn{Q}@Ie=q(Tgpa#gzg)q%^E6okrIwPAhR~!!}yb{v#dz-}YJ$(vz{m8H@Ar46b zp}aSw(L3B55qZjYp3%GXbI*T~G+Ua#kFFMX7uoqRsSt93=l6m4DeyZAjz;7wS)QQ;Vue;*hGZ7mGHFYl1*ItBl zvlH-m;^<%cNHbl=@gLKA)`kR6NbfnJLaN|(Nyo1G(vG(#wY4p)kBxOncyw*ATzc8Tx&vMV|?&g^&{ro zP14Up&dOiiU!?r^urPct8saCZJJ0)*|kRJR?Fqv&==a=uK)Pb&4nWGkvkm-Ju!2Ge&n^ zp?<5;U%oAMP6Rd#vNM~+2_6oMroOUCN?2}AGdFa*T0l@^I;&+^tPYh>;ij0{DcH%H z6sEGwy}cdukRU-N_OB9N1eWyDcy0KiVp;9tHattKP4Sz!g?8Pbb)bIUF2Ga=Q+Vq} zejp{3Q9so8l-@4Y2Ytb}^uUO5dL-Z?k)i+Ia!z+!dnKvuhe`SPFpOXio<@MJE*Z1| z$-nL|f@g-%hos0&9&~f9F_L~i$P<%jW z-ph}?9nlX=e+>B-CEyMi%3%AR`G>lE-w&La&`M}ncT|lC7U*VrLj=kEN&Ena6Ok{N zDO3VXC0gcI7J}wVHLw2|z1mX5HZ~yt5mn&+w>y&jzZ=A=R`wRIwno1-{)gy)j2426 z+G5W4=eGC#)`T}>l3XIDKB@>bb39`}L^}ZN4qJc}gcsUU*fd=X`V|WY6>QI`-Oe&) z-=dL3MA*c8&U(6nma4Sy5zIUJsv=u;^{QsG$)!29&GmA%jlUjd^}Ex-oJBN%JgoIQ z?*02~+k4wLF6r3+0p|~eOIBGA=uZZW$O7&^5x>a(4ua|!8&jvk*tQ0Yf;NTkLiXw# z6{6EVzTSL2EbH)zO;pbr_#2vswkg9Kpo&iPSQ~v%Fjt}UYlEG`Z`kf&fL0Tioi8&Q z+tPo#A)`sV+&gf8dqG3+u6gtB>DjD%yStEiFVSJn(+77PnNG)cc_?nLd1(5#GoquS zJe>1-@w^??8(&7Idbs;Y*mTr3n;c*8xaG4`uCKwo((-t%X+F6-*k5;2ceO@wdse>h zMNBQdxJi%y(8Mh>kD!IeK+DijVJ`{r+Hg%e%*c zb8CisUe`NRoo=&Fv#_#4^|!o?*)c0d-msjytVuayl%rzcFSZjH46dDJvr{`(lVPPH z>ry~9I4v#zu1OXakDZGV7!)}rBR)2!&Hx91cB{c`sA6(`?Wb?B@#vY<)GvD66{k{7 z>#2-$b7v=vho>QTcFN}KJrbu;vB|p(d|5=gvlIL4Rbm88mQwE3#qG_cLHJnj{cvut z#&&?@FRd;Khx(l8;nuAik0X%Y;TB&<`d|M@w)b>&YZ>xRSeRRgujo zO0j)t!AdjyYp5qqCaE*aSZ~1a8gY25B6>5jJxz$CpTMC9=9Pp$IL!F-x4a-Fp~=bt zNV`8|U+`J6oF~E$q1V@tEBHz{LvisdBYK$+1|jKj2;Qj%k*?O`mL!JJl6r@c3YjEr zh=z?bEKEhY*s~twK8IAdk*5>~zC?7^ljoMhZptvL3lKzA8$P_F+n-4ZqSr2%PQdxY z&}%kNz_UzW;Hw-hNUV_{SC4CTy@}9W??CYtT|~J6>lxvmJjIzN(&V%k;#<2XtHe&= z^S|%R49AjH7iAB}c>;*nCXY-<8&v5q`yLZVxW*11g6QVyQVq_WtMV=G=XGnBrRLmh6@R13uHkmb;oz^^ z2-(h<4;sikCbgd37as`G2(M!1PPvr$bgj*Kn}UAccql{zk*OnidAYwP9v>s8RjH*KU74la z=9(vt5OsuizwvC);HZu3 z6qo}CsklP>EOW)8z%#GY!{E6}YUxpbi;SuM8rV;o31iNhN4m|}ZF#L}HY7hZ>HuIV zx=;{&Bia&UMI(?hVHt-F=fpETlYvSUIbtEnev2z4{Pco#0x5cQjm`)=%upqha|#GD zR#|t!f(RUKCmK``iz=QkbYZfud%%5Y$vq}{@)>C$mI47rf%z)#L~q5gn)-dvtnKwFo3gBB0<6PvoqD+r8CIf_-EFo%lKR{ z7p4bwis`H^m(Ic@dW_i-S8{Je2Nxm|k8a6ozrZRBeTtD30UL1!VV)SHfu~>jz?_w* zbVTw)RWi^;#@tON5jv(cxui3B;v%_T%p{|=D%nGqqe^_gPIXZS7c_BC%CLpbZ4!T1Z2l-bBZ?7w=@ygt3YFlv*%$Dh>W$`%^L)et1 zI+OOLMr9Eyud6~1v|qSFJNp&eY`d4lS<BgrMWXv63rZkJQrc9tsm$4@-7ht40U-eTmrfQiMp37JFr9`}uhFy*kR?b9k;*_}FsGQ>PXt zrWQJgR9PuiS$S1iIsc9MULG6)!ATuId-;jFK`_g7ZIvjd5vW~Qq-jUQlGq_7525V5 zdgjIozM$&TL)PWrk(ot-Vc{0`D7^3Ngqusqera=4c#rRh;iT_skvVHr1rsiGF|Fcr z%Q3$dFx9oIB25N$kd#lStrB_Wq%z0ycYi^e>HN0ILw0?pv*yNNpDxTWnZ;|ijMZUt zroUq~^sogLM1h*D+hsg~cxJE$ljlS@bX$TFp&%+(l(Td2M(^Ra--`8<_9|b{kTa)WFEV6P z2HGFcf~7`b6!)9Gt6;*U1?!or(y_Llz&mB#DQ_IZpY{lxvRJ%U^p4+ zuOj#44e5oZK=02nZak&?+u$?9ZtYl6YktJahMS@`JjL>J?4gWih;7yOo9H&&=?ZUb zfnbeC=8iHyB*}&De&t6leU0rQ=11QTsZVZwW&bG6MaA3Dw`~tq{6QV&(S+;zjF4X`iJ!N@Ua(m|L9A@)}OdXCW%KIC;hw-W?yFE z?gS|VQ3$VWkv9yQg=XctFX)sTU*AB#_*YT25<%I+H#jzF{}`IbW~=%yMEP04(n|O! z>u8cp>*N_S6Ou*jMKy;7<)B%ym?2lT&2)LzXwL;xx}xw|iG~>awyl#=?xAJNnp@;a zR>V1+-$?!a+M#lCluc_ z+WQ8QHlJBmIsUwx7u`|zxBfX@7u|77@6WT4-|nXx-$<5#Z>r6sZ*04}yi$*zqL=_s zZCt8?b+F=vpT86hU|f_O4F9-+cPaP=K$Q#r&AWo;Qv?gpw6fT7T|<8fzk<&$-VB&N zsp&a(aW{0|&@5xwtuCziOZD^*dl;fN?g{Ub29N1wVgFL+ zL(!w1*BFn8=u-FW{L*#CSM<}bZ_)lGEZ^;?S=Oe!uPufW%uAb-a9@1AQAYpOyMZZg*U+h+sK|$V19@7VDXO^T33uByLCTExtYiC+> z@Yb}B0^vb*zN%;CNq+}c&XAnYC-W74zvy51U<5X7==cDe7s3cZL+~UITE!e5&=jL$ zY`lQD$fJ20oPEGZR~d#dF-{0ITS;{OpRS%{XXxs9nl#4&98#v>C=@i7bUrOEp0M8y zE&`2T-I*X5^cXm#&@9@%hyubf#))HFl(KUN5wH=9N=6akHsF3oDY>EnrP_V4P2p{B z;Nn%9w8anrFYp>{L~YsW^(W~H zDQis&DlH4T{U(;62cy9P^Mby<2nrDTyI!RtU)>=^Lp_>=WNAj%A!y=INNlua**~QF zsjH7=;Skqkkk-)w1vAR)8gTy7yp*Q=*5Ol~O}xbv!t|LrEHxws_+NSIHtkY5qziNY9HcG+O8dA z8GEz~5rZy|Xn+C5-i7xW07JZA3q1^}Zk}!JA>b?9r`ClV;I0LtIa=^7PJZ13xfLJ} zU%Ow($?DK5viBeg1H3ne7>D$b4DQ!S?9UEaEjOes80i>zHt)vqW8h>Z@7aIE-Yrpk z9pf`%UW)U7IdX442zetWI5xDG`DU((?se2UvKgs&@_o6=m2i<%oBGat zzMZ~4*=_R;%SU>%R64HsXy1P~Dxp)|u>hi#sDzHna95;6q?fDhcy(n?n)uYIkjtvT z3+R{h4a3drOk8ni%=1}UeSQC50G|G`CuI}jj~^kp|C=3!*#_zy11C~st8 zWuofjYV_Mo{6A~|u}`OHzB{WuqJR4|ct2n1GkP+brs;L^wa3Q`%?M$H6T#tRLX&|< zCM3!}nL8U9Tx^9XRlfXCMpLRlLq`GFY3o$1xFsbnt%b#{xxI;6d=v;iFki}j2e^eg zQJr!>+rx*-S>TLtAA5HlKk~<2KOZIGYHowgL~qlE8^uM}(yWAc!je6V&R}2P7Z5r@ zKth_5p}qVK>m>q9>h*(!LL*B^Owdq@;MX_BvcSeuix=I3i=s#JP`=rVkM4&JT-nz` zIv&3N!U$7h3MQp{WqCfh9wgz!l2HKF_By3aLqoQT?I$yJ*f zSeNJO-Jyn5ynm90oM!S!?WM*pMXxfSUF7QD;bzP|-kI`R%5&v+0P;K}w}D)C!Zbhh8L}Wi+AO-AMi!eq_tU&v$gId$dKiPhnA7!4 zV+v23l&Nxk?j6bHOiL$OMSgBIJEIBwBO&(ljkuG#y!5C1CRv_Bl@X^a*ik&nw%Jg1 z@2?UeZH=K-^z@$*jY&~D3uT5ZQ*huVYU}f<2~59L+Zsq&E9UA{+A@wlw=(t$P{%wX z&PeyoK#F-)M;>vrIS@|EZRada;b=IFSjnrKIZF!hb<{cn;Xs== zld&;8)rZ8C46#KuR3}b2RlR;YQ>4w&qxXA=4ral3g*a4V^}3(* z&~!yLQ#9JczUKyJMB{RU6qLUZbk**$_z~^f4P8UiX|lbo8XeHnv*vSySm7pl32f)1 z-xbxaJ}%rnjo!sX3c=a9;D&PaCvf{Fkgo|*l%85mVvVVgwV9dERjpXP&jEo5)!ADP zT<~|yz-6YNVuue80jR3y(?$D;DRdX)$B4d6!$6UzvC9SwI@VRRd3x(ITAHmvBy|^) zut>t)VNhb;>8y$11c4$-I7l;=q%re5is7QdaAkCpmx#zptYe-JwZZ9|+{o&clD%UO z*}->Pi5z%o8$~4Og*6RcuM(|y2%UHGAcNgy+r$gMKS=660}I2pBeNS@?DF@VRSz%{ zvf31e@Ll?`DGRj4)Ss%eI$jm9^XchOR(nh8d!Sw`LrPOAYI$Pz4Wp|&H zntmOFEZxGd%=c-^+72l{lf}67;Q$$dZrW$7RS6WIW~_SR&3|P~#N9enmu2isB^1?F zJLcpElqY>aQWS43vAEl8kC=`HF!ncOpB7e_=f@-~U2@mL9ri{Z&X;oJF{nffMlfII zi^-zaXs4fDZQnEw+2qHp*xq7S81 zyL@44cyLR)!XKKNx9@bNs97hW;^)*x(PPUm5zkQLVQx)VT2}%+9|(rBEzkq6bpxO0 zXykq&r>4e==n{+UbR{Aa&66s)*OO|y#k2^ZzQ{7YfQ*gt1gb3g!srBD2~fAj8fH}v zUb;ht&7N5&e4wJpo`0&a)_o*yZ(^RonL~Y|CItB!6~1ryQntlP8=LOUo7e{u#)Jce zx!+luT9-nkJR2(MCx-fL4aP$8TwuQ59G$7K@ZdwC_6uGjYEabHvTqDWm`_Pe2nlIP zxdcdX0`aoZ(vs{7m|F_%d7EaKo?5l}{E$#KL7KPs(;^+4#~%F@8w@Iv+VMyhG$~~Z zdS`Yb{EmX(F65`#5M@Jbrb{XjkBVK30uJ*GNZxtYLg% z!4GfxR$+W}FW3&3QeoGiSEB;nm|$9@=7wBRQ6`Nn7;W$SiaVJuV?@{V1YxIn)m~WI zVM0-exuVwcVGqkB=&h9`9SJ^uMXtWE>|O%5LXh9el2BXltu_JxGdFxk`y|!dWM5{9 zU4dTFg#r3;W-rJwFJQjjpD^RM?o~bAvvj6aXdrBnynp5BM4~JQXzE}q#Tkw8l@o?= zoKkN**V@B5oY38)6zc+ve>@pafgg#GkeZ1o?dykW`hIP#%4ewmr5*<5-p>(Wz2JN>yo9+=JsODDgQmn`ljw7iAT@+MZH1fX+ zVW*8uUWz>#TAZGZsDOSXms(U<#2G2L;s^Z*Z~`Bl-H6o=wsv~Q0=gk>6Q``<8WRwE z!rx1cT_24$IuTJoT z*ZAm`hd~KFZEa4 zgU=3O^PR{kK=DWXE{fVQ;EICjAVTY)s&G_Tn9Y-oI%SMc@sI}Y zqSv8_7UI`NGU%dxddTAqpYM_B32Z1-u+yNj8#3!~b<>HkliUHZUy zs4g~r>%Wt6^Nw1DCKa*4laHwJz)FQMp;L+0$w34}f)yZ;$5W77jptJp>RPDbA&Y1C zYR%YC7jo%YFlu&A=^uOU(db(wG(QPYn!w$CU3+$JKl}T5JpEmItKQc47NC52sC~Yq z2!>Cr!r+2T&7cW?8dmE?#g-)5S1`Rp&A5t)W=!BFNjOa!d5|@Q1xCY<%LAr-&9+mX zx7kO5yb0xH{!*GYzp5=TG|5LM)N+OOw({}E^}fP082*h- zV>r;jWN!Ht`UuZp|CNUm6G{5PU^al@Q7$i`!_vBMER=p1{a zXPlFzC88GtQU`luVh01VnWZI0*51MqDu@?*W6%({c4cbE9`gC-A`g3GJU_<5oypST zZ<58e$=|5n&y9XCc(?lssMmdBNF-K|;9fw-114k~vq!c?p5YByP^rh0)qh@e2A)<( zLEX$Au@-q2H=se?_ZLwgx2XOd6FcaTwRaab@cI@vkU{=8b6N0P#y1>6?jEOlkk`x} zc@~0(H^f14SUwZ7c}y?Ky%v1$+(C?3KBI<=jGsxpn($pmtG@7EMmLO*+jn#B@Ld)+ zU_pe~A7uU0@P2ji-bUT<-I=>T$T8*=Ky0W|%E?s001|MXKb~|(J{N+2s2y=rs5Z#a zW87gfcqVJNiZRFJb{USr8EoId0l1^rOsy>z0+fpsGAhi^3hm)uaUYp7$1O1>?y(-n zfR!N~6A1<-o2F}eCH!;;u840qlgDwn(QjCngC{7_Pv$@*fNyK9U-Xj&@XstrGzN?I zfF*M+gAp*f)C|Lm41eT=NCN8qPB|LU2$*dVXYN7;==WHfK5>+gyt$B)kYwh`w4kMP zfyW;w+{damb>XYcHZ1sgO7>N@7dIMB&~M3}CbwHt!om|Xd#cM8q`AbzlQgTv=Uo*t zRpR_&!r(7w3{0KfZ!skR46}NHPUnuCNMoqPB*DPl)3XU3v$Xx0<;fK{;s*#bXj>K_ zxS;qG1t9~Xu=V;Z(`O|lNTVgN?GjH_;F9}TBuvuhT0%JHF(h<6L1@#m2r{cCou<4Y z{HqS)lCb#)MKotsvH4?ISEo;`RR=@Xtou$*G5+z9S-c*8|pvq3EAY+ z;f1p$Puj+YkRbH88cj_c32i1)2T&9+&flAQb;&XIEJCKC>xMe^B4^|cz5w?}Q2m#8 zrBL^eMKbis=A+Eo^h?ryFfY-=F46Y_^Xn7)$06=%jw7;L4GGK}ec}SoMgxX~R+E3A zY6g z7^76p;Haw)g3z-9=BB83EqJ_Ev7<|1 z2JfaZbdNYYs>@Q=nQy>?UHQb^D+V-*2wtqWLdv;JYxp!X(m)<=i4$_3L?eFY(aJ>f zW&U`wFV)c_q?Q3=;As);G!-u&gm?-oIgb zAA>oFT|0La%v3&bj^V`;%x+d3pqY{llk5kmo%2r61M$jb9=CsD#Y}pSelabKh4uU~ zA(d_p%f*H54B3JuBW$%4K#h)MUDx=+RlUfsuHs_5D==c;_T8h0!$D#>ONu!{Xho2E zR@j{66?PaZ6OHiCf^+}v-IU}>X*y`VP%sd7e1NEn$RjM>WHQhj61Kmi;DxDB>0$kW zzAUNZSTHv`j!442^5gKHE%=^?S*BmOtJCFzF8C04LJxLRqIsVIS#V{BYN^7N=O>cv zwWEZ}7qJ8?Wmw16kekaHQc!lU^@ouqzJh@*;MufY4k1Id^+NajM)wO_s#c+=ex2)z z(-o2Oj9(QOb@|GJt?+ry#&O#kUR-<62W|);XF{Zc`4B-`2Duj_MtVxJqHpagBIgQj5QU_k1uF_e$GnLy94f8phIt(o z6A~xorO;9qD-B2#o6Kdf4M_#exl%jveXtgYcQ--{IeICk54aI7U!QjYBDUn^QMPe} zAZ_Umg=jbF^m1)WZDSckIM~+x(jR_}^fB@6{EPU?*Hhr5`#wWvOO4*C$n8;kl9BOkjGpKwntdL)OvU;}DcQmF%EAehfr>o_~(;P@$Jgm5zKPwe) zA~3)lRqukdDd14Lrrkn#)_~OL0ck~TAbt)48X9UCCoHio?2wjjEk&6 z!)ds=aJ{LKE4M`N3S?e!XCEwt8&1Zc-~j^ZR@3~YLxivhGD~=Mn8&Bn;RQ`x1lhs< z9FQM-sjmVbq+{P>xVAZ<(m2!9Ay zm}31-83H9U!>2!e5CzI-N>Apw>Vu`Jv^jDNxQ+)zJeDMRBDDQ#`_W`=y~edU>yrkxgDbgL6_ua!@>Pn@|F2dbM5 zg|OQy7Fkkk5jA8Nw`?Jri6MIunYQ|iK%h=< zV(J2S+%PXzl(@-YS1PnWSWk=zJqm&>(=EG+bJ#mD_lbU=`HmgJB&L8%@F>b#U6yVr zt@lldZlu5rkXjoQ@e5oK(OoHKfmo zP_R>>hR@vaRA}G;tQO+hbWH$cpeP2kE7MGDT9XNyn@Im}UAgBU#Sw|&=Z7hclko?1T@n`d3KA%HJXnj3 zLI4o{H%u(a5ROsCuw8Vk>!R{?KmWMcm zM_AJ<4Ocr}Rxj{Zb26cF&glt%&GcDNMAuo$+>i{xoz##1Y@>Stg8=*wZNh0RYH@0v z>Nc`gNU?LBGoE~(crodm30~0%0REGFDHY*doT*oVh~QCJhN6dLIjgl}aSVsCW^f{F zy#cI?1VR&?d~U=u^`~43CiR1J>@$SFX#DH+1OxsQiqI_$RTi(au z{0BV1i)PfCO;C1%Wh-!T)Ni?Ri#(9!hG6+iYV0d{D0kL9^U2aFR?vNC{)|g#4K1O~ z8x3V3>?2NEL+HV^%!eS4iwhIMXy#Mq2G3Q7#CL1aik;h)EB4i%3rV5bm;B{uuqa}1 zPtsvP*C)@Sf1!hM5l6gxT@m!?0e|MI*#AXyQ7LVK4-bRZlh#{QLf+8c4&6iS3{PPEb_2KshIlJ#Cx@TyJ zX?%2~Y2h_hA|3Yaf}jmbD5dMXwtju!c4a0&nNZ;{G=IAp4=J*KpaqHb>PBE2w(5rp zhs641qh?Kve88`nWt;{5qWw!+kkzVg__+O*gPMWJxVyks@ew8aTmiX_Ec>e>YJ@&^@>u5dDlJlxX9^bF@mC@q@x2kg!`;c@ zNaP=jNf?kWYE1bJ<{7XL)1s&}_li>L>R^9!rVt%woT$+&u(xAogf_SnRF9U6ZP`;j zsHYrCPDo^Jr@hAMyZ#y#EymWipwQl8WX+OPYhrPSAgIH{_bAWWnc5uH{p<2# zEt1k=b1$D$$LM;o@kO5e5`)wfOtl^ub#=q`>oTh@lt*`9^%v$47zd$2(NZy^8tq<7tjY8KA@+WPw7f-(-f9uLll{b=g%w`|no9+Zz(t8(RJ{PM zO^OID0?}nXg;?r>JdL;S9Ef8sSuMVh>1S6y(h-b@thAOP0MjUZPu678r$X~i+1_>u zhj=lnv095z4gz0;xh=UnKuRvO$_(!ivdi3Z6kZ@j=lm6Vb`hzo<2-fPdQ^@c1wR7H zV8H628x`3Zewl|-Q#vd%$_htVlWikWg3+b{tEM`9UbK4F+%BPV4o}@=;Nd5Y7s@by zY`8aU{0I22QXP1Mn!p0cFC1x2-Vt*EC$gv=EE_un_j2NtpTJCkjNpBKC+j7^y@-3I z2T7!gJ9Fh1KZJVDm5z$4OU73e)B^(q#(*auSO%|Gaf_Q%{Fs8UZVtTN92`YI^ByI! z2I$8!_)B(VF#&j-emV}EJMu~__8+Sc3)4cDymTX;i5aypd&+1z3bJNlntw!Jg|Oe< zy0oh{)(G|@Hr6rr#%1N~tr6uV8g4%|RL3_4R8-GKJuV^51TQ2(Ru7PZAel$D!zt7eWwQ*&K$uMsyD;|pbFe6L*K268gcB_H2N%3W z*)WUiVL}_gzz$t5E`=|Wslz&-cRYFTZMNIHz>F4dH&-QZwSo~_u}oXIRRWl*Vn2PX zD07eUx+<+?J`~*{tKP8Erz_k_0G^qTnWtL5e^)JT%AeYLtRrt)Dzc2m1~OMK#fJxU zOKMEtJUK!(k@055W|y&3*x2>ul|3GhQ)k=4i9)tzdGHdoN^|Pv-T&*S6j(@jdqY>| zg4SwkKX^yJZ54JoXcIo8{`-#1*I(@3%GYY92#hN*mr=xuQQlfE4$B&Aei$wm?Hfhz zq|T*FyVAyN_D+GkBW4H@wcXyt7^e1#4mYECcs4ij#XD>4edN$r%Tuttqgz+skcN;b z4HIW9D*Esr{|;5vv;5W%uE`Cj$tbg%PK!zYp3q80nn`>91B{jC!0JobUWLW>Z&oq_ zZs07o8!!tVk3$ZBLm_reJaoa!ng7!*{?0m1lwTWvO(8Uew+Xe~zr_pa?tzQFr6BPy zRKq(maz>))uPBU*%`Ueo<5f6z5vXS>gT=3c>Y$u`i|--6I<2~dC|Pq-g&T_&zl0?@Gyy;X<>_oe49I3~XPQ|$&E7ZYVxQlR{E9-A zvwK=8Z8KLyB1i&eR^s2{-oklJQNT+p@2&xQ`b*vfuC8Fn)-@JmYq#Uoxh|WCS47Gt z1g$#uKJF&==IEbLIJ=e3Nhovf@DdR@7Au$!InPcma8BegnBSc|h+$rcA_7q5Gt5xO z-o&B(7Uf?)q0;;SEVRL|1k+8*{qVjAMha_B3U!`?TKlr#1kmLhS}(Zl<6BZcp?QK| z`SklJMw9|$=pJ;G8-?|Xf}(}HbR%S2N!2dO2BgDSe8^3ub&L)gJ*s{FvYDPTEIFYb z8jdaTmc`JaT=hx216&n&oUVIfX|k0~Lem_1NH@uPDYJ_+Mk%v*_W4z8q{Xc=64$&#Pxb0Xf;1X`R1a6VixF@(O0T z&swDwNb3gh(LBx2niK9(ai3q$AVwFzeeCunVq6zRZu_`5H2Ve-e%f6T!j$cAXa(CDcQkh*F~6fvkWVz%KxFRw94 z3fb-8)s6_YiHM?7;j$`mRA^ei^Cz~;*u0bLgDtNo7c@(E@{~8%@bGiFb$MZh9GX|t zXNl|7j2b$|4E5L?%q*J=8C2?*vInp0PmgZL8+nOVeAf(Pzy{;8BHJ&ZpyG#faXMww zV{_@KaB<2nX4p>ltX6Dg8~=urY+(o6T|(=lhoNXTBibk-b@^A+xOYRz;AIiIo)N11 zw#V%Bh&n0eWi`B3yOu#&ABwP>?petlSm6j0bhsVEJGg?6Z(sBVmOof9r(9L5ue-(L ze#m0DX^QEI zhW_b)tOdncyn7m z*M@XD>J3w@ljzx9!CnoD0}#~#=mfBF0KW59I?_$|Z{lQ3lIrry7+;y`7~!_J68fHL zzm&0{!Tnq@JrN*k;d*g-M+83r+$o%dB%&|ydkOxu*srh;FR5=$SF@CSU4X92<~-J& zyJujor(o+yMj~cP&BseQQ5_PnRC5SIIj z+_9%<%SROaD<<`8nrHvosD91eG<-^}3XQ=r{^a31%fb7C16!*aSuCp;3CD|ng!5=K z_S<&FcJb7zz}8GQ$yO-GT_{m~08ABqOpYZ?%&xc7=hsk6Bvq&J} z?p!gJbJ)fKtoE0&-i-P%{fuL~*KmYvSGw|*8orCH=Q*)H1yI=in~ULex0dcs!;7|& z0WJSkx_+Nqs0+@^TK1W1HlO3h)z+|9E=!+*iv%Th>+5#w)^284Fo4qc^Ji4Y&sV10 zlbraQ6-F!m2%TqV;cv9vT%O(j9_o<+YRBFm0sDb{18=igQ0|HDsKb@Ng_TRh6W}6U zqfqTZiW_e|tfxR!bL4Y~*P)7(0$&gFa1fxcypk0|gPFv;;3lAa1 zJaY8VW%xXtt{xUee+u-Wh8WzKEU8ATGONIH*!g|=DEXN58jfmKz35Z(d4>7UqCVjI zL*^-A`rRGnfWw;xLkd(PY^x07sF57$9nY(Arw{w>_~qUHoobPGm%5*pAHum{;xnrV z4VIa}k`7}UieT1&d4smfq{QD4x_=fUH;{Jd`A1vac1cFTaIf0>xRuhfXS(;;Pbr!S z3TIAdff9jxAS*1^s4V3nQ93Yqm=5`C4H~f z;Fs#3RUkI}F8cs!zwrd(#>JaK}^9>09yhsSQ8pC4cLWct)rl zUo@g*iG6Skrdpg!RM-_a!LD=30h-vG8h>?9xuw{#raCn68+t>JPp@tPJA)1#E+lwK z{k(HKYdim_t%t2pu2Gu2a(M^&GN(@0In)~j9r9kbUm}@!#(mn@=xg2rU z-DwHuqn)!E-NswyZRfrpZJ44sra87&xy0M;=!y89P`J@L{F-{NSM@ECq6XB|vvz1C zED2hKp@|iAZE90OFj}k7udyF+X@5(9BP(Itt%>}+u5W#7c{AbZW8YQ}JEd7RUv^Cw z*nQh*OjB$3ZX0f=H1Z1My|N#4346zWCfLI~^)6a7xqad5WX3amfEwUEDr5zYO@=QCDQh#L3<*cg$u%+yiHU7JLf#LrpY6Lj>L{wn(x)UQffJgj z9C~XCG~$?=f@B#&rZ7Q}9pw_GEoaM?vxcu6tqZ*$we5#wL9%$z?Wa(&SrJiSu_e7n zcus)K-UR6nG7Z?Jo)DBhL-3ZGxxvvg_Q(oG$V)8dTE|83mL9kPJMyPc?)RHBH6^om zNisJPuws59tsX+HpWsTT<3yySjW!vPF+t~E<;ux6=1a)7hEG}?;?gqF6`x|tm0!YH zP7=iCOmuc!1aBMn0?;vfow&m`Vq+9rDoDgK>{m$$iS4CAGlfIh#Uynwj3>5^kudLEh{1?eIh$XvP5Z(foatNH5Z^K{S1Nj zw)_u${WDqoaGxucNrvl@zQPOK)40)w)-ivrB zW~nLiR~DvUfgJRinx0Ar@tKx(7sQO7uSgdb z?}s9dV&D0g!7jp~>r2=jb4R%kqyz2y4!)USN1o`;CLZkdVft=RHp7jPg24+{DVD$_ zHhA74e9*KrMSmJMtD(cx?Tzb{^v!hk51*6<=%+(qA1Aj(Uo-b{FA_Iqe;l`Fzalqh z4e&k!8vEx_+7%H+d~;?!)1@%lh7f;~%Gw}D^wBDfCFeTVSz}D8p545a`?~E~ ze^dC;df|eFj)eE>(u_b;;{)`%7O>e4|I(s8`Keh?9L_GwD&SI^r^gMKzG+il))r}o z)DCG@Wt|i_@q~tN=^cyM)F~HN-zgo})F~RavIRJ7Vmwt^=Ui}jk3ek3J*DCWkX~T$ z%v}`$E)XLdJ56H}{lTOm$3O@k<2*b?kTq>U+l75Y+ws*I%u6}YFzXX+1du5g=`_N4~p?WfCU zCJzQ@NiKPBY@ZUo5q_mz^4*$(g@WaIbA$`ir*UWQ515!Up9~paXMKRI%69M?fyCa$ z|5MntheNfm;js(36)|pwahK8vNo~`Na+wQ;*=$2fThFt;cU_*fet)dzec#U`uSAIYR-bmU zM27nQ^41+MPd^?lN`G3C>wTh0tEm!Qc6U4NeaSQGXpMbHH6is%3E+!(GUx3nm{z1a z7J1Np5))a{&LWi5QD07Ha$i(>j80r)suG-A!=qV@2?h>2gXFK9i731kP369ZxsDDp zG^ayzAmC~ma!t>P!Y-aZxQ2$YnyAE)=Zp%uXNsNP^{PvS!S$s=;F>Dqy_!;|;JPY9 z#7&_=;i2PJpwBHQcaqT(aOVxm-J!f zOfq9PCjwh+@@Kij<+oxOXGXP2cR8-;^Odc3A8*p~qqnE$N0}l!%y*%U&BM_<8E}j- ztd{A@uwiyH6qvgiFIj`;!RSy}Fs70LWy+e*F=b(Mtn?5m&b+%-QF|rktowC9Dc}*< zGOlURssfD*Q9x6!#Yom31S+EQml5%bUjq^dxoocIaKTx>ah?5|trN$WbF z-f3%ox%76|i&jJjzPJjrC&l92 zlSvujW6?QnuNR{upAi;>s3f-U*k)qR1RODp_?{7-06HR2nCja#5^Bk= zBZaYR7fIl$K`ZtrL02yHjSzcRB?qoCpU~4u)UEHf@PBJTZ;LhbkRJVPBP+fzyzy%> zYoO0kk8Vmxnegrz?d}VI-{#BvV7^zc<4o&#eF8!g8yI2s?o*Ew<5&9woynsOH)Su? zt5pSDY1+ofE>7o!7S@O`va@gA7`nTlrSZ^QDOr$)FHOKA=&~ejMa|Mj__w#8vD5hUe84lX? z+)NVE?W*)bxJ>p`pH@6cpu+Y2#A-MZlaZDvFo`nSCa2Ok=#oRH4K5nHwz5+du~+yZ zi*L5xPr`o@DB=XPWVp1oE&UK*hbuKSZWuqwn$;FO8i~&Wj}mKlTO_z zH>t6-4#MJ0Li88Jm4Oyl53jxY6X<5_i@QT{dZ|MiHM%DylJ{WUM+`0P%Qm)n&~fg1 z#c7COm^MIx9TH6|tjNH#6t`p!c*k>Xm09v-j&99;Uk1(vEyP7I#pyz#rFw_laG=<` z`=7OY=UsT<1rw)`&mPW6r9R+U)!x#EG1GVw;T~A-gNZeinhnQvpR_ScryF{U8=JFg z4~O|R+N+mUBzCcWL#gK1F*{JV!{Q;n+VLJ2l!zLHKp7%!glLoNvL-)r{<>w{e3d^S z8>45rxpVAwt}pTnTc?@jWY;|j1)RG~N*DtnK8CFKf`>4-QWx;fdPL2uBl#L1H`!G} zZqO2qGQ@v%(ci`)4T^zOaZDP&-cRV6zi5VF)cVguFRsw zeeIZDFXdpV!u6wP@}<0r`!ej0l5~#jkWZ3+e&o662#-}d!=`1JkljaJ2UNT$X}@XvJMX1{$W?LZ=lL4m1Nc!Dwr>V zQ|D=2_S5=cT!@&&sOLxLoWDOivbF6)i3PUN@5H5aW3jM0*bTd?)J0D51!ed)fX+#- zk#ek}beWD79GT9drrZX14mPrPzK0C)YN{;2oqSKETGhB2A>8YFZ$f`p993Q>zw;Z{ za`J0qh1M1jD8l>yaV^3Bk*k~x3pfTyRl@v3zo}Dv9G{>cBIIBlwz8W#ruzQ?7v-{z zoDBy`AOuPArqR3s;>K7zkHR4RFB>K(o2s@qWj4jBz1c;MFqU&SGo0y(p2*rAPQR6t zf(0dX8KzmcS*pfq$w}Q*#QU&Q9(XFM8T;oEpEY3fvSZZ4RoBN-_ze=xE}Svh^a=Be zd31+r0?()A4)BS&g;b1WkNfZW9NXalHc$%JnL8K>H5f6Jyy=R!dAv_ayQXf7C?~7g zv(Ir~{ArDoUqd3s+@p96x|(w-XiJmaDdgk0vf=dF6BmPyM>&__4wGe?w2Mr|lsuzm zRNg0TjF+@Jn5I0Wb4h_?leovT+|&lmX2AZ?2y~3qwJdr1CM2 zJ%c=<`~8Cf5!2DgpEzHwMqAkuO5;1yQUVcKMzad=^C6u2AJ!nN@m98`SmFf#2qpHm?DJ4v?4j{smNxd0zkk literal 0 HcmV?d00001 From 4694eaf29d6c7d8ff773e12fa26c54e89ed3c6d8 Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 3 Nov 2017 19:09:15 -0700 Subject: [PATCH 02/38] bloop --- .../org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java index ccde802702f..0d93c2a65ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java @@ -70,15 +70,13 @@ public void runOpMode() { MRI2CGyro.resetZAxisIntegrator(); } LastResState = curResState; - double APwr; - double BPwr; - double CPwr; double angle = MRI2CGyro.getIntegratedZValue(); double drivey = -gamepad1.left_stick_y; double drivex = gamepad1.left_stick_x; double turn = gamepad1.right_stick_x; double time = getRuntime(); - theta += .18*.5*(time-previoustime)*turn; + //drivebase powers + theta += .18*.5*(time-previoustime)*turn; double θerr = angle-theta; double θderiv = (θerr-θpreverr)/(time-previoustime); double θinteg = .5*(time-previoustime)*(θerr+θpreverr); @@ -103,6 +101,7 @@ public void runOpMode() { telemetry.addData("Data", "A (%.2f), B (%.2f), C (%.2f) angle (%.2f)", APwr, BPwr, CPwr, angle); telemetry.addData("MEH too lazy to name this", "temp (%.2f) drvey (%.2f) drivey, drivex (%.2f)", temp, drivey, drivex); telemetry.update(); + } } } From 71dcaea78e0a50b0e2cff6c5da0461e186ba66c7 Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 3 Nov 2017 20:09:26 -0700 Subject: [PATCH 03/38] thought I commit this --- .../org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java index 0d93c2a65ba..9089528fc1e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java @@ -64,6 +64,9 @@ public void runOpMode() { double θkp = .02; double θki = .0000001; double θkd = .00001; + double APwr; + double BPwr; + double CPwr; while (opModeIsActive()) { curResState = gamepad1.a && gamepad1.b; if(LastResState && !curResState){ From 5a256adfc2b458b5907da118d7d1eb2e96318233 Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 17 Nov 2017 21:17:45 -0800 Subject: [PATCH 04/38] I think this is what was done at comp --- .../ftc/teamcode/BasicOpMode_Linear.java | 43 +++++- .../Overly_shoddy_ultra_last_minute_auto.java | 20 +++ .../ftc/teamcode/ShoddierTeleOP.java | 133 ++++++++++++++++++ 3 files changed, 193 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Overly_shoddy_ultra_last_minute_auto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java index 9089528fc1e..eb09b3d0ce6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; import Jama.Matrix; @@ -24,6 +25,11 @@ public class BasicOpMode_Linear extends LinearOpMode { private DcMotor CDrive; private IntegratingGyroscope gyro; private ModernRoboticsI2cGyro MRI2CGyro; + private DcMotor Shoulder; + private DcMotor Elbow; + private Servo RF; + private Servo LF; + private Servo Wrist; @Override public void runOpMode() { telemetry.addData("Status", "Initialized"); @@ -35,11 +41,15 @@ public void runOpMode() { ADrive = hardwareMap.get(DcMotor.class, "A"); BDrive = hardwareMap.get(DcMotor.class, "B"); CDrive = hardwareMap.get(DcMotor.class, "C"); + Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); + Elbow = hardwareMap.get(DcMotor.class, "elbow"); + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Wrist = hardwareMap.get(Servo.class, "wrist"); MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); gyro = (IntegratingGyroscope)MRI2CGyro; telemetry.log().add("Gyro Calibrating. Do Not Move!"); MRI2CGyro.calibrate(); - // Wait until the gyro calibration is complete timer.reset(); while (!isStopRequested() && MRI2CGyro.isCalibrating()) { @@ -67,6 +77,12 @@ public void runOpMode() { double APwr; double BPwr; double CPwr; + double Fingeroffset = 0; + double Wristoffset = 0; + boolean b = false; + boolean x = false; + boolean Rbumper = false; + boolean Lbumper = true; while (opModeIsActive()) { curResState = gamepad1.a && gamepad1.b; if(LastResState && !curResState){ @@ -79,7 +95,7 @@ public void runOpMode() { double turn = gamepad1.right_stick_x; double time = getRuntime(); //drivebase powers - theta += .18*.5*(time-previoustime)*turn; + theta += .01*.5*(time-previoustime)*turn; double θerr = angle-theta; double θderiv = (θerr-θpreverr)/(time-previoustime); double θinteg = .5*(time-previoustime)*(θerr+θpreverr); @@ -104,7 +120,28 @@ public void runOpMode() { telemetry.addData("Data", "A (%.2f), B (%.2f), C (%.2f) angle (%.2f)", APwr, BPwr, CPwr, angle); telemetry.addData("MEH too lazy to name this", "temp (%.2f) drvey (%.2f) drivey, drivex (%.2f)", temp, drivey, drivex); telemetry.update(); - + //Shoddy last minute arm code + Elbow.setPower(.1*Range.clip(-1*gamepad2.left_stick_y,-1,1)); + Shoulder.setPower(.1*Range.clip(-1*gamepad2.right_stick_y, -1,1)); + if(gamepad2.b && !b){ + Fingeroffset += .1; + } + if(gamepad2.x && !x){ + Fingeroffset -= .1; + } + if(gamepad2.right_bumper && !Rbumper){ + Wristoffset += .1; + } + if(gamepad2.left_bumper && !Lbumper){ + Wristoffset -= .1; + } + RF.setPosition(Range.clip(0.5 - Fingeroffset, 0,1)); + LF.setPosition(Range.clip(Fingeroffset + 0.5, 0,1)); + Wrist.setPosition(Range.clip(Wristoffset + 0.5, 0,1)); + x = gamepad2.x; + b = gamepad2.b; + Rbumper = gamepad2.right_bumper; + Lbumper = gamepad2.left_bumper; } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Overly_shoddy_ultra_last_minute_auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Overly_shoddy_ultra_last_minute_auto.java new file mode 100644 index 00000000000..416e5e2c1c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Overly_shoddy_ultra_last_minute_auto.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.hardware.DcMotor; + +/** + * Created by jxfio on 11/4/2017. + */ + +@Autonomous(name="Overly Shoddy", group="Shoddy") +@Disabled +public class Overly_shoddy_ultra_last_minute_auto { + private DcMotor B; + private DcMotor C; + + public void runOpMode() { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java new file mode 100644 index 00000000000..e36d7854a26 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java @@ -0,0 +1,133 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +import Jama.Matrix; +import java.lang.Math; + +import static java.lang.Math.cos; +import static java.lang.Math.sin; + +@TeleOp(name="Shoddier Tele OP", group="Linear Opmode") +public class ShoddierTeleOP extends LinearOpMode { + + // Declare OpMode members. + ElapsedTime timer = new ElapsedTime(); + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor ADrive; + private DcMotor BDrive; + private DcMotor CDrive; + private IntegratingGyroscope gyro; + private ModernRoboticsI2cGyro MRI2CGyro; + private DcMotor Shoulder; + private DcMotor Elbow; + private Servo RF; + private Servo LF; + private Servo Wrist; + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + ADrive = hardwareMap.get(DcMotor.class, "A"); + BDrive = hardwareMap.get(DcMotor.class, "B"); + CDrive = hardwareMap.get(DcMotor.class, "C"); + Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); + Elbow = hardwareMap.get(DcMotor.class, "elbow"); + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Wrist = hardwareMap.get(Servo.class, "wrist"); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + MRI2CGyro.calibrate(); + // Wait until the gyro calibration is complete + timer.reset(); + while (!isStopRequested() && MRI2CGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + waitForStart(); + runtime.reset(); + telemetry.log().add("Press A & B to reset heading"); + // run until the end of the match (driver presses STOP) + double temp; + double APwr; + double BPwr; + double CPwr; + double Fingeroffset = 0; + double Wristoffset = 0; + boolean Lb = false; + boolean Lx = false; + boolean LRbumper = false; + boolean LLbumper = false; + while (opModeIsActive()) { + double drivey = -gamepad1.left_stick_y; + double drivex = gamepad1.left_stick_x; + double turn = gamepad1.right_stick_x; + double time = getRuntime(); + //drivebase powers + APwr= drivex+drivey+turn; + BPwr= drivex*cos((120)*Math.PI/180)+drivey*sin((120)*Math.PI/180)+turn; + CPwr= drivex*cos((-120)*Math.PI/180)+drivey*sin((-120)*Math.PI/180)+turn; + if(Math.max(Math.abs(APwr), Math.max(Math.abs(BPwr), Math.abs(CPwr)))>1) { + temp = 1 / Math.max(Math.abs(APwr), Math.max(Math.abs(BPwr), Math.abs(CPwr))); + }else{ + temp=1; + } + APwr = APwr*temp; + BPwr = BPwr*temp; + CPwr = CPwr*temp; + // Send calculated power to wheels + ADrive.setPower(Range.clip(APwr, -1, 1)); + BDrive.setPower(Range.clip(BPwr, -1, 1)); + CDrive.setPower(Range.clip(CPwr, -1, 1)); + //Shoddy arm code + Elbow.setPower(.1*Range.clip(-1*gamepad2.left_stick_y,-1,1)); + Shoulder.setPower(.1*Range.clip(-1*gamepad2.right_stick_y, -1,1)); + boolean b = gamepad2.b; + if(b && !Lb){ + Fingeroffset += .1; + } + Lb = b; + boolean x = gamepad2.x; + if(Lx && x){ + Fingeroffset -= .1; + } + Lx = x; + boolean Rbumper = gamepad2.right_bumper; + if(Rbumper && !LRbumper){ + Wristoffset += .1; + } + LRbumper = Rbumper; + boolean Lbumper = gamepad2.left_bumper; + if(Lbumper && !LLbumper){ + Wristoffset -= .1; + } + LLbumper = Lbumper; + if(Math.abs(Fingeroffset)>.5){ + Fingeroffset = .5*Math.abs(Fingeroffset)/Fingeroffset; + } + if(Math.abs(Wristoffset)>.5){ + Wristoffset = .5*Math.abs(Wristoffset)/Wristoffset; + } + RF.setPosition(Range.clip(0.5 - Fingeroffset, 0,1)); + LF.setPosition(Range.clip(Fingeroffset + 0.5, 0,1)); + Wrist.setPosition(Range.clip(Wristoffset + 0.5, 0,1)); + } + } +} \ No newline at end of file From e040013aec3a918464c608ac98416656296c8cd2 Mon Sep 17 00:00:00 2001 From: tazule Date: Sat, 2 Dec 2017 12:23:27 -0800 Subject: [PATCH 05/38] edits to teleop --- .../firstinspires/ftc/teamcode/robot2.java | 104 ++++++++++++++++++ 1 file changed, 104 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java new file mode 100644 index 00000000000..a6aa2413186 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java @@ -0,0 +1,104 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import static java.lang.Math.abs; +import static java.lang.Math.cos; +import static java.lang.Math.sin; + +/** + * Created by jxfio on 11/17/2017. + */ + +@TeleOp(name="robot2", group="robot2") +public class robot2 extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private DcMotor Tower; + private CRServo Slack; + private Servo RF; + private Servo LF; + @Override + public void runOpMode() { + + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + Tower = hardwareMap.get(DcMotor.class, "tower"); + Slack = hardwareMap.get(CRServo.class, "slack"); + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + + waitForStart(); + // run until the end of the match (driver presses STOP) + + double Fingeroffset = 0; + boolean b = false; + boolean x = false; + double factor=1; + double bonus = 0; + double position = 0; + double towerpower; + double max = 4416; + while (opModeIsActive()) { + bonus = 0; + towerpower = -gamepad2.left_stick_y; + /*if(towerpower>0){ + if (position>0&&position0&&positionmax){ + bonus -= .25; + } + //towerpower = (bonus+factor*towerpower);*/ + //towerpower += bonus; + Left.setPower(.5*Range.clip(gamepad1.left_stick_y, -1, 1)); + Right.setPower(.5*Range.clip(-gamepad1.right_stick_y, -1, 1)); + Tower.setPower(.5 * Range.clip(towerpower, -1, 1)); + Slack.setPower(-.5 *Range.clip((towerpower), -1, 1)); + //finger adjustment statements + if(gamepad2.b && !b && Fingeroffset <= 1){ + Fingeroffset += .1; + } + if(gamepad2.x && !x && Fingeroffset >= -1 ){ + Fingeroffset -= .1; + } + RF.setPosition(Range.clip(0.5 - Fingeroffset, -1,1)); + LF.setPosition(Range.clip(0.5 + Fingeroffset, -1,1)); + x = gamepad2.x; + b = gamepad2.b; + //Finger controls + telemetry.addData("Path1",String.valueOf(Tower.getCurrentPosition())+" " + String.valueOf(factor)+" " + String.valueOf(bonus) + " " + String.valueOf(towerpower)); + telemetry.update(); + + } + } +} From 6512aaa52a5944bdc5be0b1dcd97830d3e00eab6 Mon Sep 17 00:00:00 2001 From: tazule Date: Sun, 3 Dec 2017 15:07:56 -0800 Subject: [PATCH 06/38] beep boop --- .../ftc/teamcode/absoluteCrapAuto.java | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/absoluteCrapAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/absoluteCrapAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/absoluteCrapAuto.java new file mode 100644 index 00000000000..5c7fdb50e51 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/absoluteCrapAuto.java @@ -0,0 +1,37 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +/** + * Created by jxfio on 12/2/2017. + */ + +@Autonomous(name="Crap", group="robot2") +public class absoluteCrapAuto extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + @Override + public void runOpMode() { + + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + waitForStart(); + // run until the end of the match (driver presses STOP) + + while (opModeIsActive()) { + if(getRuntime()<1000){ + Left.setPower(.5); + Right.setPower(-.5); + } + else { + Left.setPower(0); + Right.setPower(0); + } + } + } +} From 03ec5b35cb27466f13589d132eabb59d9e93c7aa Mon Sep 17 00:00:00 2001 From: tazule Date: Thu, 14 Dec 2017 19:33:39 -0800 Subject: [PATCH 07/38] changed offset values --- .../org/firstinspires/ftc/teamcode/robot2.java | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java index a6aa2413186..970357045cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java @@ -60,8 +60,8 @@ public void runOpMode() { }else{ position=max; } - //factor = Range.clip((2/(1+(3^(Tower.getCurrentPosition()-4416))))-1,-1,1); - }else if (towerpower == 0){ + //factor = Range.clip((2/(1+(3^(Tower.getCurrentPosition()-max))))-1,-1,1); + }else if (Math.abs(towerpower) < .01){ bonus = .01*(position-Tower.getCurrentPosition()); }else{ if (position>0&&positionmax){ - bonus -= .25; + towerpower = -1; } + /* //towerpower = (bonus+factor*towerpower);*/ //towerpower += bonus; Left.setPower(.5*Range.clip(gamepad1.left_stick_y, -1, 1)); @@ -85,10 +87,10 @@ public void runOpMode() { Tower.setPower(.5 * Range.clip(towerpower, -1, 1)); Slack.setPower(-.5 *Range.clip((towerpower), -1, 1)); //finger adjustment statements - if(gamepad2.b && !b && Fingeroffset <= 1){ + if(gamepad2.b && !b && Fingeroffset <= .4){ Fingeroffset += .1; } - if(gamepad2.x && !x && Fingeroffset >= -1 ){ + if(gamepad2.x && !x && Fingeroffset >= .1 ){ Fingeroffset -= .1; } RF.setPosition(Range.clip(0.5 - Fingeroffset, -1,1)); @@ -96,7 +98,8 @@ public void runOpMode() { x = gamepad2.x; b = gamepad2.b; //Finger controls - telemetry.addData("Path1",String.valueOf(Tower.getCurrentPosition())+" " + String.valueOf(factor)+" " + String.valueOf(bonus) + " " + String.valueOf(towerpower)); + telemetry.addData("Path1","Tower position: " + String.valueOf(Tower.getCurrentPosition())+" factor: " + String.valueOf(factor)+"bonus: " + String.valueOf(bonus) + "tower power: " + String.valueOf(towerpower)+ " position: " + String.valueOf(position)); + telemetry.addData("Path2","Finger offset: " + String.valueOf(Fingeroffset)); telemetry.update(); } From 0159e16f13d940c08ae7b9205170bb8f7b42b1a6 Mon Sep 17 00:00:00 2001 From: tazule Date: Thu, 14 Dec 2017 20:48:34 -0800 Subject: [PATCH 08/38] tower limits kind of working --- .../firstinspires/ftc/teamcode/robot2.java | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java index 970357045cc..f5baf651630 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java @@ -40,7 +40,8 @@ public void runOpMode() { waitForStart(); // run until the end of the match (driver presses STOP) - + boolean limitoff=false; + boolean y = false; double Fingeroffset = 0; boolean b = false; boolean x = false; @@ -74,9 +75,13 @@ public void runOpMode() { //factor = Range.clip((2/(1+(3^(-Tower.getCurrentPosition()))))-1,-1,1); } */ - if (Tower.getCurrentPosition()<0){ - towerpower= 1; - }else if(Tower.getCurrentPosition()>max){ + if (gamepad2.y && !y){ + limitoff = !limitoff; + } + y = gamepad2.y; + if (Tower.getCurrentPosition()<0 && !limitoff){ + towerpower= .01*(0-Tower.getCurrentPosition()); + }else if(Tower.getCurrentPosition()>max && !limitoff ){ towerpower = -1; } /* @@ -98,10 +103,14 @@ public void runOpMode() { x = gamepad2.x; b = gamepad2.b; //Finger controls + if (limitoff){ + telemetry.addData("path 0","limits off"); + }else { + telemetry.addData("path 0","limits on"); + } telemetry.addData("Path1","Tower position: " + String.valueOf(Tower.getCurrentPosition())+" factor: " + String.valueOf(factor)+"bonus: " + String.valueOf(bonus) + "tower power: " + String.valueOf(towerpower)+ " position: " + String.valueOf(position)); - telemetry.addData("Path2","Finger offset: " + String.valueOf(Fingeroffset)); + telemetry.addData("Path2","Finger offset: " + String.valueOf(Fingeroffset) + " y: "+ String.valueOf(y)); telemetry.update(); - } } } From 5d1607258b73c749f492ea7016a3463f29eecbaf Mon Sep 17 00:00:00 2001 From: tazule Date: Thu, 14 Dec 2017 20:56:01 -0800 Subject: [PATCH 09/38] bug fix --- .../src/main/java/org/firstinspires/ftc/teamcode/robot2.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java index f5baf651630..651b67253d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java @@ -80,7 +80,7 @@ public void runOpMode() { } y = gamepad2.y; if (Tower.getCurrentPosition()<0 && !limitoff){ - towerpower= .01*(0-Tower.getCurrentPosition()); + towerpower= Range.clip(towerpower,0,1) + .01*(0-Tower.getCurrentPosition()); }else if(Tower.getCurrentPosition()>max && !limitoff ){ towerpower = -1; } From cef437a08d3a6a3032a0bfd4c0a32e993aae7bd1 Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 15 Dec 2017 20:22:08 -0800 Subject: [PATCH 10/38] IT LIVES got rid of some pointless things and got the autonomi working --- .../ftc/teamcode/BasicOpMode_Linear.java | 147 ------------------ .../firstinspires/ftc/teamcode/BlueRight.java | 46 ++++++ .../firstinspires/ftc/teamcode/Driver.java | 10 ++ .../ftc/teamcode/DriverNoEncoder.java | 35 +++++ .../ftc/teamcode/DriverWithEncoder.java | 63 ++++++++ .../ftc/teamcode/LITERALY_JUST_THE_ARM.java | 30 ---- .../Overly_shoddy_ultra_last_minute_auto.java | 20 --- .../firstinspires/ftc/teamcode/RedLeft.java | 46 ++++++ .../ftc/teamcode/ShoddierTeleOP.java | 3 +- .../ftc/teamcode/absoluteCrapAuto.java | 37 ----- 10 files changed, 202 insertions(+), 235 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Driver.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverNoEncoder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverWithEncoder.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LITERALY_JUST_THE_ARM.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Overly_shoddy_ultra_last_minute_auto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/absoluteCrapAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java deleted file mode 100644 index eb09b3d0ce6..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BasicOpMode_Linear.java +++ /dev/null @@ -1,147 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.IntegratingGyroscope; -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.Range; -import Jama.Matrix; -import java.lang.Math; - -import static java.lang.Math.cos; -import static java.lang.Math.sin; - -@TeleOp(name="Drivebase", group="Linear Opmode") -public class BasicOpMode_Linear extends LinearOpMode { - - // Declare OpMode members. - ElapsedTime timer = new ElapsedTime(); - private ElapsedTime runtime = new ElapsedTime(); - private DcMotor ADrive; - private DcMotor BDrive; - private DcMotor CDrive; - private IntegratingGyroscope gyro; - private ModernRoboticsI2cGyro MRI2CGyro; - private DcMotor Shoulder; - private DcMotor Elbow; - private Servo RF; - private Servo LF; - private Servo Wrist; - @Override - public void runOpMode() { - telemetry.addData("Status", "Initialized"); - telemetry.update(); - - // Initialize the hardware variables. Note that the strings used here as parameters - // to 'get' must correspond to the names assigned during the robot configuration - // step (using the FTC Robot Controller app on the phone). - ADrive = hardwareMap.get(DcMotor.class, "A"); - BDrive = hardwareMap.get(DcMotor.class, "B"); - CDrive = hardwareMap.get(DcMotor.class, "C"); - Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); - Elbow = hardwareMap.get(DcMotor.class, "elbow"); - RF = hardwareMap.get(Servo.class, "right finger"); - LF = hardwareMap.get(Servo.class, "left finger"); - Wrist = hardwareMap.get(Servo.class, "wrist"); - MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); - gyro = (IntegratingGyroscope)MRI2CGyro; - telemetry.log().add("Gyro Calibrating. Do Not Move!"); - MRI2CGyro.calibrate(); - // Wait until the gyro calibration is complete - timer.reset(); - while (!isStopRequested() && MRI2CGyro.isCalibrating()) { - telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); - telemetry.update(); - sleep(50); - } - - telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); - telemetry.clear(); telemetry.update(); - - waitForStart(); - runtime.reset(); - telemetry.log().add("Press A & B to reset heading"); - // run until the end of the match (driver presses STOP) - boolean curResState = false; - boolean LastResState = false; - double temp; - double previoustime = getRuntime(); - double theta = 0; - double θpreverr = 0; - double θkp = .02; - double θki = .0000001; - double θkd = .00001; - double APwr; - double BPwr; - double CPwr; - double Fingeroffset = 0; - double Wristoffset = 0; - boolean b = false; - boolean x = false; - boolean Rbumper = false; - boolean Lbumper = true; - while (opModeIsActive()) { - curResState = gamepad1.a && gamepad1.b; - if(LastResState && !curResState){ - MRI2CGyro.resetZAxisIntegrator(); - } - LastResState = curResState; - double angle = MRI2CGyro.getIntegratedZValue(); - double drivey = -gamepad1.left_stick_y; - double drivex = gamepad1.left_stick_x; - double turn = gamepad1.right_stick_x; - double time = getRuntime(); - //drivebase powers - theta += .01*.5*(time-previoustime)*turn; - double θerr = angle-theta; - double θderiv = (θerr-θpreverr)/(time-previoustime); - double θinteg = .5*(time-previoustime)*(θerr+θpreverr); - double θmotor = θkp*θerr+θki*θinteg+θkd*θderiv; - APwr= drivex*cos((angle)*Math.PI/180)+drivey*sin((angle)*Math.PI/180)+θmotor; - BPwr= drivex*cos((120+angle)*Math.PI/180)+drivey*sin((120+angle)*Math.PI/180)+θmotor; - CPwr= drivex*cos((angle-120)*Math.PI/180)+drivey*sin((angle-120)*Math.PI/180)+θmotor; - if(Math.max(Math.abs(APwr), Math.max(Math.abs(BPwr), Math.abs(CPwr)))>1) { - temp = 1 / Math.max(Math.abs(APwr), Math.max(Math.abs(BPwr), Math.abs(CPwr))); - }else{ - temp=1; - } - APwr = APwr*temp; - BPwr = BPwr*temp; - CPwr = CPwr*temp; - // Send calculated power to wheels - ADrive.setPower(Range.clip(APwr, -1, 1)); - BDrive.setPower(Range.clip(BPwr, -1, 1)); - CDrive.setPower(Range.clip(CPwr, -1, 1)); - // Show the elapsed game time and wheel power. - telemetry.addData("Status", "Run Time: " + runtime.toString()); - telemetry.addData("Data", "A (%.2f), B (%.2f), C (%.2f) angle (%.2f)", APwr, BPwr, CPwr, angle); - telemetry.addData("MEH too lazy to name this", "temp (%.2f) drvey (%.2f) drivey, drivex (%.2f)", temp, drivey, drivex); - telemetry.update(); - //Shoddy last minute arm code - Elbow.setPower(.1*Range.clip(-1*gamepad2.left_stick_y,-1,1)); - Shoulder.setPower(.1*Range.clip(-1*gamepad2.right_stick_y, -1,1)); - if(gamepad2.b && !b){ - Fingeroffset += .1; - } - if(gamepad2.x && !x){ - Fingeroffset -= .1; - } - if(gamepad2.right_bumper && !Rbumper){ - Wristoffset += .1; - } - if(gamepad2.left_bumper && !Lbumper){ - Wristoffset -= .1; - } - RF.setPosition(Range.clip(0.5 - Fingeroffset, 0,1)); - LF.setPosition(Range.clip(Fingeroffset + 0.5, 0,1)); - Wrist.setPosition(Range.clip(Wristoffset + 0.5, 0,1)); - x = gamepad2.x; - b = gamepad2.b; - Rbumper = gamepad2.right_bumper; - Lbumper = gamepad2.left_bumper; - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java new file mode 100644 index 00000000000..10be5a23e80 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +/** + * Created by jxfio on 12/2/2017. + */ + +@Autonomous(name="Blue Right", group="robot2") +public class BlueRight extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private Servo RF; + private Servo LF; + @Override + public void runOpMode() { + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + waitForStart(); + Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); + // run until the end of the match (driver presses STOP) + Left.setDirection(DcMotor.Direction.REVERSE); + RF.setPosition(.5); + LF.setPosition(.5); + driver.forward(31,.5); + driver.turn(90,.2); + RF.setPosition(.3); + LF.setPosition(.7); + driver.forward(15,.5); + while (opModeIsActive()) { + driver.forward(-1,.5); + driver.turn(40,.4); + driver.forward(3,.5); + driver.turn(-40,.4); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Driver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Driver.java new file mode 100644 index 00000000000..6378ac884ba --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Driver.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode; + +/** + * Created by jxfio on 12/15/2017. + */ + +public abstract class Driver { + public abstract void forward(double distance, double power); + public abstract void turn(double degrees, double power); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverNoEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverNoEncoder.java new file mode 100644 index 00000000000..8950cad7ece --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverNoEncoder.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/** + * Created by jxfio on 12/15/2017. + */ + +public class DriverNoEncoder extends Driver { + public DcMotor leftMotor; + public DcMotor rightMotor; + + public DriverNoEncoder(DcMotor left, DcMotor right){ + leftMotor = left; + rightMotor = right; + } + //by distance we mean time + public void forward(double distance, double power) { + ElapsedTime runtime = new ElapsedTime(); + while (runtime.milliseconds() < distance) { + leftMotor.setPower(power); + rightMotor.setPower(-power); + } + } + //by degrees we mean time + public void turn(double degrees, double power) { + ElapsedTime runtime = new ElapsedTime(); + while (runtime.milliseconds() < degrees) { + leftMotor.setPower(power); + rightMotor.setPower(power); + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverWithEncoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverWithEncoder.java new file mode 100644 index 00000000000..caeee4daa5d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverWithEncoder.java @@ -0,0 +1,63 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.Range; + +/** + * Created by jxfio on 12/15/2017. + */ + +public class DriverWithEncoder extends Driver{ + public DcMotor leftMotor; + public DcMotor rightMotor; + public double wheelradius; + public double ticsPerRevolution = 1416; + public double wheelSeperation; + public DriverWithEncoder(DcMotor left, DcMotor right, double radius, double wheelSep){ + leftMotor = left; + rightMotor = right; + wheelradius = radius; + leftMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + wheelSeperation = wheelSep; + } + public void forward(double distance, double power){ + double wheelcirc = 2 * Math.PI * wheelradius; + double distanceInTics = (distance/wheelcirc)* ticsPerRevolution; + double RightTicsGoal = distanceInTics + rightMotor.getCurrentPosition(); + double LeftTicsGoal = distanceInTics + leftMotor.getCurrentPosition(); + double RTtG = RightTicsGoal - rightMotor.getCurrentPosition(); + double LTtG = LeftTicsGoal - leftMotor.getCurrentPosition(); + double eps = 5; + leftMotor.setTargetPosition(((int)LeftTicsGoal)); + rightMotor.setTargetPosition((int)RightTicsGoal); + leftMotor.setPower(power); + rightMotor.setPower(power); + while (Math.abs(RTtG)>eps && Math.abs(LTtG)>eps){ + RTtG = RightTicsGoal - rightMotor.getCurrentPosition(); + LTtG = LeftTicsGoal - leftMotor.getCurrentPosition(); + } + leftMotor.setPower(0); + rightMotor.setPower(0); + } + public void turn (double degrees, double power){ + double arcLength = 1.2*degrees*wheelSeperation*Math.PI/360; + double wheelcirc = 2 * Math.PI * wheelradius; + double arcLengthInTics = (arcLength/wheelcirc)* ticsPerRevolution; + double RightTicsGoal = rightMotor.getCurrentPosition() + arcLengthInTics; + double LeftTicsGoal = leftMotor.getCurrentPosition() - arcLengthInTics; + double RTtG = RightTicsGoal - rightMotor.getCurrentPosition(); + double LTtG = LeftTicsGoal - leftMotor.getCurrentPosition(); + double eps = 5; + leftMotor.setTargetPosition(((int)LeftTicsGoal)); + rightMotor.setTargetPosition((int)RightTicsGoal); + leftMotor.setPower(power); + rightMotor.setPower(power); + while (Math.abs(RTtG)>eps && Math.abs(LTtG)>eps){ + RTtG = RightTicsGoal - rightMotor.getCurrentPosition(); + LTtG = LeftTicsGoal - leftMotor.getCurrentPosition(); + } + leftMotor.setPower(0); + rightMotor.setPower(0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LITERALY_JUST_THE_ARM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LITERALY_JUST_THE_ARM.java deleted file mode 100644 index b8934fba2b3..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LITERALY_JUST_THE_ARM.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.util.Range; - -/** - * Created by jxfio on 9/30/2017. - */ -@TeleOp(name="Literaly just the arm", group="Linear Opmode") -public class LITERALY_JUST_THE_ARM extends LinearOpMode { - public DcMotor arm1 = null; - @Override - - public void runOpMode() { - telemetry.addData("Status", "Initialized"); - telemetry.update(); - - // Initialize the hardware variables. Note that the strings used here as parameters - // to 'get' must correspond to the names assigned during the robot configuration - // step (using the FTC Robot Controller app on the phone). - arm1 = hardwareMap.get(DcMotor.class, "arm"); - waitForStart(); - // run until the end of the match (driver presses STOP) - while (opModeIsActive()) { - arm1.setPower(Range.clip(gamepad2.left_stick_y,-1,1)); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Overly_shoddy_ultra_last_minute_auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Overly_shoddy_ultra_last_minute_auto.java deleted file mode 100644 index 416e5e2c1c5..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Overly_shoddy_ultra_last_minute_auto.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.hardware.DcMotor; - -/** - * Created by jxfio on 11/4/2017. - */ - -@Autonomous(name="Overly Shoddy", group="Shoddy") -@Disabled -public class Overly_shoddy_ultra_last_minute_auto { - private DcMotor B; - private DcMotor C; - - public void runOpMode() { - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java new file mode 100644 index 00000000000..890d1ed8f8a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +/** + * Created by jxfio on 12/15/2017. + */ + +@Autonomous(name="Blue Right", group="robot2") +public class RedLeft extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private Servo RF; + private Servo LF; + @Override + public void runOpMode() { + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + waitForStart(); + Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); + // run until the end of the match (driver presses STOP) + Left.setDirection(DcMotor.Direction.REVERSE); + RF.setPosition(.5); + LF.setPosition(.5); + driver.forward(31,.5); + driver.turn(90,.2); + RF.setPosition(.3); + LF.setPosition(.7); + driver.forward(15,.5); + while (opModeIsActive()) { + driver.forward(-1,.5); + driver.turn(40,.4); + driver.forward(3,.5); + driver.turn(-40,.4); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java index e36d7854a26..469db86c475 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -13,7 +14,7 @@ import static java.lang.Math.cos; import static java.lang.Math.sin; - +@Disabled @TeleOp(name="Shoddier Tele OP", group="Linear Opmode") public class ShoddierTeleOP extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/absoluteCrapAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/absoluteCrapAuto.java deleted file mode 100644 index 5c7fdb50e51..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/absoluteCrapAuto.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; - -/** - * Created by jxfio on 12/2/2017. - */ - -@Autonomous(name="Crap", group="robot2") -public class absoluteCrapAuto extends LinearOpMode { - - // Declare OpMode members. - private DcMotor Left; - private DcMotor Right; - @Override - public void runOpMode() { - - Left = hardwareMap.get(DcMotor.class, "left"); - Right = hardwareMap.get(DcMotor.class, "right"); - - waitForStart(); - // run until the end of the match (driver presses STOP) - - while (opModeIsActive()) { - if(getRuntime()<1000){ - Left.setPower(.5); - Right.setPower(-.5); - } - else { - Left.setPower(0); - Right.setPower(0); - } - } - } -} From c247bc7db059e1415925bf660606a1fef6585ce3 Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 15 Dec 2017 20:50:10 -0800 Subject: [PATCH 11/38] bug fixes --- .../firstinspires/ftc/teamcode/BlueLeft.java | 36 +++++++++++++++++++ .../firstinspires/ftc/teamcode/BlueRight.java | 5 +++ .../firstinspires/ftc/teamcode/RedLeft.java | 5 +++ .../firstinspires/ftc/teamcode/robot2.java | 4 +-- 4 files changed, 48 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java new file mode 100644 index 00000000000..069a03b7164 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +/** + * Created by jxfio on 12/15/2017. + */ + +public class BlueLeft extends LinearOpMode{ + private DcMotor Left; + private DcMotor Right; + private Servo RF; + private Servo LF; + @Override + public void runOpMode() { + RF = hardwareMap.get(Servo.class, "right finger"); + LF = hardwareMap.get(Servo.class, "left finger"); + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + waitForStart(); + Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); + // run until the end of the match (driver presses STOP) + RF.setPosition(.3); + LF.setPosition(.7); + driver.forward(34,.5); + while (opModeIsActive()) { + driver.forward(-1,.5); + driver.turn(40,.4); + driver.forward(3,.5); + driver.turn(-40,.4); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java index 10be5a23e80..68449b8acdb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java @@ -29,9 +29,14 @@ public void runOpMode() { Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); // run until the end of the match (driver presses STOP) Left.setDirection(DcMotor.Direction.REVERSE); + //if you need to do keep it the right size comment 33/34 uncomment 35/36/38/39 RF.setPosition(.5); LF.setPosition(.5); + //RF.setPosition(.3); + //LF.setPosition(.7); driver.forward(31,.5); + //RF.setPosition(.5); + //LF.setPosition(.5); driver.turn(90,.2); RF.setPosition(.3); LF.setPosition(.7); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java index 890d1ed8f8a..a10ef94b720 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java @@ -29,9 +29,14 @@ public void runOpMode() { Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); // run until the end of the match (driver presses STOP) Left.setDirection(DcMotor.Direction.REVERSE); + //if you need to do keep it the right size comment 33/34 uncomment 35/36/38/39 RF.setPosition(.5); LF.setPosition(.5); + //RF.setPosition(.3); + //LF.setPosition(.7); driver.forward(31,.5); + //RF.setPosition(.5); + //LF.setPosition(.5); driver.turn(90,.2); RF.setPosition(.3); LF.setPosition(.7); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java index 651b67253d2..55f79fecb46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot2.java @@ -89,8 +89,8 @@ public void runOpMode() { //towerpower += bonus; Left.setPower(.5*Range.clip(gamepad1.left_stick_y, -1, 1)); Right.setPower(.5*Range.clip(-gamepad1.right_stick_y, -1, 1)); - Tower.setPower(.5 * Range.clip(towerpower, -1, 1)); - Slack.setPower(-.5 *Range.clip((towerpower), -1, 1)); + Tower.setPower(.25 * Range.clip(towerpower, -1, 1)); + Slack.setPower(-.25 *Range.clip((towerpower), -1, 1)); //finger adjustment statements if(gamepad2.b && !b && Fingeroffset <= .4){ Fingeroffset += .1; From 51dd408843a305761574576f01764fc8e860c6df Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 15 Dec 2017 21:24:14 -0800 Subject: [PATCH 12/38] bug fixes bug fixes on the dancing --- .../java/org/firstinspires/ftc/teamcode/BlueLeft.java | 8 +++++++- .../java/org/firstinspires/ftc/teamcode/BlueRight.java | 4 ++++ .../main/java/org/firstinspires/ftc/teamcode/RedLeft.java | 6 +++++- 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java index 069a03b7164..c9e8740d7d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java @@ -1,13 +1,15 @@ package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; /** * Created by jxfio on 12/15/2017. */ - +@Autonomous(name = "Blue Left/Red Right", group = "robot2") public class BlueLeft extends LinearOpMode{ private DcMotor Left; private DcMotor Right; @@ -31,6 +33,10 @@ public void runOpMode() { driver.turn(40,.4); driver.forward(3,.5); driver.turn(-40,.4); + driver.forward(3,.5); + driver.turn(40,.4); + driver.forward(-1,.5); + driver.turn(-40,.4); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java index 68449b8acdb..01bef87b9f9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueRight.java @@ -46,6 +46,10 @@ public void runOpMode() { driver.turn(40,.4); driver.forward(3,.5); driver.turn(-40,.4); + driver.forward(3,.5); + driver.turn(40,.4); + driver.forward(-1,.5); + driver.turn(-40,.4); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java index a10ef94b720..13c9fc7364d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java @@ -10,7 +10,7 @@ * Created by jxfio on 12/15/2017. */ -@Autonomous(name="Blue Right", group="robot2") +@Autonomous(name="Red Left", group="robot2") public class RedLeft extends LinearOpMode { // Declare OpMode members. @@ -46,6 +46,10 @@ public void runOpMode() { driver.turn(40,.4); driver.forward(3,.5); driver.turn(-40,.4); + driver.forward(3,.5); + driver.turn(40,.4); + driver.forward(-1,.5); + driver.turn(-40,.4); } } } From 074e13f9c10f3a3c9e2f2a2a5c64c057ff007883 Mon Sep 17 00:00:00 2001 From: tazule Date: Sat, 16 Dec 2017 07:06:42 -0800 Subject: [PATCH 13/38] bug fixes --- .../main/java/org/firstinspires/ftc/teamcode/BlueLeft.java | 5 ++++- .../main/java/org/firstinspires/ftc/teamcode/RedLeft.java | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java index c9e8740d7d3..f1b6e22e128 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueLeft.java @@ -24,10 +24,13 @@ public void runOpMode() { waitForStart(); Driver driver = new DriverWithEncoder(Left,Right,2.5, 15.375); + Left.setDirection(DcMotor.Direction.REVERSE); // run until the end of the match (driver presses STOP) + RF.setPosition(.5); + LF.setPosition(.5); + driver.forward(34,.5); RF.setPosition(.3); LF.setPosition(.7); - driver.forward(34,.5); while (opModeIsActive()) { driver.forward(-1,.5); driver.turn(40,.4); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java index 13c9fc7364d..70e1e7dfa35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeft.java @@ -37,7 +37,7 @@ public void runOpMode() { driver.forward(31,.5); //RF.setPosition(.5); //LF.setPosition(.5); - driver.turn(90,.2); + driver.turn(-90,.2); RF.setPosition(.3); LF.setPosition(.7); driver.forward(15,.5); From 815bd50b73d693cd4164827b1903848e8d9f30f9 Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 16 Feb 2018 08:23:46 -0800 Subject: [PATCH 14/38] the tribot code --- .../ftc/teamcode/TriangleRobot.java | 95 +++++++++++++++++++ 1 file changed, 95 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java new file mode 100644 index 00000000000..b91ffc9774c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java @@ -0,0 +1,95 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import static java.lang.Math.abs; +import static java.lang.Math.cos; +import static java.lang.Math.pow; +import static java.lang.Math.sin; +import static java.lang.Math.sqrt; +import static java.lang.Math.toRadians; + +/** + * Created by jxfio on 1/21/2018. + */ +@TeleOp(name="tribot", group="Linear Opmode") +public class TriangleRobot extends LinearOpMode{ + // Declare OpMode members. + ElapsedTime timer = new ElapsedTime(); + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor ADrive; + private DcMotor BDrive; + private DcMotor CDrive; + private IntegratingGyroscope gyro; + private ModernRoboticsI2cGyro MRI2CGyro; + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + ADrive = hardwareMap.get(DcMotor.class, "A"); + BDrive = hardwareMap.get(DcMotor.class, "B"); + CDrive = hardwareMap.get(DcMotor.class, "C"); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + MRI2CGyro.calibrate(); + // Wait until the gyro calibration is complete + timer.reset(); + while (!isStopRequested() && MRI2CGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + waitForStart(); + runtime.reset(); + telemetry.log().add("Press A & B to reset heading"); + // run until the end of the match (driver presses STOP) + double temp; + double APwr; + double BPwr; + double CPwr; + boolean curResetState = false; + boolean lastResetState = false; + while (opModeIsActive()) { + curResetState = (gamepad1.a && gamepad1.b); + if (curResetState && !lastResetState) { + MRI2CGyro.resetZAxisIntegrator(); + } + lastResetState = curResetState; + double θ = MRI2CGyro.getIntegratedZValue(); + double drivey = -gamepad1.left_stick_y; + double drivex = gamepad1.left_stick_x; + //double turn = gamepad1.right_stick_x; + double time = getRuntime(); + double driveθ = Math.atan(drivex/drivey); + double driveV = (abs(drivey)/drivey)*sqrt(pow(drivey,2)+pow(drivey,2)); + Range.clip(driveV,-1,1); + //drivebase powers + APwr= driveV*sin(driveθ); + BPwr= driveV*sin(driveθ+toRadians(120)); + CPwr= driveV*sin(driveθ-toRadians(120)); + // Send calculated power to wheels + ADrive.setPower(Range.clip(APwr, -1, 1)); + BDrive.setPower(Range.clip(BPwr, -1, 1)); + CDrive.setPower(Range.clip(CPwr, -1, 1)); + //Telemetry Data + telemetry.addData("path1","A Power:" + String.valueOf(APwr) + " B Power:" + String.valueOf(BPwr) + " C Power:" +String.valueOf(CPwr)); + telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue())); + } + } +} From 1afb25d6a547f84df61ec241ed30769140cf3e2c Mon Sep 17 00:00:00 2001 From: tazule Date: Sun, 4 Mar 2018 15:43:46 -0800 Subject: [PATCH 15/38] atan => atan2 --- .../java/org/firstinspires/ftc/teamcode/TriangleRobot.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java index b91ffc9774c..f74bc992be6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java @@ -76,8 +76,8 @@ public void runOpMode() { double drivex = gamepad1.left_stick_x; //double turn = gamepad1.right_stick_x; double time = getRuntime(); - double driveθ = Math.atan(drivex/drivey); - double driveV = (abs(drivey)/drivey)*sqrt(pow(drivey,2)+pow(drivey,2)); + double driveθ = Math.atan2(drivex,drivey); + double driveV = (sqrt(pow(drivey,2)+pow(drivey,2))); Range.clip(driveV,-1,1); //drivebase powers APwr= driveV*sin(driveθ); @@ -90,6 +90,7 @@ public void runOpMode() { //Telemetry Data telemetry.addData("path1","A Power:" + String.valueOf(APwr) + " B Power:" + String.valueOf(BPwr) + " C Power:" +String.valueOf(CPwr)); telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue())); + telemetry.update(); } } } From 01d1c3ab3b9c6edd01809414f80be82c9da11674 Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 20 Jul 2018 14:54:21 -0700 Subject: [PATCH 16/38] Computer vision, and tribot bugfixes --- .../ftc/teamcode/Computervision.java | 331 ++++++++++++++++++ .../ftc/teamcode/TriangleRobot.java | 52 ++- 2 files changed, 372 insertions(+), 11 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java new file mode 100644 index 00000000000..24b577d3fb9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java @@ -0,0 +1,331 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +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.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; + +import java.util.ArrayList; +import java.util.List; + +/** + * This 2016-2017 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "diamond" field configuration where the red and blue alliance stations + * are adjacent on the corner of the field furthest from the audience. + * From the Audience perspective, the Red driver station is on the right. + * The two vision target are located on the two walls closest to the audience, facing in. + * The Stones are on the RED side of the field, and the Chips are on the Blue side. + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + +@Autonomous(name="Concept: Vuforia Navigation", group ="Concept") +public class Computervision extends LinearOpMode { + + public static final String TAG = "Vuforia Navigation Sample"; + + OpenGLMatrix lastLocation = null; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + /* + * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); + * If no camera monitor is desired, use the parameterless constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + // OR... Do Not Activate the Camera Monitor View, to save power + // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + /* + * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which + * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function. + * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer + * web site at https://developer.vuforia.com/license-manager. + * + * Vuforia license keys are always 380 characters long, and look as if they contain mostly + * random data. As an example, here is a example of a fragment of a valid key: + * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ... + * Once you've obtained a license key, copy the string from the Vuforia web site + * and paste it in to your code onthe next line, between the double quotes. + */ + parameters.vuforiaLicenseKey = "AaPSQTf/////AAABmSC/czqVrkugjRHMuUPfIJ0G0Ew2pPxpfDhUTwNDDO3ZH4ZJ5tTwd+z0MV5DJOT0zc5TBJtsyyYucFpdGvJzHeiT/+XudkoYbayPMUsXox/Ql7dnAx0/fMJYYzR5uaiafhyqZNRVJICBkRFCd4xcT+7s+1jp/OCZT3kizHMtgyvO4KNgGF/UOzRt5GOnu4IiALplY+0ppb21RR8p7t9v6NXbKrhHUTSw0Wsx4OAvbFwsnrxcxOn7lfohME/rb8wZcMCsdwrPSrk+NGPmQBZILXL78eK1dqDVdWv0NYosY9TgNhqGodJAhteozrvuDBHGBn2PYHRBLKKI1Un/Yz075NQBT9yi+Z007APlAH/UVsP4"; + + /* + * We also indicate which camera on the RC that we wish to use. + * Here we chose the back (HiRes) camera (for greater range), but + * for a competition robot, the front camera might be more convenient. + */ + parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; + this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); + + /** + * Load the data sets that for the trackable objects we wish to track. These particular data + * sets are stored in the 'assets' part of our application (you'll see them in the Android + * Studio 'Project' view over there on the left of the screen). You can make your own datasets + * with the Vuforia Target Manager: https://developer.vuforia.com/target-manager. PDFs for the + * example "StonesAndChips", datasets can be found in in this project in the + * documentation directory. + */ + VuforiaTrackables stonesAndChips = this.vuforia.loadTrackablesFromAsset("StonesAndChips"); + VuforiaTrackable redTarget = stonesAndChips.get(0); + redTarget.setName("RedTarget"); // Stones + + VuforiaTrackable blueTarget = stonesAndChips.get(1); + blueTarget.setName("BlueTarget"); // Chips + + /** For convenience, gather together all the trackable objects in one easily-iterable collection */ + List allTrackables = new ArrayList(); + allTrackables.addAll(stonesAndChips); + + /** + * We use units of mm here because that's the recommended units of measurement for the + * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: + * + * You don't *have to* use mm here, but the units here and the units used in the XML + * target configuration files *must* correspond for the math to work out correctly. + */ + float mmPerInch = 25.4f; + float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot + float mmFTCFieldWidth = (12*12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels + + /** + * In order for localization to work, we need to tell the system where each target we + * wish to use for navigation resides on the field, and we need to specify where on the robot + * the phone resides. These specifications are in the form of transformation matrices. + * Transformation matrices are a central, important concept in the math here involved in localization. + * See Transformation Matrix + * for detailed information. Commonly, you'll encounter transformation matrices as instances + * of the {@link OpenGLMatrix} class. + * + * For the most part, you don't need to understand the details of the math of how transformation + * matrices work inside (as fascinating as that is, truly). Just remember these key points: + *
    + * + *
  1. You can put two transformations together to produce a third that combines the effect of + * both of them. If, for example, you have a rotation transform R and a translation transform T, + * then the combined transformation matrix RT which does the rotation first and then the translation + * is given by {@code RT = T.multiplied(R)}. That is, the transforms are multiplied in the + * reverse of the chronological order in which they applied.
    2. + * + *
  2. A common way to create useful transforms is to use methods in the {@link OpenGLMatrix} + * class and the Orientation class. See, for example, {@link OpenGLMatrix#translation(float, + * float, float)}, {@link OpenGLMatrix#rotation(AngleUnit, float, float, float, float)}, and + * {@link Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float)}. + * Related methods in {@link OpenGLMatrix}, such as {@link OpenGLMatrix#rotated(AngleUnit, + * float, float, float, float)}, are syntactic shorthands for creating a new transform and + * then immediately multiplying the receiver by it, which can be convenient at times.
    3. + * + *
  3. If you want to break open the black box of a transformation matrix to understand + * what it's doing inside, use {@link MatrixF#getTranslation()} to fetch how much the + * transform will move you in x, y, and z, and use {@link Orientation#getOrientation(MatrixF, + * AxesReference, AxesOrder, AngleUnit)} to determine the rotational motion that the transform + * will impart. See {@link #format(OpenGLMatrix)} below for an example.
    4. + * + *
+ * + * This example places the "stones" image on the perimeter wall to the Left + * of the Red Driver station wall. Similar to the Red Beacon Location on the Res-Q + * + * This example places the "chips" image on the perimeter wall to the Right + * of the Blue Driver station. Similar to the Blue Beacon Location on the Res-Q + * + * See the doc folder of this project for a description of the field Axis conventions. + * + * Initially the target is conceptually lying at the origin of the field's coordinate system + * (the center of the field), facing up. + * + * In this configuration, the target's coordinate system aligns with that of the field. + * + * In a real situation we'd also account for the vertical (Z) offset of the target, + * but for simplicity, we ignore that here; for a real robot, you'll want to fix that. + * + * To place the Stones Target on the Red Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Then we rotate it 90 around the field's Z access to face it away from the audience. + * - Finally, we translate it back along the X axis towards the red audience wall. + */ + OpenGLMatrix redTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the RED WALL. Our translation here + is a negative translation in X.*/ + .translation(-mmFTCFieldWidth/2, 0, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 90, 0)); + redTarget.setLocation(redTargetLocationOnField); + RobotLog.ii(TAG, "Red Target=%s", format(redTargetLocationOnField)); + + /* + * To place the Stones Target on the Blue Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Finally, we translate it along the Y axis towards the blue audience wall. + */ + OpenGLMatrix blueTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the Blue Audience wall. + Our translation here is a positive translation in Y.*/ + .translation(0, mmFTCFieldWidth/2, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 0, 0)); + blueTarget.setLocation(blueTargetLocationOnField); + RobotLog.ii(TAG, "Blue Target=%s", format(blueTargetLocationOnField)); + + /** + * Create a transformation matrix describing where the phone is on the robot. Here, we + * put the phone on the right hand side of the robot with the screen facing in (see our + * choice of BACK camera above) and in landscape mode. Starting from alignment between the + * robot's and phone's axes, this is a rotation of -90deg along the Y axis. + * + * When determining whether a rotation is positive or negative, consider yourself as looking + * down the (positive) axis of rotation from the positive towards the origin. Positive rotations + * are then CCW, and negative rotations CW. An example: consider looking down the positive Z + * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y + * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. + */ + OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix + .translation(mmBotWidth/2,0,0) + .multiplied(Orientation.getRotationMatrix( + AxesReference.EXTRINSIC, AxesOrder.YZY, + AngleUnit.DEGREES, -90, 0, 0)); + RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); + + /** + * Let the trackable listeners we care about know where the phone is. We know that each + * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because + * we have not ourselves installed a listener of a different type. + */ + ((VuforiaTrackableDefaultListener)redTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + ((VuforiaTrackableDefaultListener)blueTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + + /** + * A brief tutorial: here's how all the math is going to work: + * + * C = phoneLocationOnRobot maps phone coords -> robot coords + * P = tracker.getPose() maps image target coords -> phone coords + * L = redTargetLocationOnField maps image target coords -> field coords + * + * So + * + * C.inverted() maps robot coords -> phone coords + * P.inverted() maps phone coords -> imageTarget coords + * + * Putting that all together, + * + * L x P.inverted() x C.inverted() maps robot coords to field coords. + * + * @see VuforiaTrackableDefaultListener#getRobotLocation() + */ + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + stonesAndChips.activate(); + + while (opModeIsActive()) { + + for (VuforiaTrackable trackable : allTrackables) { + /** + * getUpdatedRobotLocation() will return null if no new information is available since + * the last time that call was made, or if the trackable is not currently visible. + * getRobotLocation() will return null if the trackable is not currently visible. + */ + telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); // + + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + lastLocation = robotLocationTransform; + } + } + /** + * Provide feedback as to where the robot was last located (if we know). + */ + if (lastLocation != null) { + // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); + telemetry.addData("Pos", format(lastLocation)); + } else { + telemetry.addData("Pos", "Unknown"); + } + telemetry.update(); + } + } + + /** + * A simple utility that extracts positioning information from a transformation matrix + * and formats it in a form palatable to a human being. + */ + String format(OpenGLMatrix transformationMatrix) { + return transformationMatrix.formatAsTransform(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java index f74bc992be6..7f9c4177d79 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; @@ -28,6 +29,7 @@ public class TriangleRobot extends LinearOpMode{ private DcMotor CDrive; private IntegratingGyroscope gyro; private ModernRoboticsI2cGyro MRI2CGyro; + private Servo F; @Override public void runOpMode() { telemetry.addData("Status", "Initialized"); @@ -45,6 +47,7 @@ public void runOpMode() { telemetry.log().add("Gyro Calibrating. Do Not Move!"); MRI2CGyro.calibrate(); // Wait until the gyro calibration is complete + F = hardwareMap.get(Servo.class, "F"); timer.reset(); while (!isStopRequested() && MRI2CGyro.isCalibrating()) { telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); @@ -63,8 +66,15 @@ public void runOpMode() { double APwr; double BPwr; double CPwr; + double dt; + double Desiredθ = 0; + double Fingeroffset = 0; + double TurnPWR; + double PrevTime = time; //previous time value boolean curResetState = false; boolean lastResetState = false; + boolean b = false; + boolean x = false; while (opModeIsActive()) { curResetState = (gamepad1.a && gamepad1.b); if (curResetState && !lastResetState) { @@ -72,25 +82,45 @@ public void runOpMode() { } lastResetState = curResetState; double θ = MRI2CGyro.getIntegratedZValue(); - double drivey = -gamepad1.left_stick_y; - double drivex = gamepad1.left_stick_x; + double drivey = -gamepad1.left_stick_y; //should be neg here + double drivex = -gamepad1.left_stick_x; //double turn = gamepad1.right_stick_x; + //negative sign because y goes in wrong direction double time = getRuntime(); - double driveθ = Math.atan2(drivex,drivey); - double driveV = (sqrt(pow(drivey,2)+pow(drivey,2))); - Range.clip(driveV,-1,1); + double driveθ = Math.atan2(drivex,drivey); //direction //took out neg + double driveV = (sqrt(pow(drivey,2)+pow(drivex,2))); //magnitude -pythagorean theorem + driveV = Range.clip(driveV,-1,1); + //integrating for angle + dt = time - PrevTime; + Desiredθ += (-gamepad1.right_stick_x * dt * 90); // 45 degrees per second + PrevTime = time; + //PIDθ + TurnPWR = (θ - Desiredθ)/-100; + TurnPWR = Range.clip(TurnPWR, -.75, .75); + if(gamepad2.b && !b && Fingeroffset <= .4){ + Fingeroffset += .1; + } + if(gamepad2.x && !x && Fingeroffset >= .1 ){ + Fingeroffset -= .1; + } + b = gamepad2.b; + x = gamepad2.x; //drivebase powers - APwr= driveV*sin(driveθ); - BPwr= driveV*sin(driveθ+toRadians(120)); - CPwr= driveV*sin(driveθ-toRadians(120)); + temp= 1 - TurnPWR; + APwr= TurnPWR + temp * driveV*sin(driveθ); //power to send motor- proportional to the sign of the angle to drive at + BPwr= TurnPWR + temp * driveV*sin(driveθ+toRadians(120)); + CPwr= TurnPWR + temp * driveV*sin(driveθ-toRadians(120)); + F.setPosition(Range.clip(0.5 - Fingeroffset, -1,1)); // Send calculated power to wheels - ADrive.setPower(Range.clip(APwr, -1, 1)); + ADrive.setPower(Range.clip(APwr, -1, 1)); //range.clip concerns for ratios?- put in in case BDrive.setPower(Range.clip(BPwr, -1, 1)); CDrive.setPower(Range.clip(CPwr, -1, 1)); //Telemetry Data + telemetry.addData("path0","driveθ:"); telemetry.addData("path1","A Power:" + String.valueOf(APwr) + " B Power:" + String.valueOf(BPwr) + " C Power:" +String.valueOf(CPwr)); - telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue())); - telemetry.update(); + telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue()) + " Desiredθ: " + String.valueOf(Desiredθ)); + telemetry.addData("path3","Time: "+ toString().valueOf(time)); + telemetry.update();// prints above stuff to phone } } } From ccbc714eca1ae5d38f401b1f7bf705d2f9e2dc46 Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Fri, 20 Jul 2018 16:00:51 -0700 Subject: [PATCH 17/38] Fixed file path issue Took out code that tried to find a file path on Julian's computer and removed its import into shoddierteleop. Removed unused dependency on Jama. --- TeamCode/build.gradle | 2 +- .../java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index d88d17bd1aa..87e39acb8a0 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -11,5 +11,5 @@ // Include common definitions from above. apply from: '../build.common.gradle' dependencies { - compile files('C:/Users/jxfio/OneDrive/Documents/GitHub/ftc_app/libs/Jama-1.0.3.jar') + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java index 469db86c475..5d8962981bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ShoddierTeleOP.java @@ -9,7 +9,6 @@ import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; -import Jama.Matrix; import java.lang.Math; import static java.lang.Math.cos; From c4cda77da22b53832a443e1decd9a130ac015338 Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Fri, 20 Jul 2018 20:13:48 -0700 Subject: [PATCH 18/38] Relic Computer Vision Switched the file to work for the relic pictures from last season. Seemed to work well, next step is to write a program to spin in circle until robot sees one relic, then go towards that. --- .../ftc/teamcode/Computervision.java | 62 +++++++++++-------- 1 file changed, 36 insertions(+), 26 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java index 24b577d3fb9..161511189f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java @@ -41,10 +41,12 @@ 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.RelicRecoveryVuMark; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; +//import org.firstinspires.ftc.ftccommon.external.navigation.VuforiaTrackables.RelicRecoveryVuMark; import java.util.ArrayList; import java.util.List; @@ -134,16 +136,14 @@ public class Computervision extends LinearOpMode { * example "StonesAndChips", datasets can be found in in this project in the * documentation directory. */ - VuforiaTrackables stonesAndChips = this.vuforia.loadTrackablesFromAsset("StonesAndChips"); - VuforiaTrackable redTarget = stonesAndChips.get(0); - redTarget.setName("RedTarget"); // Stones + VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark"); + VuforiaTrackable relicTemplate = relicTrackables.get(0); + relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary - VuforiaTrackable blueTarget = stonesAndChips.get(1); - blueTarget.setName("BlueTarget"); // Chips + telemetry.addData(">", "Press Play to start"); telemetry.update(); + waitForStart(); + relicTrackables.activate(); - /** For convenience, gather together all the trackable objects in one easily-iterable collection */ - List allTrackables = new ArrayList(); - allTrackables.addAll(stonesAndChips); /** * We use units of mm here because that's the recommended units of measurement for the @@ -212,12 +212,12 @@ public class Computervision extends LinearOpMode { * - Then we rotate it 90 around the field's Z access to face it away from the audience. * - Finally, we translate it back along the X axis towards the red audience wall. */ - OpenGLMatrix redTargetLocationOnField = OpenGLMatrix + /* OpenGLMatrix redTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here - is a negative translation in X.*/ + is a negative translation in X. .translation(-mmFTCFieldWidth/2, 0, 0) .multiplied(Orientation.getRotationMatrix( - /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ + /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); redTarget.setLocation(redTargetLocationOnField); @@ -228,12 +228,12 @@ public class Computervision extends LinearOpMode { * - First we rotate it 90 around the field's X axis to flip it upright * - Finally, we translate it along the Y axis towards the blue audience wall. */ - OpenGLMatrix blueTargetLocationOnField = OpenGLMatrix + //OpenGLMatrix blueTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. - Our translation here is a positive translation in Y.*/ + Our translation here is a positive translation in Y. .translation(0, mmFTCFieldWidth/2, 0) .multiplied(Orientation.getRotationMatrix( - /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ + /* First, in the fixed (field) coordinate system, we rotate 90deg in X AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); blueTarget.setLocation(blueTargetLocationOnField); @@ -250,7 +250,7 @@ public class Computervision extends LinearOpMode { * are then CCW, and negative rotations CW. An example: consider looking down the positive Z * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. - */ + OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix .translation(mmBotWidth/2,0,0) .multiplied(Orientation.getRotationMatrix( @@ -258,12 +258,12 @@ public class Computervision extends LinearOpMode { AngleUnit.DEGREES, -90, 0, 0)); RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); - /** + * Let the trackable listeners we care about know where the phone is. We know that each * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because * we have not ourselves installed a listener of a different type. - */ - ((VuforiaTrackableDefaultListener)redTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + + ((VuforiaTrackableDefaultListener)redTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener)blueTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); /** @@ -285,22 +285,23 @@ public class Computervision extends LinearOpMode { * @see VuforiaTrackableDefaultListener#getRobotLocation() */ + /** Wait for the game to begin */ telemetry.addData(">", "Press Play to start tracking"); telemetry.update(); waitForStart(); /** Start tracking the data sets we care about. */ - stonesAndChips.activate(); + relicTrackables.activate(); while (opModeIsActive()) { - for (VuforiaTrackable trackable : allTrackables) { + /* for (VuforiaTrackable trackable : allTrackables) { /** * getUpdatedRobotLocation() will return null if no new information is available since * the last time that call was made, or if the trackable is not currently visible. * getRobotLocation() will return null if the trackable is not currently visible. - */ + telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); // OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); @@ -311,12 +312,20 @@ public class Computervision extends LinearOpMode { /** * Provide feedback as to where the robot was last located (if we know). */ - if (lastLocation != null) { + + RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate); + if (vuMark != RelicRecoveryVuMark.UNKNOWN) { +/* Found an instance of the template. In the actual game, you will probably +* loop until this condition occurs, then move on to act accordingly depending * on which VuMark was visible. */ + telemetry.addData("VuMark", "%s visible", vuMark); + } + + /* if (lastLocation != null) { // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); telemetry.addData("Pos", format(lastLocation)); } else { telemetry.addData("Pos", "Unknown"); - } + }*/ telemetry.update(); } } @@ -325,7 +334,8 @@ public class Computervision extends LinearOpMode { * A simple utility that extracts positioning information from a transformation matrix * and formats it in a form palatable to a human being. */ - String format(OpenGLMatrix transformationMatrix) { - return transformationMatrix.formatAsTransform(); - } + //String format(OpenGLMatrix transformationMatrix) { + //return transformationMatrix.formatAsTransform(); } + +//super helpful website https://www.firstinspires.org/sites/default/files/uploads/resource_library/ftc/using-vumarks.pdf \ No newline at end of file From db3cce6d3c58bff38e393116362b45646b468a0a Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Sun, 22 Jul 2018 15:20:18 -0700 Subject: [PATCH 19/38] Separated computer vision programs Visionstonesnchips is for stones and chips, directly copied off ConceptVuforiaNavigation. VisionTest is for Relics, and is what we are further experimenting with --- .../ftc/teamcode/VisionTest.java | 184 ++++++++++ .../ftc/teamcode/Visionstonesnchips.java | 332 ++++++++++++++++++ 2 files changed, 516 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java new file mode 100644 index 00000000000..44cc9a3bdb9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java @@ -0,0 +1,184 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +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.RelicRecoveryVuMark; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; +//import org.firstinspires.ftc.ftccommon.external.navigation.VuforiaTrackables.RelicRecoveryVuMark; + +import java.util.ArrayList; +import java.util.List; + +/** + * This 2016-2017 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "diamond" field configuration where the red and blue alliance stations + * are adjacent on the corner of the field furthest from the audience. + * From the Audience perspective, the Red driver station is on the right. + * The two vision target are located on the two walls closest to the audience, facing in. + * The Stones are on the RED side of the field, and the Chips are on the Blue side. + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + +@Autonomous(name="Concept: Vuforia Navigation", group ="Concept") +public class VisionTest extends LinearOpMode { + + public static final String TAG = "Vuforia Navigation Sample"; + + OpenGLMatrix lastLocation = null; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + /* + * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); + * If no camera monitor is desired, use the parameterless constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + + /* + * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which + * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function. + * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer + * web site at https://developer.vuforia.com/license-manager. + * + * Vuforia license keys are always 380 characters long, and look as if they contain mostly + * random data. As an example, here is a example of a fragment of a valid key: + * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ... + * Once you've obtained a license key, copy the string from the Vuforia web site + * and paste it in to your code onthe next line, between the double quotes. + */ + parameters.vuforiaLicenseKey = "AaPSQTf/////AAABmSC/czqVrkugjRHMuUPfIJ0G0Ew2pPxpfDhUTwNDDO3ZH4ZJ5tTwd+z0MV5DJOT0zc5TBJtsyyYucFpdGvJzHeiT/+XudkoYbayPMUsXox/Ql7dnAx0/fMJYYzR5uaiafhyqZNRVJICBkRFCd4xcT+7s+1jp/OCZT3kizHMtgyvO4KNgGF/UOzRt5GOnu4IiALplY+0ppb21RR8p7t9v6NXbKrhHUTSw0Wsx4OAvbFwsnrxcxOn7lfohME/rb8wZcMCsdwrPSrk+NGPmQBZILXL78eK1dqDVdWv0NYosY9TgNhqGodJAhteozrvuDBHGBn2PYHRBLKKI1Un/Yz075NQBT9yi+Z007APlAH/UVsP4"; + + /* + * We also indicate which camera on the RC that we wish to use. + * Here we chose the back (HiRes) camera (for greater range), but + * for a competition robot, the front camera might be more convenient. + */ + parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; + this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); + + /** + * Load the data sets that for the trackable objects we wish to track. These particular data + * sets are stored in the 'assets' part of our application (you'll see them in the Android + * Studio 'Project' view over there on the left of the screen). You can make your own datasets + * with the Vuforia Target Manager: https://developer.vuforia.com/target-manager. PDFs for the + * example "StonesAndChips", datasets can be found in in this project in the + * documentation directory. + */ + VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark"); + VuforiaTrackable relicTemplate = relicTrackables.get(0); + relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary + + telemetry.addData(">", "Press Play to start"); telemetry.update(); + waitForStart(); + relicTrackables.activate(); + + + /** + * We use units of mm here because that's the recommended units of measurement for the + * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: + * + * You don't *have to* use mm here, but the units here and the units used in the XML + * target configuration files *must* correspond for the math to work out correctly. + */ + float mmPerInch = 25.4f; + float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot + float mmFTCFieldWidth = (12*12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels + + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + relicTrackables.activate(); + + while (opModeIsActive()) { + + RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate); + if (vuMark != RelicRecoveryVuMark.UNKNOWN) { + /* Found an instance of the template. In the actual game, you will probably + * loop until this condition occurs, then move on to act accordingly depending + * on which VuMark was visible. */ + telemetry.addData("VuMark", "%s visible", vuMark); + + + } + + telemetry.update(); + } + } + +} + +//super helpful website https://www.firstinspires.org/sites/default/files/uploads/resource_library/ftc/using-vumarks.pdf \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java new file mode 100644 index 00000000000..a0d60412567 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java @@ -0,0 +1,332 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +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.VuforiaLocalizer; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; +import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; + +import java.util.ArrayList; +import java.util.List; + +/** + * This 2016-2017 OpMode illustrates the basics of using the Vuforia localizer to determine + * positioning and orientation of robot on the FTC field. + * The code is structured as a LinearOpMode + * + * Vuforia uses the phone's camera to inspect it's surroundings, and attempt to locate target images. + * + * When images are located, Vuforia is able to determine the position and orientation of the + * image relative to the camera. This sample code than combines that information with a + * knowledge of where the target images are on the field, to determine the location of the camera. + * + * This example assumes a "diamond" field configuration where the red and blue alliance stations + * are adjacent on the corner of the field furthest from the audience. + * From the Audience perspective, the Red driver station is on the right. + * The two vision target are located on the two walls closest to the audience, facing in. + * The Stones are on the RED side of the field, and the Chips are on the Blue side. + * + * A final calculation then uses the location of the camera on the robot to determine the + * robot's location and orientation on the field. + * + * @see VuforiaLocalizer + * @see VuforiaTrackableDefaultListener + * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list. + * + * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as + * is explained below. + */ + +@Autonomous(name="Concept: Vuforia Navigation", group ="Concept") + +public class Visionstonesnchips extends LinearOpMode { + + public static final String TAG = "Vuforia Navigation Sample"; + + OpenGLMatrix lastLocation = null; + + /** + * {@link #vuforia} is the variable we will use to store our instance of the Vuforia + * localization engine. + */ + VuforiaLocalizer vuforia; + + @Override public void runOpMode() { + /* + * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone); + * If no camera monitor is desired, use the parameterless constructor instead (commented out below). + */ + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); + + // OR... Do Not Activate the Camera Monitor View, to save power + // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + + /* + * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which + * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function. + * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer + * web site at https://developer.vuforia.com/license-manager. + * + * Vuforia license keys are always 380 characters long, and look as if they contain mostly + * random data. As an example, here is a example of a fragment of a valid key: + * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ... + * Once you've obtained a license key, copy the string from the Vuforia web site + * and paste it in to your code onthe next line, between the double quotes. + */ + parameters.vuforiaLicenseKey = "ATsODcD/////AAAAAVw2lR...d45oGpdljdOh5LuFB9nDNfckoxb8COxKSFX"; + + /* + * We also indicate which camera on the RC that we wish to use. + * Here we chose the back (HiRes) camera (for greater range), but + * for a competition robot, the front camera might be more convenient. + */ + parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; + this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); + + /** + * Load the data sets that for the trackable objects we wish to track. These particular data + * sets are stored in the 'assets' part of our application (you'll see them in the Android + * Studio 'Project' view over there on the left of the screen). You can make your own datasets + * with the Vuforia Target Manager: https://developer.vuforia.com/target-manager. PDFs for the + * example "StonesAndChips", datasets can be found in in this project in the + * documentation directory. + */ + VuforiaTrackables stonesAndChips = this.vuforia.loadTrackablesFromAsset("StonesAndChips"); + VuforiaTrackable redTarget = stonesAndChips.get(0); + redTarget.setName("RedTarget"); // Stones + + VuforiaTrackable blueTarget = stonesAndChips.get(1); + blueTarget.setName("BlueTarget"); // Chips + + /** For convenience, gather together all the trackable objects in one easily-iterable collection */ + List allTrackables = new ArrayList(); + allTrackables.addAll(stonesAndChips); + + /** + * We use units of mm here because that's the recommended units of measurement for the + * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: + * + * You don't *have to* use mm here, but the units here and the units used in the XML + * target configuration files *must* correspond for the math to work out correctly. + */ + float mmPerInch = 25.4f; + float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot + float mmFTCFieldWidth = (12*12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels + + /** + * In order for localization to work, we need to tell the system where each target we + * wish to use for navigation resides on the field, and we need to specify where on the robot + * the phone resides. These specifications are in the form of transformation matrices. + * Transformation matrices are a central, important concept in the math here involved in localization. + * See Transformation Matrix + * for detailed information. Commonly, you'll encounter transformation matrices as instances + * of the {@link OpenGLMatrix} class. + * + * For the most part, you don't need to understand the details of the math of how transformation + * matrices work inside (as fascinating as that is, truly). Just remember these key points: + *
    + * + *
  1. You can put two transformations together to produce a third that combines the effect of + * both of them. If, for example, you have a rotation transform R and a translation transform T, + * then the combined transformation matrix RT which does the rotation first and then the translation + * is given by {@code RT = T.multiplied(R)}. That is, the transforms are multiplied in the + * reverse of the chronological order in which they applied.
    2. + * + *
  2. A common way to create useful transforms is to use methods in the {@link OpenGLMatrix} + * class and the Orientation class. See, for example, {@link OpenGLMatrix#translation(float, + * float, float)}, {@link OpenGLMatrix#rotation(AngleUnit, float, float, float, float)}, and + * {@link Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float)}. + * Related methods in {@link OpenGLMatrix}, such as {@link OpenGLMatrix#rotated(AngleUnit, + * float, float, float, float)}, are syntactic shorthands for creating a new transform and + * then immediately multiplying the receiver by it, which can be convenient at times.
    3. + * + *
  3. If you want to break open the black box of a transformation matrix to understand + * what it's doing inside, use {@link MatrixF#getTranslation()} to fetch how much the + * transform will move you in x, y, and z, and use {@link Orientation#getOrientation(MatrixF, + * AxesReference, AxesOrder, AngleUnit)} to determine the rotational motion that the transform + * will impart. See {@link #format(OpenGLMatrix)} below for an example.
    4. + * + *
+ * + * This example places the "stones" image on the perimeter wall to the Left + * of the Red Driver station wall. Similar to the Red Beacon Location on the Res-Q + * + * This example places the "chips" image on the perimeter wall to the Right + * of the Blue Driver station. Similar to the Blue Beacon Location on the Res-Q + * + * See the doc folder of this project for a description of the field Axis conventions. + * + * Initially the target is conceptually lying at the origin of the field's coordinate system + * (the center of the field), facing up. + * + * In this configuration, the target's coordinate system aligns with that of the field. + * + * In a real situation we'd also account for the vertical (Z) offset of the target, + * but for simplicity, we ignore that here; for a real robot, you'll want to fix that. + * + * To place the Stones Target on the Red Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Then we rotate it 90 around the field's Z access to face it away from the audience. + * - Finally, we translate it back along the X axis towards the red audience wall. + */ + OpenGLMatrix redTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the RED WALL. Our translation here + is a negative translation in X.*/ + .translation(-mmFTCFieldWidth/2, 0, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 90, 0)); + redTarget.setLocation(redTargetLocationOnField); + RobotLog.ii(TAG, "Red Target=%s", format(redTargetLocationOnField)); + + /* + * To place the Stones Target on the Blue Audience wall: + * - First we rotate it 90 around the field's X axis to flip it upright + * - Finally, we translate it along the Y axis towards the blue audience wall. + */ + OpenGLMatrix blueTargetLocationOnField = OpenGLMatrix + /* Then we translate the target off to the Blue Audience wall. + Our translation here is a positive translation in Y.*/ + .translation(0, mmFTCFieldWidth/2, 0) + .multiplied(Orientation.getRotationMatrix( + /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ + AxesReference.EXTRINSIC, AxesOrder.XZX, + AngleUnit.DEGREES, 90, 0, 0)); + blueTarget.setLocation(blueTargetLocationOnField); + RobotLog.ii(TAG, "Blue Target=%s", format(blueTargetLocationOnField)); + + /** + * Create a transformation matrix describing where the phone is on the robot. Here, we + * put the phone on the right hand side of the robot with the screen facing in (see our + * choice of BACK camera above) and in landscape mode. Starting from alignment between the + * robot's and phone's axes, this is a rotation of -90deg along the Y axis. + * + * When determining whether a rotation is positive or negative, consider yourself as looking + * down the (positive) axis of rotation from the positive towards the origin. Positive rotations + * are then CCW, and negative rotations CW. An example: consider looking down the positive Z + * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y + * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. + */ + OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix + .translation(mmBotWidth/2,0,0) + .multiplied(Orientation.getRotationMatrix( + AxesReference.EXTRINSIC, AxesOrder.YZY, + AngleUnit.DEGREES, -90, 0, 0)); + RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); + + /** + * Let the trackable listeners we care about know where the phone is. We know that each + * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because + * we have not ourselves installed a listener of a different type. + */ + ((VuforiaTrackableDefaultListener)redTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + ((VuforiaTrackableDefaultListener)blueTarget.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); + + /** + * A brief tutorial: here's how all the math is going to work: + * + * C = phoneLocationOnRobot maps phone coords -> robot coords + * P = tracker.getPose() maps image target coords -> phone coords + * L = redTargetLocationOnField maps image target coords -> field coords + * + * So + * + * C.inverted() maps robot coords -> phone coords + * P.inverted() maps phone coords -> imageTarget coords + * + * Putting that all together, + * + * L x P.inverted() x C.inverted() maps robot coords to field coords. + * + * @see VuforiaTrackableDefaultListener#getRobotLocation() + */ + + /** Wait for the game to begin */ + telemetry.addData(">", "Press Play to start tracking"); + telemetry.update(); + waitForStart(); + + /** Start tracking the data sets we care about. */ + stonesAndChips.activate(); + + while (opModeIsActive()) { + + for (VuforiaTrackable trackable : allTrackables) { + /** + * getUpdatedRobotLocation() will return null if no new information is available since + * the last time that call was made, or if the trackable is not currently visible. + * getRobotLocation() will return null if the trackable is not currently visible. + */ + telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); // + + OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation(); + if (robotLocationTransform != null) { + lastLocation = robotLocationTransform; + } + } + /** + * Provide feedback as to where the robot was last located (if we know). + */ + if (lastLocation != null) { + // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); + telemetry.addData("Pos", format(lastLocation)); + } else { + telemetry.addData("Pos", "Unknown"); + } + telemetry.update(); + } + } + + /** + * A simple utility that extracts positioning information from a transformation matrix + * and formats it in a form palatable to a human being. + */ + String format(OpenGLMatrix transformationMatrix) { + return transformationMatrix.formatAsTransform(); + } +} From 9be296e22db31d32df651418a4d83ceb6a7b4de4 Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Sun, 22 Jul 2018 15:31:24 -0700 Subject: [PATCH 20/38] Fixed camel case Added camel case to be better --- .../java/org/firstinspires/ftc/teamcode/Computervision.java | 2 +- .../java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java index 161511189f4..87d61cfa827 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java @@ -83,7 +83,7 @@ */ @Autonomous(name="Concept: Vuforia Navigation", group ="Concept") -public class Computervision extends LinearOpMode { +public class ComputerVision extends LinearOpMode { public static final String TAG = "Vuforia Navigation Sample"; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java index a0d60412567..88a7e0faabc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Visionstonesnchips.java @@ -82,7 +82,7 @@ @Autonomous(name="Concept: Vuforia Navigation", group ="Concept") -public class Visionstonesnchips extends LinearOpMode { +public class VisionStonesNChips extends LinearOpMode { public static final String TAG = "Vuforia Navigation Sample"; From bee953e1d930ffc20cce79b5a54f97107f86f054 Mon Sep 17 00:00:00 2001 From: tazule Date: Sun, 22 Jul 2018 15:43:44 -0700 Subject: [PATCH 21/38] set up driving on the vision test --- .../firstinspires/ftc/teamcode/VisionTest.java | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java index 44cc9a3bdb9..4726c1d2d80 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java @@ -32,6 +32,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.robotcore.external.ClassFactory; @@ -84,7 +85,9 @@ @Autonomous(name="Concept: Vuforia Navigation", group ="Concept") public class VisionTest extends LinearOpMode { - + private DcMotor ADrive; + private DcMotor BDrive; + private DcMotor CDrive; public static final String TAG = "Vuforia Navigation Sample"; OpenGLMatrix lastLocation = null; @@ -102,7 +105,9 @@ public class VisionTest extends LinearOpMode { */ int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); - + ADrive = hardwareMap.get(DcMotor.class, "A"); + BDrive = hardwareMap.get(DcMotor.class, "B"); + CDrive = hardwareMap.get(DcMotor.class, "C"); /* * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which @@ -171,8 +176,15 @@ public class VisionTest extends LinearOpMode { * loop until this condition occurs, then move on to act accordingly depending * on which VuMark was visible. */ telemetry.addData("VuMark", "%s visible", vuMark); + ADrive.setPower(0); + BDrive.setPower(.5); + CDrive.setPower(-.5); - + } + else { + ADrive.setPower(.5); + BDrive.setPower(.5); + CDrive.setPower(.5); } telemetry.update(); From 793e40c7a1057a2ca5966f37a65d3eb025201c90 Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Sun, 9 Sep 2018 13:04:53 -0700 Subject: [PATCH 22/38] Created Tank Drive file for this year Gonna put in VuMarks and a basic tank drive for testing. Need to check for ftc_app update first. --- .../firstinspires/ftc/teamcode/TankDriveRoverRuckus.java | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java new file mode 100644 index 00000000000..ca9e8eb2bcb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -0,0 +1,9 @@ +package org.firstinspires.ftc.teamcode; + +/** + * Created by emilyhinds on 9/9/18. + */ + +public class TankDriveRoverRuckus { + +} From 24f63e2975735625ffb461b9000960bd8ac8b9cc Mon Sep 17 00:00:00 2001 From: tazule Date: Sat, 8 Sep 2018 16:25:46 -0700 Subject: [PATCH 23/38] vision test --- .../ftc/teamcode/Computervision.java | 2 +- .../firstinspires/ftc/teamcode/VisionTest.java | 17 +++++++++-------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java index 87d61cfa827..5f75eacaa62 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java @@ -82,7 +82,7 @@ * is explained below. */ -@Autonomous(name="Concept: Vuforia Navigation", group ="Concept") +@Autonomous(name="Computer Vision", group ="Concept") public class ComputerVision extends LinearOpMode { public static final String TAG = "Vuforia Navigation Sample"; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java index 4726c1d2d80..971d9a3f6d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionTest.java @@ -83,7 +83,7 @@ * is explained below. */ -@Autonomous(name="Concept: Vuforia Navigation", group ="Concept") +@Autonomous(name="VisionTest", group ="Concept") public class VisionTest extends LinearOpMode { private DcMotor ADrive; private DcMotor BDrive; @@ -167,9 +167,9 @@ public class VisionTest extends LinearOpMode { /** Start tracking the data sets we care about. */ relicTrackables.activate(); - + double count = 0; while (opModeIsActive()) { - + count = count + 1; RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate); if (vuMark != RelicRecoveryVuMark.UNKNOWN) { /* Found an instance of the template. In the actual game, you will probably @@ -177,14 +177,15 @@ public class VisionTest extends LinearOpMode { * on which VuMark was visible. */ telemetry.addData("VuMark", "%s visible", vuMark); ADrive.setPower(0); - BDrive.setPower(.5); - CDrive.setPower(-.5); + BDrive.setPower(.25); + CDrive.setPower(-.25); } else { - ADrive.setPower(.5); - BDrive.setPower(.5); - CDrive.setPower(.5); + + ADrive.setPower(.1); + BDrive.setPower(.1); + CDrive.setPower(.1); } telemetry.update(); From 4045f063aab39ee37128b78c1066178b8327fd02 Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Fri, 12 Oct 2018 12:59:40 -0700 Subject: [PATCH 24/38] Working on tank drive Making a program for tank drive teleop --- .../ftc/teamcode/TankDriveRoverRuckus.java | 47 ++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index ca9e8eb2bcb..aa678c8719c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -1,9 +1,54 @@ package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.robotcontroller.external.samples.HardwareK9bot; + /** * Created by emilyhinds on 9/9/18. */ -public class TankDriveRoverRuckus { +@TeleOp(name="TankDriveRoverRuckus", group="TankDriveRoverRuckus") +public class TankDriveRoverRuckus extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + + @Override + public void runOpMode() { + + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + + waitForStart(); + // run until the end of the match (driver presses STOP) + boolean limitoff=false; + boolean y = false; + double Fingeroffset = 0; + boolean b = false; + boolean x = false; + double factor=1; + double bonus = 0; + double position = 0; + double towerpower; + double max = 4416; + while (opModeIsActive()) { + bonus = 0; + + /* + //towerpower = (bonus+factor*towerpower);*/ + //towerpower += bonus; + Left.setPower(.5*Range.clip(gamepad1.left_stick_y, -1, 1)); + Right.setPower(.5*Range.clip(-gamepad1.right_stick_y, -1, 1)); + } + } } From 30db1f45be9f308692f09fc53f7f8ed8845a02af Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Fri, 12 Oct 2018 14:59:56 -0700 Subject: [PATCH 25/38] Update TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComputerVision.java --- .../ftc/teamcode/{Computervision.java => ComputerVision.java} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Computervision.java => ComputerVision.java} (100%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComputerVision.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Computervision.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ComputerVision.java From e5a122c2c6166857900be3d06a067388a5e827a4 Mon Sep 17 00:00:00 2001 From: tazule Date: Tue, 30 Oct 2018 18:25:37 -0700 Subject: [PATCH 26/38] added the 2018 climber bot code and started a robotics utilities thingybooper --- .../ftc/teamcode/ClimberBot2018.java | 126 ++++++++++++++++++ .../ftc/teamcode/RoboticsUtils.java | 28 ++++ 2 files changed, 154 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java new file mode 100644 index 00000000000..77cbbe70358 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java @@ -0,0 +1,126 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +import static java.lang.Math.abs; +import static java.lang.Math.cos; +import static java.lang.Math.pow; +import static java.lang.Math.sin; +import static java.lang.Math.sqrt; +import static java.lang.Math.toRadians; + +/** + * Created by jxfio on 1/21/2018. + */ +@TeleOp(name="tribot", group="Linear Opmode") +public class ClimberBot2018 extends LinearOpMode{ + // Declare OpMode members. + ElapsedTime timer = new ElapsedTime(); + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor ADrive; + private DcMotor BDrive; + private DcMotor CDrive; + private IntegratingGyroscope gyro; + private ModernRoboticsI2cGyro MRI2CGyro; + private Servo F; + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + ADrive = hardwareMap.get(DcMotor.class, "A"); + BDrive = hardwareMap.get(DcMotor.class, "B"); + CDrive = hardwareMap.get(DcMotor.class, "C"); + MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)MRI2CGyro; + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + MRI2CGyro.calibrate(); + // Wait until the gyro calibration is complete + F = hardwareMap.get(Servo.class, "F"); + timer.reset(); + while (!isStopRequested() && MRI2CGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + waitForStart(); + runtime.reset(); + telemetry.log().add("Press A & B to reset heading"); + // run until the end of the match (driver presses STOP) + double temp; + double APwr; + double BPwr; + double CPwr; + double dt; + double Desiredθ = 0; + double Fingeroffset = 0; + double TurnPWR; + double PrevTime = time; //previous time value + boolean curResetState = false; + boolean lastResetState = false; + boolean b = false; + boolean x = false; + while (opModeIsActive()) { + curResetState = (gamepad1.a && gamepad1.b); + if (curResetState && !lastResetState) { + MRI2CGyro.resetZAxisIntegrator(); + } + lastResetState = curResetState; + double θ = MRI2CGyro.getIntegratedZValue(); + double drivey = -gamepad1.left_stick_y; //should be neg here + double drivex = -gamepad1.left_stick_x; + //double turn = gamepad1.right_stick_x; + //negative sign because y goes in wrong direction + double time = getRuntime(); + double driveθ = Math.atan2(drivex,drivey); //direction //took out neg + double driveV = (sqrt(pow(drivey,2)+pow(drivex,2))); //magnitude -pythagorean theorem + driveV = Range.clip(driveV,-1,1); + //integrating for angle + dt = time - PrevTime; + Desiredθ += (-gamepad1.right_stick_x * dt * 90); // 45 degrees per second + PrevTime = time; + //PIDθ + TurnPWR = (θ - Desiredθ)/-100; + TurnPWR = Range.clip(TurnPWR, -.75, .75); + if(gamepad2.b && !b && Fingeroffset <= .4){ + Fingeroffset += .1; + } + if(gamepad2.x && !x && Fingeroffset >= .1 ){ + Fingeroffset -= .1; + } + b = gamepad2.b; + x = gamepad2.x; + //drivebase powers + temp= 1 - TurnPWR; + APwr= TurnPWR + temp * driveV*sin(driveθ); //power to send motor- proportional to the sign of the angle to drive at + BPwr= TurnPWR + temp * driveV*sin(driveθ+toRadians(120)); + CPwr= TurnPWR + temp * driveV*sin(driveθ-toRadians(120)); + F.setPosition(Range.clip(0.5 - Fingeroffset, -1,1)); + // Send calculated power to wheels + ADrive.setPower(Range.clip(APwr, -1, 1)); //range.clip concerns for ratios?- put in in case + BDrive.setPower(Range.clip(BPwr, -1, 1)); + CDrive.setPower(Range.clip(CPwr, -1, 1)); + //Telemetry Data + telemetry.addData("path0","driveθ:"); + telemetry.addData("path1","A Power:" + String.valueOf(APwr) + " B Power:" + String.valueOf(BPwr) + " C Power:" +String.valueOf(CPwr)); + telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue()) + " Desiredθ: " + String.valueOf(Desiredθ)); + telemetry.addData("path3","Time: "+ toString().valueOf(time)); + telemetry.update();// prints above stuff to phone + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils.java new file mode 100644 index 00000000000..a18e606b1c6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode; + +/** + * Created by jxfio on 10/30/2018. + */ + +public class RoboticsUtils { + public class PID { + public double kp; + public double ki; + public double kd; + public double pi=0; + public double ii=0; + public double di=0; + public double preverror = 0; + public PID(double p, double i, double d){ + kp = p; + ki = i; + kd = d; + } + public void iteratePID (double error){ + pi=kp*error; + ii+=ki*error; + kd = di*error/preverror; + preverror = error; + } + } +} From a0517561a0ec1ca6f73ce2622a8253e347cc4bcc Mon Sep 17 00:00:00 2001 From: tazule Date: Thu, 1 Nov 2018 23:31:12 -0700 Subject: [PATCH 27/38] added almost all of Climberbot2018 --- .../ftc/teamcode/ClimberBot2018.java | 202 ++++++++++++++---- .../ftc/teamcode/RoboticsUtils.java | 28 --- .../ftc/teamcode/RoboticsUtils/PID.java | 25 +++ .../ftc/teamcode/TankDriveRoverRuckus.java | 2 +- 4 files changed, 188 insertions(+), 69 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java index 77cbbe70358..f685dffe871 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java @@ -9,27 +9,41 @@ import com.qualcomm.robotcore.util.ElapsedTime; import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.teamcode.RoboticsUtils.PID; + +import static android.content.Context.*; import static java.lang.Math.abs; -import static java.lang.Math.cos; import static java.lang.Math.pow; import static java.lang.Math.sin; import static java.lang.Math.sqrt; import static java.lang.Math.toRadians; + /** * Created by jxfio on 1/21/2018. */ @TeleOp(name="tribot", group="Linear Opmode") public class ClimberBot2018 extends LinearOpMode{ + // Declare OpMode members. + //packageContext(org.firstinspires.ftc.teamcode, 0); ElapsedTime timer = new ElapsedTime(); private ElapsedTime runtime = new ElapsedTime(); - private DcMotor ADrive; - private DcMotor BDrive; - private DcMotor CDrive; + private DcMotor left; + private DcMotor back; + private DcMotor right; + private DcMotor wheelLift; + private DcMotor swivel; + private DcMotor arm; + private DcMotor leftClimb; + private DcMotor rightClimb; + private Servo lift; + private Servo leftFinger; + private Servo rightFinger; + private Servo backSwivel; private IntegratingGyroscope gyro; private ModernRoboticsI2cGyro MRI2CGyro; - private Servo F; + PID θPID = new PID(1,.0001,.1); @Override public void runOpMode() { telemetry.addData("Status", "Initialized"); @@ -39,15 +53,23 @@ public void runOpMode() { // Initialize the hardware variables. Note that the strings used here as parameters // to 'get' must correspond to the names assigned during the robot configuration // step (using the FTC Robot Controller app on the phone). - ADrive = hardwareMap.get(DcMotor.class, "A"); - BDrive = hardwareMap.get(DcMotor.class, "B"); - CDrive = hardwareMap.get(DcMotor.class, "C"); + left = hardwareMap.get(DcMotor.class, "B1"); + back = hardwareMap.get(DcMotor.class, "B3"); + right = hardwareMap.get(DcMotor.class, "B2"); + wheelLift = hardwareMap.get(DcMotor.class, "B0"); + swivel = hardwareMap.get(DcMotor.class,"R0"); + arm = hardwareMap.get(DcMotor.class, "R1"); + leftClimb = hardwareMap.get(DcMotor.class, "R2"); + rightClimb = hardwareMap.get(DcMotor.class, "R3"); + lift = hardwareMap.get(Servo.class,"S2"); + leftFinger = hardwareMap.get(Servo.class, "S3"); + rightFinger = hardwareMap.get(Servo.class, "S4"); + backSwivel = hardwareMap.get(Servo.class, "S5"); MRI2CGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); gyro = (IntegratingGyroscope)MRI2CGyro; telemetry.log().add("Gyro Calibrating. Do Not Move!"); MRI2CGyro.calibrate(); // Wait until the gyro calibration is complete - F = hardwareMap.get(Servo.class, "F"); timer.reset(); while (!isStopRequested() && MRI2CGyro.isCalibrating()) { telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); @@ -63,61 +85,161 @@ public void runOpMode() { telemetry.log().add("Press A & B to reset heading"); // run until the end of the match (driver presses STOP) double temp; - double APwr; - double BPwr; - double CPwr; + double leftPwr; + double backPwr; + double rightPwr; double dt; double Desiredθ = 0; - double Fingeroffset = 0; double TurnPWR; double PrevTime = time; //previous time value boolean curResetState = false; boolean lastResetState = false; - boolean b = false; - boolean x = false; + boolean upRailMode = false; + boolean onRail = false; + boolean y = false; + double pos; + double prevpos=0; + double prevderiv=0; + double wheelLiftZero=0; + final double wheelLiftZerotoGround=47; + PID wheelLiftPID = new PID(1,.0001,.1); + PID armPID = new PID(1,.00001,.1); + PID armSwivelPID = new PID(1,.00001,.1); + boolean x=false; + boolean b=false; + boolean armSwivelZeroingState; + boolean lastArmSwivelZeroingState=false; + double armSwivelZeroState=0; + boolean armZeroingState; + boolean lastArmZeroingState=false; + double armZeroState=0; + boolean leftOpen=true; + boolean rightOpen = true; while (opModeIsActive()) { + //Gyro reset curResetState = (gamepad1.a && gamepad1.b); if (curResetState && !lastResetState) { MRI2CGyro.resetZAxisIntegrator(); } lastResetState = curResetState; + armZeroingState = (gamepad2.a && gamepad2.b); + if (armZeroingState&&!lastArmZeroingState){ + armZeroState=arm.getCurrentPosition(); + } + lastArmZeroingState =armZeroingState; + armSwivelZeroingState = (gamepad2.a && gamepad2.b); + if (armSwivelZeroingState&&!lastArmSwivelZeroingState){ + armSwivelZeroState=arm.getCurrentPosition(); + } + lastArmSwivelZeroingState =armSwivelZeroingState; + if(gamepad1.y&&! y){ + upRailMode=true; + } + y = gamepad1.y; + if(gamepad2.y){ + lift.setPosition(1); + }else if(gamepad2.a){ + lift.setPosition(-1); + } + if(gamepad2.x&&!x){ + leftOpen=!leftOpen; + } + x=gamepad2.x; + if(gamepad2.b&&!b){ + rightOpen=!rightOpen; + } + b=gamepad2.b; + if(leftOpen){ + leftFinger.setPosition(1); + }else{ + leftFinger.setPosition(.25); + } + if(rightOpen){ + rightFinger.setPosition(0); + }else{ + rightFinger.setPosition(.75); + } + while (upRailMode){ + double dp; + dt = time - PrevTime; + PrevTime = time; + time = getRuntime(); + if(gamepad1.y&&! y){ + upRailMode=false; + } + armPID.iteratePID(arm.getCurrentPosition()-armZeroState+70,dt); + armSwivelPID.iteratePID(swivel.getCurrentPosition()-armSwivelZeroState,dt); + arm.setPower(Range.clip(armPID.getPID(),-1,1)); + swivel.setPower(Range.clip(armSwivelPID.getPID(),-1,1)); + y = gamepad1.y; + backSwivel.setPosition(1); + leftClimb.setPower(-1); + rightClimb.setPower(1); + pos = rightClimb.getCurrentPosition(); + dp = pos-prevpos; + prevpos = pos; + if(Math.abs(dp/dt-prevderiv)>10){ + onRail = true; + } + + while (onRail){ + dt = time - PrevTime; + PrevTime = time; + time = getRuntime(); + wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()- wheelLiftZero,dt); + wheelLift.setPower(Range.clip(wheelLiftPID.getPID(),-1,1)); + leftClimb.setPower(Range.clip(-gamepad1.left_stick_y,-1,1)); + rightClimb.setPower(Range.clip(gamepad1.left_stick_y,-1,1)); + if(Math.abs((pos-prevpos)/dt-prevderiv)>10){ + onRail = false; + } + wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()-wheelLiftZero,dt); + armPID.iteratePID(arm.getCurrentPosition()-armZeroState+70,dt); + armSwivelPID.iteratePID(swivel.getCurrentPosition()-armSwivelZeroState,dt); + arm.setPower(Range.clip(armPID.getPID(),-1,1)); + swivel.setPower(Range.clip(armSwivelPID.getPID(),-1,1)); + wheelLift.setPower(Range.clip(wheelLiftPID.getPID(),-1,1)); + } + wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()-wheelLiftZero+wheelLiftZerotoGround,dt); + wheelLift.setPower(Range.clip(wheelLiftPID.getPID(),-1,1)); + prevderiv=(pos-prevpos)/dt; + back.setPower(Range.clip(gamepad1.left_stick_y,-1,1)); + } double θ = MRI2CGyro.getIntegratedZValue(); double drivey = -gamepad1.left_stick_y; //should be neg here double drivex = -gamepad1.left_stick_x; - //double turn = gamepad1.right_stick_x; - //negative sign because y goes in wrong direction + //double turn = gamepad1.right_stick_x; + //negative sign because y goes in wrong direction double time = getRuntime(); double driveθ = Math.atan2(drivex,drivey); //direction //took out neg - double driveV = (sqrt(pow(drivey,2)+pow(drivex,2))); //magnitude -pythagorean theorem + double driveV = sqrt(pow(drivey,2)+pow(drivex,2)); //magnitude -pythagorean theorem driveV = Range.clip(driveV,-1,1); - //integrating for angle + //integrating for angle dt = time - PrevTime; Desiredθ += (-gamepad1.right_stick_x * dt * 90); // 45 degrees per second PrevTime = time; - //PIDθ - TurnPWR = (θ - Desiredθ)/-100; + //PID + wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()-wheelLiftZero+wheelLiftZerotoGround,dt); + wheelLift.setPower(Range.clip(wheelLiftPID.getPID(),-1,1)); + θPID.iteratePID(θ-Desiredθ,dt); + TurnPWR = θPID.getPID(); + //TurnPWR = (θ - Desiredθ)/-100; TurnPWR = Range.clip(TurnPWR, -.75, .75); - if(gamepad2.b && !b && Fingeroffset <= .4){ - Fingeroffset += .1; - } - if(gamepad2.x && !x && Fingeroffset >= .1 ){ - Fingeroffset -= .1; - } - b = gamepad2.b; - x = gamepad2.x; - //drivebase powers + //drivebase powers temp= 1 - TurnPWR; - APwr= TurnPWR + temp * driveV*sin(driveθ); //power to send motor- proportional to the sign of the angle to drive at - BPwr= TurnPWR + temp * driveV*sin(driveθ+toRadians(120)); - CPwr= TurnPWR + temp * driveV*sin(driveθ-toRadians(120)); - F.setPosition(Range.clip(0.5 - Fingeroffset, -1,1)); - // Send calculated power to wheels - ADrive.setPower(Range.clip(APwr, -1, 1)); //range.clip concerns for ratios?- put in in case - BDrive.setPower(Range.clip(BPwr, -1, 1)); - CDrive.setPower(Range.clip(CPwr, -1, 1)); - //Telemetry Data + leftPwr= TurnPWR + temp * driveV*sin(driveθ+toRadians(60)); //power to send motor- proportional to the sign of the angle to drive at + backPwr= TurnPWR + temp * driveV*sin(driveθ+toRadians(180)); + rightPwr= TurnPWR + temp * driveV*sin(driveθ-toRadians(60)); + // Send calculated power to motors + swivel.setPower(Range.clip(gamepad2.left_stick_x,-1,1)); + arm.setPower(Range.clip(gamepad2.left_stick_y,-1,1)); + backSwivel.setPosition(1/2); + left.setPower(Range.clip(leftPwr, -1, 1)); //range.clip concerns for ratios?- put in in case + back.setPower(Range.clip(backPwr, -1, 1)); + right.setPower(Range.clip(rightPwr, -1, 1)); + //Telemetry Data telemetry.addData("path0","driveθ:"); - telemetry.addData("path1","A Power:" + String.valueOf(APwr) + " B Power:" + String.valueOf(BPwr) + " C Power:" +String.valueOf(CPwr)); + telemetry.addData("path1","A Power:" + String.valueOf(leftPwr) + " B Power:" + String.valueOf(backPwr) + " C Power:" +String.valueOf(rightPwr)); telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue()) + " Desiredθ: " + String.valueOf(Desiredθ)); telemetry.addData("path3","Time: "+ toString().valueOf(time)); telemetry.update();// prints above stuff to phone diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils.java deleted file mode 100644 index a18e606b1c6..00000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -/** - * Created by jxfio on 10/30/2018. - */ - -public class RoboticsUtils { - public class PID { - public double kp; - public double ki; - public double kd; - public double pi=0; - public double ii=0; - public double di=0; - public double preverror = 0; - public PID(double p, double i, double d){ - kp = p; - ki = i; - kd = d; - } - public void iteratePID (double error){ - pi=kp*error; - ii+=ki*error; - kd = di*error/preverror; - preverror = error; - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java new file mode 100644 index 00000000000..0d57dcf85fe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.RoboticsUtils; + +public class PID { + public double kp; + public double ki; + public double kd; + public double pi=0; + public double ii=0; + public double di=0; + public double preverror = 0; + public PID(double kp, double ki, double kd){ + this.kp = kp; + this.ki = ki; + this.kd = kd; + } + public void iteratePID (double error,double dt){ + pi=kp*error; + ii+=ki*error*dt; + kd = di*error/preverror; + this.preverror = error; + } + public double getPID(){ + return pi+ki+di; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index aa678c8719c..be0eeeffe35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; -import org.firstinspires.ftc.robotcontroller.external.samples.HardwareK9bot; +//import org.firstinspires.ftc.robotcontroller.external.samples.HardwareK9bot; /** * Created by emilyhinds on 9/9/18. From ccf2609db521bb09d218558eb5c4edae6643147d Mon Sep 17 00:00:00 2001 From: tazule Date: Sat, 3 Nov 2018 12:47:59 -0700 Subject: [PATCH 28/38] initial commit --- .../ftc/teamcode/ClimberBot2018.java | 31 ++++++++++++------- 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java index f685dffe871..3c239e7c214 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java @@ -22,7 +22,7 @@ /** * Created by jxfio on 1/21/2018. */ -@TeleOp(name="tribot", group="Linear Opmode") +@TeleOp(name="Climber", group="Linear Opmode") public class ClimberBot2018 extends LinearOpMode{ // Declare OpMode members. @@ -43,7 +43,7 @@ public class ClimberBot2018 extends LinearOpMode{ private Servo backSwivel; private IntegratingGyroscope gyro; private ModernRoboticsI2cGyro MRI2CGyro; - PID θPID = new PID(1,.0001,.1); + PID θPID = new PID(.5,.0001,.1); @Override public void runOpMode() { telemetry.addData("Status", "Initialized"); @@ -101,8 +101,8 @@ public void runOpMode() { double prevpos=0; double prevderiv=0; double wheelLiftZero=0; - final double wheelLiftZerotoGround=47; - PID wheelLiftPID = new PID(1,.0001,.1); + final double wheelLiftZerotoGround=-20; + PID wheelLiftPID = new PID(1,0,0); PID armPID = new PID(1,.00001,.1); PID armSwivelPID = new PID(1,.00001,.1); boolean x=false; @@ -115,6 +115,11 @@ public void runOpMode() { double armZeroState=0; boolean leftOpen=true; boolean rightOpen = true; + wheelLift.setPower(.25); + sleep(1000); + wheelLiftZero = wheelLift.getCurrentPosition(); + wheelLift.setPower(0); + while (opModeIsActive()) { //Gyro reset curResetState = (gamepad1.a && gamepad1.b); @@ -127,7 +132,7 @@ public void runOpMode() { armZeroState=arm.getCurrentPosition(); } lastArmZeroingState =armZeroingState; - armSwivelZeroingState = (gamepad2.a && gamepad2.b); + armSwivelZeroingState = (gamepad2.y && gamepad2.a); if (armSwivelZeroingState&&!lastArmSwivelZeroingState){ armSwivelZeroState=arm.getCurrentPosition(); } @@ -137,9 +142,11 @@ public void runOpMode() { } y = gamepad1.y; if(gamepad2.y){ - lift.setPosition(1); + lift.setPosition(0); }else if(gamepad2.a){ - lift.setPosition(-1); + lift.setPosition(1); + }else{ + lift.setPosition(.5); } if(gamepad2.x&&!x){ leftOpen=!leftOpen; @@ -198,10 +205,10 @@ public void runOpMode() { armSwivelPID.iteratePID(swivel.getCurrentPosition()-armSwivelZeroState,dt); arm.setPower(Range.clip(armPID.getPID(),-1,1)); swivel.setPower(Range.clip(armSwivelPID.getPID(),-1,1)); - wheelLift.setPower(Range.clip(wheelLiftPID.getPID(),-1,1)); + wheelLift.setPower(-Range.clip(wheelLiftPID.getPID(),-1,1)); } wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()-wheelLiftZero+wheelLiftZerotoGround,dt); - wheelLift.setPower(Range.clip(wheelLiftPID.getPID(),-1,1)); + wheelLift.setPower(-Range.clip(wheelLiftPID.getPID(),-1,1)); prevderiv=(pos-prevpos)/dt; back.setPower(Range.clip(gamepad1.left_stick_y,-1,1)); } @@ -220,7 +227,7 @@ public void runOpMode() { PrevTime = time; //PID wheelLiftPID.iteratePID(wheelLift.getCurrentPosition()-wheelLiftZero+wheelLiftZerotoGround,dt); - wheelLift.setPower(Range.clip(wheelLiftPID.getPID(),-1,1)); + wheelLift.setPower(Range.clip(-wheelLiftPID.getPID(),-1,1)); θPID.iteratePID(θ-Desiredθ,dt); TurnPWR = θPID.getPID(); //TurnPWR = (θ - Desiredθ)/-100; @@ -238,11 +245,11 @@ public void runOpMode() { back.setPower(Range.clip(backPwr, -1, 1)); right.setPower(Range.clip(rightPwr, -1, 1)); //Telemetry Data - telemetry.addData("path0","driveθ:"); + /*telemetry.addData("path0","driveθ:"); telemetry.addData("path1","A Power:" + String.valueOf(leftPwr) + " B Power:" + String.valueOf(backPwr) + " C Power:" +String.valueOf(rightPwr)); telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue()) + " Desiredθ: " + String.valueOf(Desiredθ)); telemetry.addData("path3","Time: "+ toString().valueOf(time)); - telemetry.update();// prints above stuff to phone + telemetry.update();// prints above stuff to phone*/ } } } From be48459910c3ea548886fdcd8611e9aa83be4acb Mon Sep 17 00:00:00 2001 From: tazule Date: Sun, 4 Nov 2018 14:20:16 -0800 Subject: [PATCH 29/38] Work from Competition Trying to stop robot from flailing by zeroing out I and D values in PID. Also the arm kept pressing into the robot so commented out some lines of code to help that --- .../ftc/teamcode/ClimberBot2018.java | 17 +++++++++-------- .../ftc/teamcode/RoboticsUtils/PID.java | 2 +- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java index 3c239e7c214..ecbc20752ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ClimberBot2018.java @@ -43,7 +43,7 @@ public class ClimberBot2018 extends LinearOpMode{ private Servo backSwivel; private IntegratingGyroscope gyro; private ModernRoboticsI2cGyro MRI2CGyro; - PID θPID = new PID(.5,.0001,.1); + PID θPID = new PID(.5,0,0);//changed from .5, .0001, 0.1 @Override public void runOpMode() { telemetry.addData("Status", "Initialized"); @@ -102,9 +102,9 @@ public void runOpMode() { double prevderiv=0; double wheelLiftZero=0; final double wheelLiftZerotoGround=-20; - PID wheelLiftPID = new PID(1,0,0); - PID armPID = new PID(1,.00001,.1); - PID armSwivelPID = new PID(1,.00001,.1); + PID wheelLiftPID = new PID(.01,0,0);//set all the ps to .01 instead of 1 + PID armPID = new PID(.01,0,0);//originally 1, ,00001,.1 (armswivel same) + PID armSwivelPID = new PID(.01,0,0); boolean x=false; boolean b=false; boolean armSwivelZeroingState; @@ -115,10 +115,11 @@ public void runOpMode() { double armZeroState=0; boolean leftOpen=true; boolean rightOpen = true; - wheelLift.setPower(.25); - sleep(1000); - wheelLiftZero = wheelLift.getCurrentPosition(); - wheelLift.setPower(0); + // wheelLift.setPower(.01);//changed from 0.25 because it was slamming + // sleep(1000); //changed from 1000 because it was pushing into robot for too long + // wheelLiftZero = wheelLift.getCurrentPosition(); + // wheelLift.setPower(-.01); + // commented all of this out because no matter what we do it would just slam on the robot and we want to drive first and foremost. while (opModeIsActive()) { //Gyro reset diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java index 0d57dcf85fe..85a63df0a0e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoboticsUtils/PID.java @@ -9,7 +9,7 @@ public class PID { public double di=0; public double preverror = 0; public PID(double kp, double ki, double kd){ - this.kp = kp; + this.kp = kp; //making 3 number variables to set later on this.ki = ki; this.kd = kd; } From 32361f20b0032d45afe12061d5c5c03e38b806af Mon Sep 17 00:00:00 2001 From: tazule Date: Mon, 12 Nov 2018 13:12:39 -0800 Subject: [PATCH 30/38] basic tank drive for oragami bot --- .../ftc/teamcode/TankDriveRoverRuckus.java | 32 ++----------------- 1 file changed, 2 insertions(+), 30 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index be0eeeffe35..c424160ea69 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -1,54 +1,26 @@ package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; - //import org.firstinspires.ftc.robotcontroller.external.samples.HardwareK9bot; - /** * Created by emilyhinds on 9/9/18. */ - @TeleOp(name="TankDriveRoverRuckus", group="TankDriveRoverRuckus") public class TankDriveRoverRuckus extends LinearOpMode { - // Declare OpMode members. private DcMotor Left; private DcMotor Right; - @Override public void runOpMode() { - Left = hardwareMap.get(DcMotor.class, "left"); Right = hardwareMap.get(DcMotor.class, "right"); - - waitForStart(); // run until the end of the match (driver presses STOP) - boolean limitoff=false; - boolean y = false; - double Fingeroffset = 0; - boolean b = false; - boolean x = false; - double factor=1; - double bonus = 0; - double position = 0; - double towerpower; - double max = 4416; while (opModeIsActive()) { - bonus = 0; - - /* - //towerpower = (bonus+factor*towerpower);*/ - //towerpower += bonus; - Left.setPower(.5*Range.clip(gamepad1.left_stick_y, -1, 1)); - Right.setPower(.5*Range.clip(-gamepad1.right_stick_y, -1, 1)); - + Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); + Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); } } } From c58f80982b722b5aee80523dc19f3f8f88e7634d Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Mon, 12 Nov 2018 16:43:42 -0800 Subject: [PATCH 31/38] Update Tank Drive- 11/12 Fixing the left power --- .../ftc/teamcode/TankDriveRoverRuckus.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index c424160ea69..4538b7d0ee5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -12,15 +12,23 @@ public class TankDriveRoverRuckus extends LinearOpMode { // Declare OpMode members. private DcMotor Left; private DcMotor Right; + private DcMotor Shoulder; + private DcMotor Elbow; @Override public void runOpMode() { Left = hardwareMap.get(DcMotor.class, "left"); Right = hardwareMap.get(DcMotor.class, "right"); + Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); + Elbow = hardwareMap.get(DcMotor.class, "elbow"); + + waitForStart(); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { - Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); + Left.setPower(-Range.clip(gamepad1.left_stick_y, -1, 1)); Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); + Shoulder.setPower(Range.clip(gamepad2.left_stick_y, -1,1)); + Elbow.setPower(Range.clip(-gamepad2.right_stick_y, -1,1)); } } } From 9034b958d27033b3cf896b9db309c2041dc75a03 Mon Sep 17 00:00:00 2001 From: tazule Date: Sat, 1 Dec 2018 12:59:48 -0800 Subject: [PATCH 32/38] Code Changes for Winch- taking off lift --- .../ftc/teamcode/TankDriveRoverRuckus.java | 58 ++++++++++++++++++- .../ftc/teamcode/TriangleRobot.java | 1 + 2 files changed, 56 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index 4538b7d0ee5..0da4d57a892 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -2,7 +2,10 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.teamcode.RoboticsUtils.PID; //import org.firstinspires.ftc.robotcontroller.external.samples.HardwareK9bot; /** * Created by emilyhinds on 9/9/18. @@ -14,21 +17,70 @@ public class TankDriveRoverRuckus extends LinearOpMode { private DcMotor Right; private DcMotor Shoulder; private DcMotor Elbow; + private DcMotor Wrist; + private Servo LGrabber; + private Servo RGrabber; + private DcMotor Winch; + private DcMotor Sideways; @Override public void runOpMode() { Left = hardwareMap.get(DcMotor.class, "left"); Right = hardwareMap.get(DcMotor.class, "right"); Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); Elbow = hardwareMap.get(DcMotor.class, "elbow"); - - + Wrist = hardwareMap.get(DcMotor.class,"wrist"); + LGrabber = hardwareMap.get(Servo.class, "lgrab"); + RGrabber = hardwareMap.get(Servo.class, "rgrab"); + Winch = hardwareMap.get(DcMotor.class,"winch"); + Sideways = hardwareMap.get(DcMotor.class,"sideways"); + boolean bumper = false; + int wristdir = 1; waitForStart(); + boolean b = true; + boolean x = true; + double loffset = 0; + double roffset = 0; + double prevtime=0; // run until the end of the match (driver presses STOP) while (opModeIsActive()) { - Left.setPower(-Range.clip(gamepad1.left_stick_y, -1, 1)); + Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); Shoulder.setPower(Range.clip(gamepad2.left_stick_y, -1,1)); Elbow.setPower(Range.clip(-gamepad2.right_stick_y, -1,1)); + if(gamepad2.right_bumper&&!bumper){ + wristdir = wristdir*-1; + } + bumper = gamepad2.right_bumper; + Wrist.setPower(Range.clip(.5*gamepad2.right_trigger*wristdir,-1,1)); + /*if(gamepad2.b&&!b){ + roffset = roffset*-1; + }*/ + if(gamepad2.b&&!b&&loffset==0){ + roffset=.6; + }else if(gamepad2.b&&!b){ + roffset=0; + } + b = gamepad2.b; + RGrabber.setPosition(Range.clip(RGrabber.MAX_POSITION-roffset,RGrabber.MIN_POSITION,RGrabber.MAX_POSITION)); + if(gamepad2.x&&!x&&loffset==0){ + loffset=.6; + }else if(gamepad2.x&&!x){ + loffset=0; + } + x = gamepad2.x; + LGrabber.setPosition(Range.clip(LGrabber.MIN_POSITION+loffset,LGrabber.MIN_POSITION,LGrabber.MAX_POSITION)); + + //Winch code- up: right trigger, down: left trigger + Winch.setPower(Range.clip(gamepad1.right_trigger-gamepad1.left_trigger,-1,1)); + //Sideways- front wheel, push both sticks to control + // both gamepads are inversed, so need to minus + Sideways.setPower(Range.clip(gamepad1.right_stick_y-gamepad1.left_stick_y,-1,1)); + prevtime = getRuntime(); + //telemetry.addData("bools",liftmode+" "+a); + //telemetry.addData("lift pos", Lift.getCurrentPosition()); + telemetry.addData("loffset",loffset); + telemetry.addData("roffset", roffset); + telemetry.update(); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java index 7f9c4177d79..9d7f6cb4691 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TriangleRobot.java @@ -121,6 +121,7 @@ public void runOpMode() { telemetry.addData("path2","Integrated Z:" + String.valueOf(MRI2CGyro.getIntegratedZValue()) + " Desiredθ: " + String.valueOf(Desiredθ)); telemetry.addData("path3","Time: "+ toString().valueOf(time)); telemetry.update();// prints above stuff to phone + } } } From 449bf527a2898b859967199a1628fe8ef9f16757 Mon Sep 17 00:00:00 2001 From: tazule Date: Sun, 9 Dec 2018 14:37:06 -0800 Subject: [PATCH 33/38] minor fixes --- .../org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index 0da4d57a892..0dd6084dc8a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -40,7 +40,7 @@ public void runOpMode() { boolean x = true; double loffset = 0; double roffset = 0; - double prevtime=0; + double prevtime= 0; // run until the end of the match (driver presses STOP) while (opModeIsActive()) { Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); @@ -74,7 +74,7 @@ public void runOpMode() { Winch.setPower(Range.clip(gamepad1.right_trigger-gamepad1.left_trigger,-1,1)); //Sideways- front wheel, push both sticks to control // both gamepads are inversed, so need to minus - Sideways.setPower(Range.clip(gamepad1.right_stick_y-gamepad1.left_stick_y,-1,1)); + Sideways.setPower(Range.clip(-gamepad1.right_stick_y+gamepad1.left_stick_y,-1,1)); prevtime = getRuntime(); //telemetry.addData("bools",liftmode+" "+a); //telemetry.addData("lift pos", Lift.getCurrentPosition()); From 8db21cb5f8d3c8455f9f232fdee4928b9a0abccf Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Sun, 9 Dec 2018 15:23:55 -0800 Subject: [PATCH 34/38] Creating AutoStillPhont --- .../java/org/firstinspires/ftc/teamcode/AutoStillPhone.java | 4 ++++ build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 3 ++- 3 files changed, 7 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java new file mode 100644 index 00000000000..006e798210d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode; + +public class AutoStillPhone { +} diff --git a/build.gradle b/build.gradle index e13ce162e19..491c8452273 100644 --- a/build.gradle +++ b/build.gradle @@ -9,7 +9,7 @@ buildscript { jcenter() } dependencies { - classpath 'com.android.tools.build:gradle:3.1.3' + classpath 'com.android.tools.build:gradle:3.2.1' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index b6517bb1d16..e0ed7a2690e 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Sun Dec 09 14:38:58 PST 2018 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-4.4-all.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-4.6-all.zip From 235c1283445afa5b8cf0b6fa3c4b6c9d068efebb Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Tue, 11 Dec 2018 20:32:16 -0800 Subject: [PATCH 35/38] Autonomous!!! Making 2 dead reconing autos, one for each position. They are working!! --- .../ftc/teamcode/AutoStillPhone.java | 81 ++++++++++++++- .../firstinspires/ftc/teamcode/AutoTurn.java | 99 +++++++++++++++++++ .../ftc/teamcode/TankDriveRoverRuckus.java | 4 +- 3 files changed, 181 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTurn.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java index 006e798210d..986f8b3ee46 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoStillPhone.java @@ -1,4 +1,83 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + package org.firstinspires.ftc.teamcode; -public class AutoStillPhone { +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous(name="AutoStillPhone", group="AutoStillPhone") + +public class AutoStillPhone extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor Left; + private DcMotor Right; + private ElapsedTime runtime = new ElapsedTime(); + + + static final double FORWARD_SPEED = 0.6; + static final double TURN_SPEED = 0.5; + + public void runOpMode() { + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + //@Override + //public void runOpMode() { + + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Ready to run"); // + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // Step through each leg of the path, ensuring that the Auto mode has not been stopped along the way + + // Step 1: Drive forward for 7 seconds + Right.setPower(-FORWARD_SPEED); + Left.setPower(FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 2)) { + telemetry.addData("Path", "Leg 1: %2.5f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + + + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTurn.java new file mode 100644 index 00000000000..cf9949f2a03 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTurn.java @@ -0,0 +1,99 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Autonomous(name="AutoTurn", group="AutoTurn") + +public class AutoTurn extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor Left; + private DcMotor Right; + private ElapsedTime runtime = new ElapsedTime(); + + + static final double FORWARD_SPEED = 0.6; + static final double TURN_SPEED = 0.5; + + public void runOpMode() { + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + + //@Override + //public void runOpMode() { + + /* + * Initialize the drive system variables. + * The init() method of the hardware class does all the work here + */ + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Ready to run"); // + telemetry.update(); + + // Wait for the game to start (driver presses PLAY) + waitForStart(); + + // Step through each leg of the path, ensuring that the Auto mode has not been stopped along the way + + // Step 1: Drive forward for .5 seconds + Right.setPower(-FORWARD_SPEED); + Left.setPower(FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 0.8)) { + telemetry.addData("Path", "Leg 1: %2.5f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 2: Spin right for 1 seconds + Right.setPower(-TURN_SPEED); + Left.setPower(-TURN_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 0.6)) { + telemetry.addData("Path", "Leg 2: %2.5f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 3: Drive Forward for 1 Second + Right.setPower(-FORWARD_SPEED); + Left.setPower(FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 3.5)) { + telemetry.addData("Path", "Leg 3: %2.5f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index 0dd6084dc8a..7b89bed10b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -45,8 +45,8 @@ public void runOpMode() { while (opModeIsActive()) { Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); - Shoulder.setPower(Range.clip(gamepad2.left_stick_y, -1,1)); - Elbow.setPower(Range.clip(-gamepad2.right_stick_y, -1,1)); + Shoulder.setPower(.5*Range.clip(gamepad2.left_stick_y, -1,1)); + Elbow.setPower(.5*Range.clip(-gamepad2.right_stick_y, -1,1)); if(gamepad2.right_bumper&&!bumper){ wristdir = wristdir*-1; } From df021d41e1ca1b08f24c69aa07e454fca809a6fe Mon Sep 17 00:00:00 2001 From: tazule Date: Thu, 13 Dec 2018 20:23:00 -0800 Subject: [PATCH 36/38] Added in PID for the elbow and shoulder --- .../ftc/teamcode/TankDriveRoverRuckus.java | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index 0dd6084dc8a..711e74df931 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -40,13 +40,18 @@ public void runOpMode() { boolean x = true; double loffset = 0; double roffset = 0; - double prevtime= 0; + double prevTime= 0; + double currentTime = 0; + double elbow = Elbow.getCurrentPosition(); + double shoulder = Shoulder.getCurrentPosition(); + PID elbowPID = new PID(1,.0000000001,.00001); + PID shoulderPID = new PID(1,.0000000001,.00001); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); - Shoulder.setPower(Range.clip(gamepad2.left_stick_y, -1,1)); - Elbow.setPower(Range.clip(-gamepad2.right_stick_y, -1,1)); + Shoulder.setPower(Range.clip(shoulderPID.getPID(), -1,1)); + Elbow.setPower(Range.clip(elbowPID.getPID(), -1,1)); if(gamepad2.right_bumper&&!bumper){ wristdir = wristdir*-1; } @@ -69,13 +74,17 @@ public void runOpMode() { } x = gamepad2.x; LGrabber.setPosition(Range.clip(LGrabber.MIN_POSITION+loffset,LGrabber.MIN_POSITION,LGrabber.MAX_POSITION)); - + prevTime = currentTime; + currentTime =getRuntime(); + elbow-=gamepad1.right_stick_y*(currentTime-prevTime)*4/100; + shoulder-=gamepad1.left_stick_y*(currentTime-prevTime)*4/100; + elbowPID.iteratePID(elbow-Elbow.getCurrentPosition(), currentTime-prevTime); + shoulderPID.iteratePID(shoulder-Shoulder.getCurrentPosition(), currentTime-prevTime); //Winch code- up: right trigger, down: left trigger Winch.setPower(Range.clip(gamepad1.right_trigger-gamepad1.left_trigger,-1,1)); //Sideways- front wheel, push both sticks to control // both gamepads are inversed, so need to minus Sideways.setPower(Range.clip(-gamepad1.right_stick_y+gamepad1.left_stick_y,-1,1)); - prevtime = getRuntime(); //telemetry.addData("bools",liftmode+" "+a); //telemetry.addData("lift pos", Lift.getCurrentPosition()); telemetry.addData("loffset",loffset); From 81e530ee3736fb912615741d79c3c1ffffe877c6 Mon Sep 17 00:00:00 2001 From: tazule Date: Fri, 14 Dec 2018 21:17:00 -0800 Subject: [PATCH 37/38] last min grind --- .../ftc/teamcode/TankDriveRoverRuckus.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index fa81c877130..c38cf36c2ce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -17,22 +17,22 @@ public class TankDriveRoverRuckus extends LinearOpMode { private DcMotor Right; private DcMotor Shoulder; private DcMotor Elbow; - private DcMotor Wrist; + //private DcMotor Wrist; private Servo LGrabber; private Servo RGrabber; - private DcMotor Winch; - private DcMotor Sideways; + //private DcMotor Winch; + //private DcMotor Sideways; @Override public void runOpMode() { Left = hardwareMap.get(DcMotor.class, "left"); Right = hardwareMap.get(DcMotor.class, "right"); Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); Elbow = hardwareMap.get(DcMotor.class, "elbow"); - Wrist = hardwareMap.get(DcMotor.class,"wrist"); + //Wrist = hardwareMap.get(DcMotor.class,"wrist"); LGrabber = hardwareMap.get(Servo.class, "lgrab"); RGrabber = hardwareMap.get(Servo.class, "rgrab"); - Winch = hardwareMap.get(DcMotor.class,"winch"); - Sideways = hardwareMap.get(DcMotor.class,"sideways"); + //Winch = hardwareMap.get(DcMotor.class,"winch"); + //Sideways = hardwareMap.get(DcMotor.class,"sideways"); boolean bumper = false; int wristdir = 1; waitForStart(); @@ -58,7 +58,7 @@ public void runOpMode() { wristdir = wristdir*-1; } bumper = gamepad2.right_bumper; - Wrist.setPower(Range.clip(.5*gamepad2.right_trigger*wristdir,-1,1)); + //Wrist.setPower(Range.clip(.5*gamepad2.right_trigger*wristdir,-1,1)); /*if(gamepad2.b&&!b){ roffset = roffset*-1; }*/ @@ -83,10 +83,10 @@ public void runOpMode() { elbowPID.iteratePID(elbow-Elbow.getCurrentPosition(), currentTime-prevTime); shoulderPID.iteratePID(shoulder-Shoulder.getCurrentPosition(), currentTime-prevTime); //Winch code- up: right trigger, down: left trigger - Winch.setPower(Range.clip(gamepad1.right_trigger-gamepad1.left_trigger,-1,1)); + //Winch.setPower(Range.clip(gamepad1.right_trigger-gamepad1.left_trigger,-1,1)); //Sideways- front wheel, push both sticks to control // both gamepads are inversed, so need to minus - Sideways.setPower(Range.clip(-gamepad1.right_stick_y+gamepad1.left_stick_y,-1,1)); + //Sideways.setPower(Range.clip(-gamepad1.right_stick_y+gamepad1.left_stick_y,-1,1)); //telemetry.addData("bools",liftmode+" "+a); //telemetry.addData("lift pos", Lift.getCurrentPosition()); telemetry.addData("loffset",loffset); From 3b335b1dc3b569cddf68ff52831ff1254457801a Mon Sep 17 00:00:00 2001 From: Emilyhinds Date: Sat, 15 Dec 2018 10:26:46 -0800 Subject: [PATCH 38/38] Fixing controls Tried to make a simpler tele op but we decided to stick to the original --- .../firstinspires/ftc/teamcode/CleanTank.java | 41 +++++++++++++++++ .../ftc/teamcode/TankDriveRoverRuckus.java | 46 +++++++++---------- 2 files changed, 64 insertions(+), 23 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleanTank.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleanTank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleanTank.java new file mode 100644 index 00000000000..14520ad02e8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CleanTank.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.teamcode.RoboticsUtils.PID; +//import org.firstinspires.ftc.robotcontroller.external.samples.HardwareK9bot; +/** + * Created by emilyhinds on 12/14/18. + */ +@TeleOp(name="CleanTank", group="CleanTank") +public class CleanTank extends LinearOpMode { + + // Declare OpMode members. + private DcMotor Left; + private DcMotor Right; + private DcMotor Shoulder; + private DcMotor Elbow; + + @Override + public void runOpMode() { + Left = hardwareMap.get(DcMotor.class, "left"); + Right = hardwareMap.get(DcMotor.class, "right"); + Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); + Elbow = hardwareMap.get(DcMotor.class, "elbow"); + waitForStart(); + + while (opModeIsActive()) { + Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); + Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); + Shoulder.setPower(.5 * Range.clip(gamepad2.left_stick_y, -1, 1)); + Elbow.setPower(.5 * Range.clip(-gamepad2.right_stick_y, -1, 1)); + + } + } + + + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java index c38cf36c2ce..2ba80d8e1a7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDriveRoverRuckus.java @@ -18,8 +18,8 @@ public class TankDriveRoverRuckus extends LinearOpMode { private DcMotor Shoulder; private DcMotor Elbow; //private DcMotor Wrist; - private Servo LGrabber; - private Servo RGrabber; + // private Servo LGrabber; + //private Servo RGrabber; //private DcMotor Winch; //private DcMotor Sideways; @Override @@ -29,14 +29,14 @@ public void runOpMode() { Shoulder = hardwareMap.get(DcMotor.class, "shoulder"); Elbow = hardwareMap.get(DcMotor.class, "elbow"); //Wrist = hardwareMap.get(DcMotor.class,"wrist"); - LGrabber = hardwareMap.get(Servo.class, "lgrab"); - RGrabber = hardwareMap.get(Servo.class, "rgrab"); + //LGrabber = hardwareMap.get(Servo.class, "lgrab"); + // RGrabber = hardwareMap.get(Servo.class, "rgrab"); //Winch = hardwareMap.get(DcMotor.class,"winch"); //Sideways = hardwareMap.get(DcMotor.class,"sideways"); - boolean bumper = false; - int wristdir = 1; + //boolean bumper = false; + //int wristdir = 1; waitForStart(); - boolean b = true; + /* boolean b = true; boolean x = true; double loffset = 0; double roffset = 0; @@ -44,25 +44,25 @@ public void runOpMode() { double currentTime = 0; double elbow = Elbow.getCurrentPosition(); double shoulder = Shoulder.getCurrentPosition(); - PID elbowPID = new PID(1,.0000000001,.00001); - PID shoulderPID = new PID(1,.0000000001,.00001); - // run until the end of the match (driver presses STOP) + //PID elbowPID = new PID(1,.0000000001,.00001); + //PID shoulderPID = new PID(1,.0000000001,.00001); + // run until the end of the match (driver presses STOP) */ while (opModeIsActive()) { Left.setPower(Range.clip(gamepad1.left_stick_y, -1, 1)); Right.setPower(Range.clip(-gamepad1.right_stick_y, -1, 1)); - Shoulder.setPower(Range.clip(shoulderPID.getPID(), -1,1)); - Elbow.setPower(Range.clip(elbowPID.getPID(), -1,1)); + //Shoulder.setPower(Range.clip(shoulderPID.getPID(), -1,1)); + //Elbow.setPower(Range.clip(elbowPID.getPID(), -1,1)); Shoulder.setPower(.5*Range.clip(gamepad2.left_stick_y, -1,1)); - Elbow.setPower(.5*Range.clip(-gamepad2.right_stick_y, -1,1)); - if(gamepad2.right_bumper&&!bumper){ + Elbow.setPower(.5*Range.clip(gamepad2.right_stick_y, -1,1)); + /*if(gamepad2.right_bumper&&!bumper){ wristdir = wristdir*-1; } bumper = gamepad2.right_bumper; - //Wrist.setPower(Range.clip(.5*gamepad2.right_trigger*wristdir,-1,1)); + //Wrist.setPower(Range.clip(.5*gamepad2.right_trigger*wristdir,-1,1)); */ /*if(gamepad2.b&&!b){ roffset = roffset*-1; }*/ - if(gamepad2.b&&!b&&loffset==0){ + /* if(gamepad2.b&&!b&&loffset==0){ roffset=.6; }else if(gamepad2.b&&!b){ roffset=0; @@ -77,11 +77,11 @@ public void runOpMode() { x = gamepad2.x; LGrabber.setPosition(Range.clip(LGrabber.MIN_POSITION+loffset,LGrabber.MIN_POSITION,LGrabber.MAX_POSITION)); prevTime = currentTime; - currentTime =getRuntime(); - elbow-=gamepad1.right_stick_y*(currentTime-prevTime)*4/100; - shoulder-=gamepad1.left_stick_y*(currentTime-prevTime)*4/100; - elbowPID.iteratePID(elbow-Elbow.getCurrentPosition(), currentTime-prevTime); - shoulderPID.iteratePID(shoulder-Shoulder.getCurrentPosition(), currentTime-prevTime); + currentTime =getRuntime(); */ + //elbow-=gamepad1.right_stick_y*(currentTime-prevTime)*4/100; + //shoulder-=gamepad1.left_stick_y*(currentTime-prevTime)*4/100; + //elbowPID.iteratePID(elbow-Elbow.getCurrentPosition(), currentTime-prevTime); + //shoulderPID.iteratePID(shoulder-Shoulder.getCurrentPosition(), currentTime-prevTime); //Winch code- up: right trigger, down: left trigger //Winch.setPower(Range.clip(gamepad1.right_trigger-gamepad1.left_trigger,-1,1)); //Sideways- front wheel, push both sticks to control @@ -89,8 +89,8 @@ public void runOpMode() { //Sideways.setPower(Range.clip(-gamepad1.right_stick_y+gamepad1.left_stick_y,-1,1)); //telemetry.addData("bools",liftmode+" "+a); //telemetry.addData("lift pos", Lift.getCurrentPosition()); - telemetry.addData("loffset",loffset); - telemetry.addData("roffset", roffset); + //telemetry.addData("loffset",loffset); + //telemetry.addData("roffset", roffset); telemetry.update(); } }