From 80f095cd572b82f058bb0f39c11a99f1ea3d5030 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Tue, 27 Jan 2026 17:47:17 -0600 Subject: [PATCH] Fixed some stuff presumably..untested --- .../ftc/teamcode/autonomous/Auto_LT.java | 260 ++++++++++++++++++ .../autonomous/ProtoAutoClose_V3.java | 1 + .../ftc/teamcode/constants/Poses_V2.java | 43 +++ .../ftc/teamcode/utils/Robot.java | 1 + build.dependencies.gradle | 8 +- 5 files changed, 309 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java new file mode 100644 index 0000000..ce3098b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT.java @@ -0,0 +1,260 @@ +package org.firstinspires.ftc.teamcode.autonomous; + +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; +import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Poses_V2.*; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto; +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.teleop.TeleopV3.spinPow; +import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease; +import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spindexPos; + +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.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; +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; + +@Autonomous(preselectTeleOp = "TeleopV3") +@Config +public class Auto_LT extends LinearOpMode { + + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + + Servos servos; + Spindexer spindexer; + + Flywheel flywheel; + + + private double x1, y1, h1; + + public static double shoot0Vel = 2300, shoot0Hood = 0.93; + public static double autoSpinStartPos = 0.2; + + public static double shoot0SpinSpeedIncrease = 0.03; + + public Action shootAll(int vel, double shootTime) { + 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 + shoot0SpinSpeedIncrease); + robot.spin2.setPosition(1 - prevSpinPos - shoot0SpinSpeedIncrease); + } + + return true; + + } else { + robot.transferServo.setPosition(transferServo_out); + //spindexPos = spindexer_intakePos1; + + + spindexer.resetSpindexer(); + spindexer.processIntake(); + + return false; + + } + + } + }; + } + 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; + + } + }; + } + + @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 targeting = new Targeting(); + Targeting.Settings targetingSettings = new Targeting.Settings(0.0, 0.0); + + spindexer = new Spindexer(hardwareMap); + + robot.limelight.pipelineSwitch(1); + + Turret turret = new Turret(robot, TELE, robot.limelight); + + turret.manualSetTurret(0.4); + + + drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(bx1, by1), bh1); + + robot.spin1.setPosition(autoSpinStartPos); + robot.spin2.setPosition(1-autoSpinStartPos); + + robot.light.setPosition(1); + + while (opModeInInit()) { + + robot.hood.setPosition(shoot0Hood); + + + + if (gamepad2.crossWasPressed()) { + redAlliance = !redAlliance; + } + + if (redAlliance) { + robot.light.setPosition(0.28); + x1 = rx1; + y1 = ry1; + h1 = rh1; + + } else { + robot.light.setPosition(0.6); + x1 = bx1; + y1 = by1; + h1 = bh1; + } + + shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0)) + .strafeToLinearHeading(new Vector2d(x1, y1), h1); + + } + + waitForStart(); + + if (isStopRequested()) return; + + if (opModeIsActive()) { + + Actions.runBlocking( + new ParallelAction( + shoot0.build(), + manageFlywheel( + shoot0Vel, + shoot0Hood, + 3.0, + x1, + 0.501, + 1, + 0.501 + ) + + ) + ); + + Actions.runBlocking( + shootAll(2300, 3.0) + ); + + + sleep(5000); + } + + } +} 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/ProtoAutoClose_V3.java index ccf2421..656e446 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V3.java @@ -203,6 +203,7 @@ public class ProtoAutoClose_V3 extends LinearOpMode { }; } + public Action Shoot(int vel) { return new Action() { int ticker = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java new file mode 100644 index 0000000..bbe123f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses_V2.java @@ -0,0 +1,43 @@ +package org.firstinspires.ftc.teamcode.constants; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; + +@Config +public class Poses_V2 { + + public static double goalHeight = 42; //in inches + + public static double turretHeight = 12; + + public static double relativeGoalHeight = goalHeight - turretHeight; + + public static Pose2d goalPose = new Pose2d(-10, 0, 0); + + public static double rx1 = 20, ry1 = 0, rh1 = 0; + public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); + public static double rx2b = 23, ry2b = 36, rh2b = Math.toRadians(140); + + public static double rx2c = 34, ry2c = 50, rh2c = Math.toRadians(140); + + public static double rx3a = 55, ry3a = 39, rh3a = Math.toRadians(140); + public static double rx3b = 33, ry3b = 61, rh3b = Math.toRadians(140); + + public static double rx4a = 72, ry4a = 55, rh4a = Math.toRadians(140); + public static double rx4b = 48, ry4b = 79, rh4b = Math.toRadians(140); + + public static double bx1 = 20, by1 = 0, bh1 = 0; + public static double bx2a = 45, by2a = -18, bh2a = Math.toRadians(-140); + public static double bx2b = 25, by2b = -38, bh2b = Math.toRadians(-140); + public static double bx2c = 34, by2c = -50, bh2c = Math.toRadians(-140); + + public static double bx3a = 55, by3a = -43, bh3a = Math.toRadians(-140); + public static double bx3b = 37, by3b = -61, bh3b = Math.toRadians(-140); + + public static double bx4a = 72, by4a = -55, bh4a = Math.toRadians(-140); + public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140); + public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this + + public static Pose2d teleStart = new Pose2d(0, 0, 0); + +} 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 bbe7192..7b07256 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.utils; import com.acmerobotics.dashboard.config.Config; +import com.arcrobotics.ftclib.hardware.ServoEx; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 770fdae..f9030be 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -26,10 +26,10 @@ dependencies { implementation 'com.bylazar:fullpanels:1.0.2' //Panels - implementation "com.acmerobotics.roadrunner:ftc:0.1.25" //RR - implementation "com.acmerobotics.roadrunner:core:1.0.1" //RR - implementation "com.acmerobotics.roadrunner:actions:1.0.1" //RR - implementation "com.acmerobotics.dashboard:dashboard:0.4.17" //FTC Dash + implementation "com.acmerobotics.roadrunner:ftc:0.1.25" + implementation "com.acmerobotics.roadrunner:core:1.0.1" + implementation "com.acmerobotics.roadrunner:actions:1.0.1" + implementation "com.acmerobotics.dashboard:dashboard:0.5.1" implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB