KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Fully Merged Presumably

Browse Source
This commit is contained in:
Keshav Anand
2026-01-29 15:25:12 -06:00
parent 6b71bb17f4
commit 8840205fb3
6 changed files with 13 additions and 879 deletions
Download Patch File Download Diff File Expand all files Collapse all files

872
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_12Ball.java
View File

@@ -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<Boolean> s1 = new ArrayList<>();
//
// List<Boolean> s2 = new ArrayList<>();
// List<Boolean> 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
//
// }
//
// }
//}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_12Ball.java
View File

@@ -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;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Unindexed.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close_GateCycle.java
View File

@@ -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;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoClose_V3.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoClose_V3.java
View File

@@ -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 {

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoFar_V1.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/AutoFar_V1.java
View File

@@ -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;

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/ProtoAutoClose_V3.java
View File

@@ -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 {