From 8840205fb3d010e2c3130b7556b17597d0972c18 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Thu, 29 Jan 2026 15:25:12 -0600 Subject: [PATCH] Fully Merged Presumably --- .../teamcode/autonomous/Auto_LT_12Ball.java | 872 ------------------ ...lose_V4.java => Auto_LT_Close_12Ball.java} | 2 +- ...exed.java => Auto_LT_Close_GateCycle.java} | 2 +- .../{ => disabled}/AutoClose_V3.java | 4 +- .../autonomous/{ => disabled}/AutoFar_V1.java | 7 +- .../{ => disabled}/ProtoAutoClose_V3.java | 5 +- 6 files changed, 13 insertions(+), 879 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_12Ball.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{ProtoAutoClose_V4.java => Auto_LT_Close_12Ball.java} (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{Auto_LT_Unindexed.java => Auto_LT_Close_GateCycle.java} (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{ => disabled}/AutoClose_V3.java (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{ => disabled}/AutoFar_V1.java (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/{ => disabled}/ProtoAutoClose_V3.java (99%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_12Ball.java deleted file mode 100644 index 518e821..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_12Ball.java +++ /dev/null @@ -1,872 +0,0 @@ -//package org.firstinspires.ftc.teamcode.autonomous; -// -//import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bHGate; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShoot1Tangent; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootH; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootX; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bShootY; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bXGate; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bYGate; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh1; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2a; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2b; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bh2c; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2a; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2b; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bt2c; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx1; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2a; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2b; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.bx2c; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by1; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2a; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2b; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.by2c; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rHGate; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShoot1Tangent; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootH; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootX; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rShootY; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rXGate; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rYGate; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh1; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2a; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2b; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rh2c; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2a; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2b; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rt2c; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx1; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2a; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2b; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.rx2c; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry1; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2a; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2b; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.ry2c; -//import static org.firstinspires.ftc.teamcode.constants.Poses_LT_Indexed.teleStart; -//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; -//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -// -//import androidx.annotation.NonNull; -// -//import com.acmerobotics.dashboard.FtcDashboard; -//import com.acmerobotics.dashboard.config.Config; -//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -//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.SequentialAction; -//import com.acmerobotics.roadrunner.TrajectoryActionBuilder; -//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.LinearOpMode; -//import com.qualcomm.robotcore.hardware.Servo; -// -//import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -//import org.firstinspires.ftc.teamcode.utils.Flywheel; -//import org.firstinspires.ftc.teamcode.utils.Robot; -//import org.firstinspires.ftc.teamcode.utils.Servos; -//import org.firstinspires.ftc.teamcode.utils.Spindexer; -//import org.firstinspires.ftc.teamcode.utils.Targeting; -//import org.firstinspires.ftc.teamcode.utils.Turret; -// -//import java.util.ArrayList; -//import java.util.List; -// -//@Autonomous(preselectTeleOp = "TeleopV3") -//@Config -//public class Auto_LT_Indexed extends LinearOpMode { -// -// public static double shoot0Vel = 2300, shoot0Hood = 0.93; -// public static double autoSpinStartPos = 0.2; -// public static double shoot0SpinSpeedIncrease = 0.014; -// -// public static double spindexerSpeedIncrease = 0.02; -// -// public static double obeliskTurrPos = 0.53; -// public static double gatePickupTime = 3.0; -// public static double shoot1Turr = 0.57; -// public static double shoot0XTolerance = 1.0; -// public static double turretShootPos = 0.72; -// public static double shootAllTime = 1.8; -// public static double shoot0Time = 1.6; -// public static double intake1Time = 3.0; -// public static double flywheel0Time = 3.5; -// public static double pickup1Speed = 20.0; -// public static double pickup2Speed = 20.0; -// public static double pickup3Speed = 20.0; -// -// // ---- SECOND SHOT / PICKUP ---- -// public static double shoot1Vel = 2300; -// public static double shoot1Hood = 0.93; -// -// public static double shootAllVelocity = 2500; -// public static double shootAllHood = 0.78; -// // ---- PICKUP TIMING ---- -// public static double pickup1Time = 3.0; -// // ---- PICKUP POSITION TOLERANCES ---- -// public static double pickup1XTolerance = 2.0; -// public static double pickup1YTolerance = 2.0; -// // ---- OBELISK DETECTION ---- -// public static double obelisk1Time = 1.5; -// public static double obelisk1XTolerance = 2.0; -// public static double obelisk1YTolerance = 2.0; -// public static double shoot1ToleranceX = 2.0; -// public static double shoot1ToleranceY = 2.0; -// public static double shoot1Time = 2.0; -// public static double shootCycleTime = 2.5; -// -// List s1 = new ArrayList<>(); -// -// List s2 = new ArrayList<>(); -// List s3 = new ArrayList<>(); -// public int motif = 0; -// Robot robot; -// MultipleTelemetry TELE; -// MecanumDrive drive; -// Servos servos; -// Spindexer spindexer; -// Flywheel flywheel; -// Turret turret; -// Targeting targeting; -// Targeting.Settings targetingSettings; -// private double x1, y1, h1; -// -// private double x2a, y2a, h2a, t2a; -// -// private double x2b, y2b, h2b, t2b; -// private double x2c, y2c, h2c, t2c; -// -// private double xShoot, yShoot, hShoot; -// private double xGate, yGate, hGate; -// -// private double shoot1Tangent; -// -// // ---- OPEN GATE / MIDFIELD ---- -// private double x3, y3, h3, t3; -// -// // ---- PICKUP 2 ---- -// private double x4a, y4a, h4a; -// private double x4b, y4b, h4b; -// -// // ---- PICKUP 3 ---- -// private double x5a, y5a, h5a; -// private double x5b, y5b, h5b; -// -// // ---- PARK ---- -// private double xPark, yPark, hPark; -// -// public Action prepareShootIndexed(double time) { -// return new Action() { -// double stamp = 0.0; -// int ticker = 0; -// -// boolean testColor = false; -// -// int s1Green = 0; -// int s2Green = 0; -// int s3Green = 0; -// -// -// @Override -// public boolean run(@NonNull TelemetryPacket telemetryPacket) { -// if (ticker == 0) { -// stamp = System.currentTimeMillis(); -// } -// ticker++; -// -// if ((System.currentTimeMillis() - stamp) < (goToDetectTime)) { //0.25-0.4 ish??? -// robot.spin1.setPosition(colorDetectPos); //0.43 -// robot.spin2.setPosition(1 - colorDetectPos); -// } else if ((System.currentTimeMillis() - stamp) < (colorSenseTime + goToDetectTime)) { -// -// -// double green1 = robot.color1.getNormalizedColors().green; -// double red1 = robot.color1.getNormalizedColors().red; -// double blue1 = robot.color1.getNormalizedColors().blue; -// -// double gP1 = green1 / (green1 + red1 + blue1); -// -// -// if (gP1 >= color1Thresh) { -// s1Green ++; -// } -// -// double green2 = robot.color2.getNormalizedColors().green; -// double red2 = robot.color2.getNormalizedColors().red; -// double blue2 = robot.color2.getNormalizedColors().blue; -// -// double gP2 = green2 / (green2 + red2 + blue2); -// -// if (gP2 >= color2Thresh) { -// s2Green ++; -// } -// -// -// double green3 = robot.color3.getNormalizedColors().green; -// double red3 = robot.color3.getNormalizedColors().red; -// double blue3 = robot.color3.getNormalizedColors().blue; -// -// double gP3 = green3 / (green3 + red3 + blue3); -// -// if (gP3 >= color3Thresh) { -// s3Green ++; -// } -// -// -// testColor = true; -// -// -// -// } else { -// -// double spindexPos; -// if (motif == 21) { -// spindexPos = -// } -// } -// -// robot.transferServo.setPosition(transferServo_out); -// -// turret.manualSetTurret(turretShootPos); -// -// robot.intake.setPower(-((System.currentTimeMillis() - stamp)) / 1000); -// -// return (System.currentTimeMillis() - stamp) < (time * 1000); -// -// } -// }; -// } -// -// public Action shootAll(int vel, double shootTime, double spindexSpeed) { -// return new Action() { -// int ticker = 1; -// -// double stamp = 0.0; -// -// double velo = vel; -// -// int shooterTicker = 0; -// -// @Override -// public boolean run(@NonNull TelemetryPacket telemetryPacket) { -// TELE.addData("Velocity", velo); -// TELE.addLine("shooting"); -// TELE.update(); -// -// flywheel.manageFlywheel(vel); -// velo = flywheel.getVelo(); -// -// drive.updatePoseEstimate(); -// -// teleStart = drive.localizer.getPose(); -// -// robot.intake.setPower(-0.3); -// -// if (ticker == 1) { -// stamp = getRuntime(); -// } -// ticker++; -// -// robot.intake.setPower(0); -// -// if (getRuntime() - stamp < shootTime) { -// -// if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { -// robot.spin1.setPosition(autoSpinStartPos); -// robot.spin2.setPosition(1 - autoSpinStartPos); -// } else { -// robot.transferServo.setPosition(transferServo_in); -// shooterTicker++; -// double prevSpinPos = robot.spin1.getPosition(); -// robot.spin1.setPosition(prevSpinPos + spindexSpeed); -// robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); -// } -// -// return true; -// -// } else { -// robot.transferServo.setPosition(transferServo_out); -// //spindexPos = spindexer_intakePos1; -// -// spindexer.resetSpindexer(); -// spindexer.processIntake(); -// -// return false; -// -// } -// -// } -// }; -// } -// -// public Action shootAllAuto(double shootTime, double spindexSpeed) { -// return new Action() { -// int ticker = 1; -// -// double stamp = 0.0; -// -// double velo = 0.0; -// -// int shooterTicker = 0; -// -// @Override -// public boolean run(@NonNull TelemetryPacket telemetryPacket) { -// TELE.addData("Velocity", velo); -// TELE.addLine("shooting"); -// TELE.update(); -// -// velo = flywheel.getVelo(); -// -// drive.updatePoseEstimate(); -// -// teleStart = drive.localizer.getPose(); -// -// robot.intake.setPower(-0.3); -// -// if (ticker == 1) { -// stamp = getRuntime(); -// } -// ticker++; -// -// robot.intake.setPower(0); -// -// if (getRuntime() - stamp < shootTime) { -// -// if (shooterTicker == 0 && !servos.spinEqual(autoSpinStartPos)) { -// robot.spin1.setPosition(autoSpinStartPos); -// robot.spin2.setPosition(1 - autoSpinStartPos); -// } else { -// robot.transferServo.setPosition(transferServo_in); -// shooterTicker++; -// double prevSpinPos = robot.spin1.getPosition(); -// robot.spin1.setPosition(prevSpinPos + spindexSpeed); -// robot.spin2.setPosition(1 - prevSpinPos - spindexSpeed); -// } -// -// return true; -// -// } else { -// robot.transferServo.setPosition(transferServo_out); -// //spindexPos = spindexer_intakePos1; -// -// spindexer.resetSpindexer(); -// spindexer.processIntake(); -// -// return false; -// -// } -// -// } -// }; -// } -// -// public Action intake(double intakeTime) { -// return new Action() { -// double stamp = 0.0; -// int ticker = 0; -// -// @Override -// public boolean run(@NonNull TelemetryPacket telemetryPacket) { -// if (ticker == 0) { -// stamp = System.currentTimeMillis(); -// } -// ticker++; -// -// spindexer.processIntake(); -// robot.intake.setPower(1); -// -// motif = turret.detectObelisk(); -// -// spindexer.ballCounterLight(); -// -// return (System.currentTimeMillis() - stamp) < (intakeTime * 1000); -// -// } -// }; -// } -// -// public Action detectObelisk( -// double time, -// double posX, -// double posY, -// double posXTolerance, -// double posYTolerance, -// double turrPos -// ) { -// -// boolean timeFallback = (time != 0.501); -// boolean posXFallback = (posX != 0.501); -// boolean posYFallback = (posY != 0.501); -// -// return new Action() { -// -// double stamp = 0.0; -// int ticker = 0; -// -// @Override -// public boolean run(@NonNull TelemetryPacket telemetryPacket) { -// -// drive.updatePoseEstimate(); -// Pose2d currentPose = drive.localizer.getPose(); -// -// if (ticker == 0) { -// stamp = System.currentTimeMillis(); -// } -// -// ticker++; -// -// motif = turret.detectObelisk(); -// -// robot.turr1.setPosition(turrPos); -// robot.turr2.setPosition(1 - turrPos); -// -// boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; -// boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; -// boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; -// -// boolean shouldFinish = timeDone || xDone || yDone; -// -// return !shouldFinish; -// -// } -// }; -// } -// -// public Action manageFlywheel( -// double vel, -// double hoodPos, -// double time, -// double posX, -// double posY, -// double posXTolerance, -// double posYTolerance -// ) { -// -// boolean timeFallback = (time != 0.501); -// boolean posXFallback = (posX != 0.501); -// boolean posYFallback = (posY != 0.501); -// -// return new Action() { -// -// double stamp = 0.0; -// int ticker = 0; -// -// @Override -// public boolean run(@NonNull TelemetryPacket telemetryPacket) { -// -// drive.updatePoseEstimate(); -// Pose2d currentPose = drive.localizer.getPose(); -// -// if (ticker == 0) { -// stamp = System.currentTimeMillis(); -// } -// -// ticker++; -// -// flywheel.manageFlywheel(vel); -// robot.hood.setPosition(hoodPos); -// -// boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; -// boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; -// boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; -// -// boolean shouldFinish = timeDone || xDone || yDone; -// -// return !shouldFinish; -// -// } -// }; -// } -// -// public Action manageShooterAuto( -// double time, -// double posX, -// double posY, -// double posXTolerance, -// double posYTolerance -// ) { -// -// boolean timeFallback = (time != 0.501); -// boolean posXFallback = (posX != 0.501); -// boolean posYFallback = (posY != 0.501); -// -// return new Action() { -// -// double stamp = 0.0; -// int ticker = 0; -// -// @Override -// public boolean run(@NonNull TelemetryPacket telemetryPacket) { -// -// drive.updatePoseEstimate(); -// Pose2d currentPose = drive.localizer.getPose(); -// -// if (ticker == 0) { -// stamp = System.currentTimeMillis(); -// } -// -// ticker++; -// -// double robotX = drive.localizer.getPose().position.x; -// double robotY = drive.localizer.getPose().position.y; -// -// double robotHeading = drive.localizer.getPose().heading.toDouble(); -// -// double goalX = -15; -// double goalY = 0; -// -// double dx = robotX - goalX; // delta x from robot to goal -// double dy = robotY - 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 -// (robotX, robotY, robotHeading, 0.0, false); -// -// turret.trackGoal(deltaPose); -// -// robot.hood.setPosition(targetingSettings.hoodAngle); -// -// flywheel.manageFlywheel(targetingSettings.flywheelRPM); -// -// boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; -// boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; -// boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; -// -// boolean shouldFinish = timeDone || xDone || yDone; -// -// return !shouldFinish; -// -// } -// }; -// } -// -// public Action manageFlywheelAuto( -// double time, -// double posX, -// double posY, -// double posXTolerance, -// double posYTolerance -// ) { -// -// boolean timeFallback = (time != 0.501); -// boolean posXFallback = (posX != 0.501); -// boolean posYFallback = (posY != 0.501); -// -// return new Action() { -// -// double stamp = 0.0; -// int ticker = 0; -// -// @Override -// public boolean run(@NonNull TelemetryPacket telemetryPacket) { -// -// drive.updatePoseEstimate(); -// Pose2d currentPose = drive.localizer.getPose(); -// -// if (ticker == 0) { -// stamp = System.currentTimeMillis(); -// } -// -// ticker++; -// -// double robotX = drive.localizer.getPose().position.x; -// double robotY = drive.localizer.getPose().position.y; -// -// double robotHeading = drive.localizer.getPose().heading.toDouble(); -// -// double goalX = -15; -// double goalY = 0; -// -// double dx = robotX - goalX; // delta x from robot to goal -// double dy = robotY - 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 -// (robotX, robotY, robotHeading, 0.0, false); -// -// robot.hood.setPosition(targetingSettings.hoodAngle); -// -// flywheel.manageFlywheel(targetingSettings.flywheelRPM); -// -// boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; -// boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; -// boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; -// -// boolean shouldFinish = timeDone || xDone || yDone; -// -// return !shouldFinish; -// -// } -// }; -// } -// -// @Override -// public void runOpMode() throws InterruptedException { -// -// hardwareMap.get(Servo.class, "light").setPosition(0); -// -// robot = new Robot(hardwareMap); -// -// TELE = new MultipleTelemetry( -// telemetry, FtcDashboard.getInstance().getTelemetry() -// ); -// -// flywheel = new Flywheel(hardwareMap); -// -// targeting = new Targeting(); -// targetingSettings = new Targeting.Settings(0.0, 0.0); -// -// spindexer = new Spindexer(hardwareMap); -// -// servos = new Servos(hardwareMap); -// -// robot.limelight.pipelineSwitch(1); -// -// turret = new Turret(robot, TELE, robot.limelight); -// -// turret.manualSetTurret(0.4); -// -// drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); -// -// TrajectoryActionBuilder shoot0 = null; -// -// TrajectoryActionBuilder pickup1 = null; -// TrajectoryActionBuilder shoot1 = null; -// TrajectoryActionBuilder pickup2 = null; -// TrajectoryActionBuilder shoot2 = null; -// TrajectoryActionBuilder pickup3 = null; -// TrajectoryActionBuilder shoot3 = null; -// TrajectoryActionBuilder openGate = null; -// TrajectoryActionBuilder park = null; -// -// robot.spin1.setPosition(autoSpinStartPos); -// robot.spin2.setPosition(1 - autoSpinStartPos); -// -// robot.transferServo.setPosition(transferServo_out); -// -// robot.light.setPosition(1); -// -// while (opModeInInit()) { -// -// robot.hood.setPosition(shoot0Hood); -// -// if (gamepad2.crossWasPressed()) { -// redAlliance = !redAlliance; -// } -// -// if (redAlliance) { -// robot.light.setPosition(0.28); -// -// // ===== FIRST SHOT ===== -// x1 = rx1; -// y1 = ry1; -// h1 = rh1; -// -// // ===== PICKUP PATH ===== -// x2a = rx2a; -// y2a = ry2a; -// h2a = rh2a; -// t2a = rt2a; -// -// x2b = rx2b; -// y2b = ry2b; -// h2b = rh2b; -// t2b = rt2b; -// -// x2c = rx2c; -// y2c = ry2c; -// h2c = rh2c; -// t2c = rt2c; -// -// // ===== SHOOT POSE ===== -// xShoot = rShootX; -// yShoot = rShootY; -// hShoot = rShootH; -// shoot1Tangent = rShoot1Tangent; -// -// // ===== GATE ===== -// xGate = rXGate; -// yGate = rYGate; -// hGate = rHGate; -// -// } else { -// robot.light.setPosition(0.6); -// -// // ===== FIRST SHOT ===== -// x1 = bx1; -// y1 = by1; -// h1 = bh1; -// -// // ===== PICKUP PATH ===== -// x2a = bx2a; -// y2a = by2a; -// h2a = bh2a; -// t2a = bt2a; -// -// x2b = bx2b; -// y2b = by2b; -// h2b = bh2b; -// t2b = bt2b; -// -// x2c = bx2c; -// y2c = by2c; -// h2c = bh2c; -// t2c = bt2c; -// -// // ===== SHOOT POSE ===== -// xShoot = bShootX; -// yShoot = bShootY; -// hShoot = bShootH; -// shoot1Tangent = bShoot1Tangent; -// -// // ===== GATE ===== -// xGate = bXGate; -// yGate = bYGate; -// hGate = bHGate; -// } -// -// shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) -// .strafeToLinearHeading(new Vector2d(x1, y1), h1); -// -// pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1)) -// .strafeToLinearHeading(new Vector2d(x2a, y2a), h2a) -// .strafeToLinearHeading(new Vector2d(x2b, y2b), h2b, -// new TranslationalVelConstraint(pickup1Speed)); -// -// openGate = drive.actionBuilder(new Pose2d(x2b, y2b, h2b)) -// .setReversed(true) -// .splineToConstantHeading(new Vector2d(x3, y3), t3); -// -// shoot1 = drive.actionBuilder(new Pose2d(x3, y3, h2b)) -// .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); -// -// pickup2 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) -// .strafeToLinearHeading(new Vector2d(x4a, y4a), h4a) -// .strafeToLinearHeading(new Vector2d(x4b, y4b), h4b, -// new TranslationalVelConstraint(pickup2Speed)); -// shoot2 = drive.actionBuilder(new Pose2d(x4b, y4b, h4b)) -// .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); -// -// pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) -// .strafeToLinearHeading(new Vector2d(x5a, y5a), h5a) -// .strafeToLinearHeading(new Vector2d(x5b, y5b), h5b, -// new TranslationalVelConstraint(pickup3Speed)); -// shoot3 = drive.actionBuilder(new Pose2d(x5b, y5b, h5b)) -// .strafeToLinearHeading(new Vector2d(xShoot, yShoot), hShoot); -// park = drive.actionBuilder(new Pose2d(xShoot, yShoot, hShoot)) -// .strafeToLinearHeading(new Vector2d(xPark, yPark), hPark); -// -// } -// -// waitForStart(); -// -// if (isStopRequested()) return; -// -// if (opModeIsActive()) { -// -// robot.transfer.setPower(1); -// -// assert shoot0 != null; -// -// Actions.runBlocking( -// new ParallelAction( -// shoot0.build(), -// manageFlywheel( -// shoot0Vel, -// shoot0Hood, -// flywheel0Time, -// x1, -// 0.501, -// shoot0XTolerance, -// 0.501 -// ) -// -// ) -// ); -// -// Actions.runBlocking( -// shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease) -// ); -// -// //WORKING SHOOT ALL for the preload -// -// //Pickup first pile -// -// Actions.runBlocking( -// new SequentialAction( -// new ParallelAction( -// pickup1.build(), -// manageFlywheel( -// shootAllVelocity, -// shootAllHood, -// pickup1Time, -// x2b, -// y2b, -// pickup1XTolerance, -// pickup1YTolerance -// ), -// intake(intake1Time), -// detectObelisk( -// obelisk1Time, -// x2b, -// y2c, -// obelisk1XTolerance, -// obelisk1YTolerance, -// obeliskTurrPos -// ) -// ), -// new ParallelAction( -// openGate.build() //TODO: Add other managing stuff here -// ) -// ) -// -// ); -// -// motif = turret.detectObelisk(); //Detect Obelisk if not alr -// -// Actions.runBlocking( -// new ParallelAction( -// manageFlywheel( -// shootAllVelocity, -// shootAllHood, -// shoot1Time, -// 0.501, -// 0.501, -// 0.501, -// 0.501 -// ), -// shoot1.build(), -// prepareShootIndexed(shoot1Time) -// ) -// ); -// -// Actions.runBlocking( -// new ParallelAction( -// manageShooterAuto( -// shootAllTime, -// 0.501, -// 0.501, -// 0.501, -// 0.501 -// ), -// shootAllIndexed(shootAllTime, spindexerSpeedIncrease) -// ) -// -// ); -// -// //Shoot from first pile -// -// } -// -// } -//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java index 039c5c8..8005df8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java @@ -86,7 +86,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Config @Autonomous(preselectTeleOp = "TeleopV3") -public class ProtoAutoClose_V4 extends LinearOpMode { +public class Auto_LT_Close_12Ball extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; public static double autoSpinStartPos = 0.2; public static double shoot0SpinSpeedIncrease = 0.014; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Unindexed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateCycle.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Unindexed.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateCycle.java index 20fc350..89b7da7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Unindexed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateCycle.java @@ -76,7 +76,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret; @Autonomous(preselectTeleOp = "TeleopV3") @Config -public class Auto_LT_Unindexed extends LinearOpMode { +public class Auto_LT_Close_GateCycle extends LinearOpMode { public static double shoot0Vel = 2300, shoot0Hood = 0.93; public static double autoSpinStartPos = 0.2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoClose_V3.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoClose_V3.java index d4af8e7..d38a711 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoClose_V3.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.autonomous; +package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.*; import static org.firstinspires.ftc.teamcode.constants.Poses.*; @@ -21,6 +21,7 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; 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.robotcore.external.navigation.DistanceUnit; @@ -31,6 +32,7 @@ import org.firstinspires.ftc.teamcode.utils.Servos; import java.util.List; +@Disabled @Config @Autonomous(preselectTeleOp = "TeleopV3") public class AutoClose_V3 extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoFar_V1.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoFar_V1.java index 5ec38d6..7ed89b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoFar_V1.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.autonomous; +package org.firstinspires.ftc.teamcode.autonomous.disabled; import static org.firstinspires.ftc.teamcode.constants.Color.*; import static org.firstinspires.ftc.teamcode.constants.Poses.*; @@ -21,6 +21,7 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; 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.robotcore.external.navigation.DistanceUnit; @@ -30,7 +31,9 @@ import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import java.util.List; - +@Disabled +@Config +@Autonomous public class AutoFar_V1 extends LinearOpMode { Robot robot; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java index 656e446..d2b5214 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.autonomous; +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; @@ -72,6 +72,7 @@ import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; 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.robotcore.external.navigation.DistanceUnit; @@ -81,7 +82,7 @@ import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import java.util.List; - +@Disabled @Config @Autonomous(preselectTeleOp = "TeleopV3") public class ProtoAutoClose_V3 extends LinearOpMode {