From e4dd2147d6e9f8b5cc2039dbbe4749210bdcac47 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 6 Feb 2026 21:39:23 -0600 Subject: [PATCH] bunch of minor changes plus major change in running auton with customizable settings --- ...12Ball_Indexed.java => Auto_LT_Close.java} | 556 ++++++++++-------- .../autonomous/Auto_LT_Close_12Ball.java | 111 ++-- .../autonomous/Auto_LT_Close_GateOpen.java | 112 ++-- ...uto_LT_Far_3Ball.java => Auto_LT_Far.java} | 93 +-- .../disabled/ProtoAutoClose_V3.java | 86 +-- .../ftc/teamcode/constants/Back_Poses.java | 10 + .../ftc/teamcode/constants/Front_Poses.java | 52 ++ .../ftc/teamcode/constants/Poses.java | 49 -- .../ftc/teamcode/teleop/TeleopV3.java | 31 +- .../ftc/teamcode/utils/Robot.java | 7 +- .../ftc/teamcode/utils/Targeting.java | 1 + 11 files changed, 607 insertions(+), 501 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{Auto_LT_Close_12Ball_Indexed.java => Auto_LT_Close.java} (64%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{Auto_LT_Far_3Ball.java => Auto_LT_Far.java} (90%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java similarity index 64% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 7626e53..bb5a7b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball_Indexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -1,62 +1,69 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bLeaveY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rLeaveY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; @@ -64,6 +71,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import androidx.annotation.NonNull; @@ -82,7 +90,6 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.constants.Poses_V2; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -95,7 +102,7 @@ import java.util.Objects; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { +public class Auto_LT_Close extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93 + hoodOffset; public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; @@ -110,7 +117,7 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { public static double blueObeliskTurrPos1 = -0.12; public static double blueObeliskTurrPos2 = -0.13; public static double blueObeliskTurrPos3 = -0.14; - public static double redTurretShootPos = 0.12; + public static double redTurretShootPos = 0.1; public static double blueTurretShootPos = -0.14; double obeliskTurrPos1 = 0.0; @@ -163,21 +170,22 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { Targeting.Settings targetingSettings; private double firstSpindexShootPos = autoSpinStartPos; private boolean shootForward = true; - private double x1, y1, h1; + double x1, y1, h1; - private double x2a, y2a, h2a, t2a; + double x2a, y2a, h2a, t2a; - private double x2b, y2b, h2b, t2b; - private double x2c, y2c, h2c, t2c; + double x2b, y2b, h2b, t2b; + double x2c, y2c, h2c, t2c; - private double x3a, y3a, h3a; - private double x3b, y3b, h3b; - private double x4a, y4a, h4a; - private double x4b, y4b, h4b; + double x3a, y3a, h3a; + double x3b, y3b, h3b; + double x4a, y4a, h4a; + double x4b, y4b, h4b; - private double xShoot, yShoot, hShoot; - private double xGate, yGate, hGate; - private double xPrep, yPrep, hPrep; + double xShoot, yShoot, hShoot; + double xGate, yGate, hGate; + double xPrep, yPrep, hPrep; + double xLeave, yLeave, hLeave; private double shoot1Tangent; @@ -186,6 +194,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { private int rearSlotGreen = 0; private int mostGreenSlot = 0; + int ballCycles = 3; + int prevMotif = 0; public Action prepareShootAll(double colorSenseTime, double time, int motif_id) { return new Action() { @@ -340,7 +350,25 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { teleStart = drive.localizer.getPose(); - if (getRuntime() - stamp < shootTime) { + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { robot.spin1.setPosition(autoSpinStartPos); @@ -403,6 +431,24 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { teleStart = drive.localizer.getPose(); + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + if (getRuntime() - stamp < shootTime) { if (getRuntime() - stamp < firstShootTime) { @@ -457,11 +503,8 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; } @@ -781,6 +824,13 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { turrDefault += 0.01; } + if (gamepad2.rightBumperWasPressed()){ + ballCycles++; + } + if (gamepad2.leftBumperWasPressed()){ + ballCycles--; + } + if (redAlliance) { robot.light.setPosition(0.28); @@ -814,6 +864,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { xShoot = rShootX; yShoot = rShootY; hShoot = rShootH; + xLeave = rLeaveX; + yLeave = rLeaveY; + hLeave = rLeaveH; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; @@ -854,6 +907,9 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { xShoot = bShootX; yShoot = bShootY; hShoot = bShootH; + xLeave = bLeaveX; + yLeave = bLeaveY; + hLeave = bLeaveH; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; @@ -863,33 +919,45 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { } shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) - .strafeToLinearHeading(new Vector2d(x1, y1), h1); + .strafeToLinearHeading(new Vector2d(x1, y1), Math.toRadians(h1)); - pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) - .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, + pickup1 = drive.actionBuilder(new Pose2d(x1, y1, Math.toRadians(h1))) + .strafeToLinearHeading(new Vector2d(x2a, y2a), Math.toRadians(h2a)) + .strafeToLinearHeading(new Vector2d(x2b, y2b), Math.toRadians(h2b), new TranslationalVelConstraint(pickup1Speed)); - shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + if (ballCycles < 2){ + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + } else { + shoot1 = drive.actionBuilder(new Pose2d(x2b, y2b, Math.toRadians(h2b))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); + } - pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) - .strafeToLinearHeading(new Vector2d(x3a, y3a), h3a) - .strafeToLinearHeading(new Vector2d(x3b, y3b), h3b, + pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(x3a, y3a), Math.toRadians(h3a)) + .strafeToLinearHeading(new Vector2d(x3b, y3b), Math.toRadians(h3b), new TranslationalVelConstraint(pickup1Speed)); - shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, h3b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + if (ballCycles < 3){ + shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + } else { + shoot2 = drive.actionBuilder(new Pose2d(x3b, y3b, Math.toRadians(h3b))) + .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hLeave)); + } - pickup3 = drive.actionBuilder(new Pose2d(x1, y1, h1)) - .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) - .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, + pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) + .strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a)) + .strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b), new TranslationalVelConstraint(pickup1Speed)); - shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) - .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); + + shoot3 = drive.actionBuilder(new Pose2d(x4b, y4b, Math.toRadians(h4b))) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); + TELE.addData("Ball Cycles", ballCycles); TELE.update(); } @@ -924,178 +992,184 @@ public class Auto_LT_Close_12Ball_Indexed extends LinearOpMode { shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) ); - Actions.runBlocking( - new ParallelAction( - pickup1.build(), - manageFlywheel( - shootAllVelocity, - shootAllHood, - intake1Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake1Time), - detectObelisk( - intake1Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos1 - ) + if (ballCycles > 0){ + Actions.runBlocking( + new ParallelAction( + pickup1.build(), + manageFlywheel( + shootAllVelocity, + shootAllHood, + intake1Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake1Time), + detectObelisk( + intake1Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos1 + ) - ) - ); + ) + ); - motif = turret.getObeliskID(); + motif = turret.getObeliskID(); - if (motif == 0) motif = 22; - int prevMotif = motif; + if (motif == 0) motif = 22; + prevMotif = motif; - Actions.runBlocking( - new ParallelAction( - manageFlywheel( - shootAllVelocity, - shootAllHood, - shoot1Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot1.build(), - prepareShootAll(colorSenseTime, shoot1Time, motif) - ) - ); + Actions.runBlocking( + new ParallelAction( + manageFlywheel( + shootAllVelocity, + shootAllHood, + shoot1Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot1.build(), + prepareShootAll(colorSenseTime, shoot1Time, motif) + ) + ); - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) - ); + ); + } - Actions.runBlocking( - new ParallelAction( - pickup2.build(), - manageShooterAuto( - intake2Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake2Time), - detectObelisk( - intake2Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos2 - ) + if (ballCycles > 1){ + Actions.runBlocking( + new ParallelAction( + pickup2.build(), + manageShooterAuto( + intake2Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake2Time), + detectObelisk( + intake2Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos2 + ) - ) - ); + ) + ); - motif = turret.getObeliskID(); + motif = turret.getObeliskID(); - if (motif == 0) motif = prevMotif; - prevMotif = motif; + if (motif == 0) motif = prevMotif; + prevMotif = motif; - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot2Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot2.build(), - prepareShootAll(colorSenseTime, shoot2Time, motif) - ) - ); + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot2Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot2.build(), + prepareShootAll(colorSenseTime, shoot2Time, motif) + ) + ); - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - shootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(shootAllTime, spindexerSpeedIncrease) - ) + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + shootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(shootAllTime, spindexerSpeedIncrease) + ) - ); + ); + } - Actions.runBlocking( - new ParallelAction( - pickup3.build(), - manageShooterAuto( - intake3Time, - x2b, - y2b, - pickup1XTolerance, - pickup1YTolerance - ), - intake(intake3Time), - detectObelisk( - intake3Time, - 0.501, - 0.501, - obelisk1XTolerance, - obelisk1YTolerance, - obeliskTurrPos3 - ) + if (ballCycles > 2){ + Actions.runBlocking( + new ParallelAction( + pickup3.build(), + manageShooterAuto( + intake3Time, + x2b, + y2b, + pickup1XTolerance, + pickup1YTolerance + ), + intake(intake3Time), + detectObelisk( + intake3Time, + 0.501, + 0.501, + obelisk1XTolerance, + obelisk1YTolerance, + obeliskTurrPos3 + ) - ) - ); + ) + ); - motif = turret.getObeliskID(); + motif = turret.getObeliskID(); - if (motif == 0) motif = prevMotif; - prevMotif = motif; + if (motif == 0) motif = prevMotif; + prevMotif = motif; - Actions.runBlocking( - new ParallelAction( - manageFlywheelAuto( - shoot3Time, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shoot3.build(), - prepareShootAll(colorSenseTime, shoot3Time, motif) - ) - ); + Actions.runBlocking( + new ParallelAction( + manageFlywheelAuto( + shoot3Time, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shoot3.build(), + prepareShootAll(colorSenseTime, shoot3Time, motif) + ) + ); - Actions.runBlocking( - new ParallelAction( - manageShooterAuto( - finalShootAllTime, - 0.501, - 0.501, - 0.501, - 0.501 - ), - shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) - ) + Actions.runBlocking( + new ParallelAction( + manageShooterAuto( + finalShootAllTime, + 0.501, + 0.501, + 0.501, + 0.501 + ), + shootAllAuto(finalShootAllTime, finalSpindexerSpeedIncrease) + ) - ); + ); + } while (opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java index 56376fe..ad11f86 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java @@ -1,61 +1,61 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; @@ -76,7 +76,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.constants.Poses_V2; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java index 268855c..86cd3b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateOpen.java @@ -1,61 +1,61 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.bShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.byPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootH; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootX; -import static org.firstinspires.ftc.teamcode.constants.Poses.rShootY; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rhPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rxPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3aG; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ryPrep; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.byPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rShootY; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rhPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rxPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3aG; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ryPrep; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; @@ -78,6 +78,7 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.constants.Poses_V2; @@ -91,6 +92,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; import java.util.Objects; +@Disabled @Config @Autonomous(preselectTeleOp = "TeleopV3") public class Auto_LT_Close_GateOpen extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java index 3cac5a7..fa0a85d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far_3Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Far.java @@ -1,9 +1,16 @@ package org.firstinspires.ftc.teamcode.autonomous; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveX; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.bLeaveY; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveH; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveX; +import static org.firstinspires.ftc.teamcode.constants.Back_Poses.rLeaveY; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleEnd; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodOffset; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinEndPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; @@ -11,6 +18,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import androidx.annotation.NonNull; @@ -22,6 +30,8 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.ParallelAction; import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -38,7 +48,7 @@ import java.util.Objects; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class Auto_LT_Far_3Ball extends LinearOpMode { +public class Auto_LT_Far extends LinearOpMode { public static double shoot0Vel = 3200, shoot0Hood = 0.5 + hoodOffset; public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.015; @@ -46,12 +56,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { public static double redTurretShootPos = 0.05; public static double blueTurretShootPos = -0.05; public static int fwdTime = 200, strafeTime = 2300; - public static double rPickupGateX = 1, rPickupGateY = 1, rPickupGateH = 1; - public static double rPickupZoneX = 1, rPickupZoneY = 1, rPickupZoneH = 1; - public static double rShootX = 1, rShootY = 1, rShootH = 1; - public static double bPickupZoneX = 1, bPickupZoneY = 1, bPickupZoneH = 1; - public static double bPickupGateX = 1, bPickupGateY = 1, bPickupGateH = 1; - public static double bShootX = 1, bShootY = 1, bShootH = 1; + double xLeave, yLeave, hLeave; public static int sleepTime = 1300; public int motif = 0; double turretShootPos = 0.0; @@ -228,7 +233,25 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { teleStart = drive.localizer.getPose(); - if (getRuntime() - stamp < shootTime) { + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -15; + double goalY = 0; + + double dx = robX - goalX; // delta x from robot to goal + double dy = robY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + targetingSettings = targeting.calculateSettings + (robX, robY, robotHeading, 0.0, turretInterpolate); + + turret.trackGoal(deltaPose); + + if ((getRuntime() - stamp < shootTime && servos.getSpinPos() < spinEndPos) || shooterTicker == 0) { if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { robot.spin1.setPosition(autoSpinStartPos); @@ -345,11 +368,8 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { drive.updatePoseEstimate(); teleStart = drive.localizer.getPose(); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Hood", robot.hood.getPosition()); - TELE.update(); - return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); + return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull(); } }; } @@ -642,6 +662,8 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { robot.light.setPosition(1); + TrajectoryActionBuilder leave = null; + while (opModeInInit()) { // Recalibration in initialization @@ -669,37 +691,25 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { if (redAlliance) { robot.light.setPosition(0.28); - pickupGateX = rPickupGateX; - pickupGateY = rPickupGateY; - pickupGateH = rPickupGateH; - - pickupZoneX = rPickupZoneX; - pickupZoneY = rPickupZoneY; - pickupZoneH = rPickupZoneH; - - xShoot = rShootX; - yShoot = rShootY; - hShoot = rShootH; + xLeave = rLeaveX; + yLeave = rLeaveY; + hLeave = rLeaveH; turretShootPos = turrDefault + redTurretShootPos; } else { robot.light.setPosition(0.6); - pickupGateX = bPickupGateX; - pickupGateY = bPickupGateY; - pickupGateH = bPickupGateH; - - pickupZoneX = bPickupZoneX; - pickupZoneY = bPickupZoneY; - pickupZoneH = bPickupZoneH; - - xShoot = bShootX; - yShoot = bShootY; - hShoot = bShootH; + xLeave = bLeaveX; + yLeave = bLeaveY; + hLeave = bLeaveH; turretShootPos = turrDefault + blueTurretShootPos; } + leave = drive.actionBuilder(teleEnd) + .strafeToLinearHeading(new Vector2d(xLeave, yLeave), Math.toRadians(hLeave)); + + TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); TELE.addData("Start Position", teleEnd); @@ -721,7 +731,7 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { manageFlywheel( shoot0Vel, shoot0Hood, - 9, + 5, 0.501, 0.501, shoot0XTolerance, @@ -736,9 +746,16 @@ public class Auto_LT_Far_3Ball extends LinearOpMode { teleStart = drive.localizer.getPose(); Actions.runBlocking( - shootAll((int) shoot0Vel, 6, shoot0SpinSpeedIncrease) + shootAll((int) shoot0Vel, 4, shoot0SpinSpeedIncrease) ); + drive.updatePoseEstimate(); + + teleStart = drive.localizer.getPose(); + + assert leave != null; + Actions.runBlocking(leave.build()); + // Actual way to end autonomous in to find final position while (opModeIsActive()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java index d2b5214..755b71f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java @@ -1,49 +1,49 @@ package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.bx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by1; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.by4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rh4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx1; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.rx4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry1; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry2b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry3b; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4a; -import static org.firstinspires.ftc.teamcode.constants.Poses.ry4b; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.bx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.by4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rh4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.rx4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry1; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry2b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry3b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4a; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.ry4b; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java new file mode 100644 index 0000000..7927364 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Back_Poses.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; + +@Config +public class Back_Poses { + public static double rLeaveX = 80, rLeaveY = 70, rLeaveH = 50; + public static double bLeaveX = 80, bLeaveY = -70, bLeaveH = -50; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java new file mode 100644 index 0000000..43e18df --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; + +@Config +public class Front_Poses { + + + public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; + public static double rx2a = 41, ry2a = 18, rh2a = 140; + public static double rx2b = 19, ry2b = 40, rh2b = 140; + + + public static double rx3a = 55, ry3a = 39, rh3a = 140; + + public static double rx3aG = 60, ry3aG = 34, rh3aG = 140; + + public static double rx3b = 36, ry3b = 58, rh3b = 140.1; + + public static double rx4a = 80, ry4a = 48, rh4a = 140; + public static double rx4b = 45, ry4b = 83, rh4b = 140.1; + + public static double bx1 = 20, by1 = 0.5, bh1 = 0.1; + public static double bx2a = 41, by2a = -18, bh2a = -140; + public static double bx2b = 19, by2b = -40, bh2b = -140.1; + + public static double bx3a = 55, by3a = -39, bh3a = -140; + public static double bx3b = 41, by3b = -59, bh3b = -140.1; + public static double bx3aG = 55, by3aG = -43, bh3aG = -140; + + public static double bx4a = 75, by4a = -53, bh4a = -140; + public static double bx4b = 47, by4b = -85, bh4b = -140.1; + public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; + + public static double rShootX = 40, rShootY = 10, rShootH = 50; + public static double rxPrep = 45, ryPrep = 10, rhPrep = 50; + + public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50; + + public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50; + + public static double bShootX = 40, bShootY = 0, bShootH = -50; + public static double bxPrep = 45, byPrep = -10, bhPrep = -50; + + + public static Pose2d teleStart = new Pose2d(0, 0, 0); + + public static Pose2d teleEnd = new Pose2d(0, 0, 0); + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java deleted file mode 100644 index fbdeb9a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ /dev/null @@ -1,49 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - -import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.roadrunner.Pose2d; - -@Config -public class Poses { - - - public static double rx1 = 20, ry1 = 0.5, rh1 = Math.toRadians(0.1); - public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); - public static double rx2b = 19, ry2b = 40, rh2b = Math.toRadians(140.1); - - - public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); - - public static double rx3aG = 60, ry3aG = 34, rh3aG = Math.toRadians(140); - - public static double rx3b = 36, ry3b = 58, rh3b = Math.toRadians(140.1); - - public static double rx4a = 75, ry4a = 53, rh4a = Math.toRadians(140); - public static double rx4b = 45, ry4b = 83, rh4b = Math.toRadians(140.1); - - public static double bx1 = 20, by1 = 0.5, bh1 = Math.toRadians(0.1); - public static double bx2a = 41, by2a = -18, bh2a = Math.toRadians(-140); - public static double bx2b = 19, by2b = -40, bh2b = Math.toRadians(-140.1); - - public static double bx3a = 55, by3a = -39, bh3a = Math.toRadians(-140); - public static double bx3b = 41, by3b = -59, bh3b = Math.toRadians(-140.1); - public static double bx3aG = 55, by3aG = -43, bh3aG = Math.toRadians(-140); - - - public static double bx4a = 75, by4a = -53, bh4a = Math.toRadians(-140); - public static double bx4b = 47, by4b = -85, bh4b = Math.toRadians(-140.1); - public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this - - public static double rShootX = 40, rShootY = -7, rShootH = Math.toRadians(50); - public static double rxPrep = 45, ryPrep = 10, rhPrep = Math.toRadians(50); - - public static double bShootX = 40, bShootY = 7, bShootH = Math.toRadians(-50); - public static double bxPrep = 45, byPrep = -10, bhPrep = Math.toRadians(-50); - - - public static Pose2d teleStart = new Pose2d(0, 0, 0); - - public static Pose2d teleEnd = new Pose2d(0, 0, 0); - - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 34e42c9..e32e81f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleEnd; -import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; +import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import com.acmerobotics.dashboard.FtcDashboard; @@ -70,8 +70,6 @@ public class TeleopV3 extends LinearOpMode { boolean overrideTurr = false; int intakeTicker = 0; - - boolean turretInterpolate = false; private boolean shootAll = false; @Override @@ -141,7 +139,7 @@ public class TeleopV3 extends LinearOpMode { robot.transfer.setPower(1); - double offset; + //TURRET TRACKING double robX = drive.localizer.getPose().position.x; double robY = drive.localizer.getPose().position.y; @@ -318,18 +316,19 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("spinTestCounter", spindexer.counter); // TELE.addData("autoSpintake", autoSpintake); // - TELE.addData("shootall commanded", shootAll); +// TELE.addData("shootall commanded", shootAll); // Targeting Debug - TELE.addData("robotX", robotX); - TELE.addData("robotY", robotY); - TELE.addData("robotInchesX", targeting.robotInchesX); - TELE.addData( "robotInchesY", targeting.robotInchesY); - TELE.addData("Targeting Interpolate", turretInterpolate); - TELE.addData("Targeting GridX", targeting.robotGridX); - TELE.addData("Targeting GridY", targeting.robotGridY); - TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); - TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); - TELE.addData("timeSinceStamp", getRuntime() - shootStamp); +// TELE.addData("robotX", robotX); +// TELE.addData("robotY", robotY); +// TELE.addData("robotInchesX", targeting.robotInchesX); +// TELE.addData( "robotInchesY", targeting.robotInchesY); +// TELE.addData("Targeting Interpolate", turretInterpolate); +// TELE.addData("Targeting GridX", targeting.robotGridX); +// TELE.addData("Targeting GridY", targeting.robotGridY); +// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); +// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); +// TELE.addData("timeSinceStamp", getRuntime() - shootStamp); + TELE.addData("Voltage", robot.voltage.getVoltage()); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 7b07256..f5716a4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -12,6 +12,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; @@ -56,11 +57,11 @@ public class Robot { public RevColorSensorV3 color3; public Limelight3A limelight; public Servo light; + public VoltageSensor voltage; public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map - //TODO: fix the configuration of these - I trust you to figure it out yourself @KeshavAnandCode frontLeft = hardwareMap.get(DcMotorEx.class, "fl"); frontRight = hardwareMap.get(DcMotorEx.class, "fr"); backLeft = hardwareMap.get(DcMotorEx.class, "bl"); @@ -78,7 +79,7 @@ public class Robot { shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); - //TODO: figure out which shooter motor is reversed using ShooterTest and change it in code @KeshavAnandCode + shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); @@ -96,7 +97,6 @@ public class Robot { turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port - //TODO: check spindexer configuration (both servo and analog input) - check comments in PositionalServoProgrammer spin1 = hardwareMap.get(Servo.class, "spin2"); spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); @@ -128,5 +128,6 @@ public class Robot { } light = hardwareMap.get(Servo.class, "light"); + voltage = hardwareMap.voltageSensor.iterator().next(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index 6b32de0..0ca44b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -77,6 +77,7 @@ public class Targeting { double cancelOffsetY = 0.0; // was 7.0 double unitConversionFactor = 0.95; int tileSize = 24; //inches + public static boolean turretInterpolate = false; public Targeting() { }