From 458b6f53a26a83646f2e09f1be3a108d720f7444 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sat, 11 Oct 2025 22:03:40 -0500 Subject: [PATCH] even more updates --- .../firstinspires/ftc/teamcode/constants/blank.java | 4 ---- .../ftc/teamcode/libs/RR/MecanumDrive.java | 6 ++++-- .../ftc/teamcode/libs/RR/OTOSLocalizer.java | 2 +- .../ftc/teamcode/libs/RR/PinpointLocalizer.java | 2 +- .../ftc/teamcode/libs/RR/TankDrive.java | 4 ++-- .../teamcode/libs/RR/ThreeDeadWheelLocalizer.java | 2 +- .../ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java | 2 +- .../ftc/teamcode/tests/LimelightTest.java | 12 +++++++++++- .../org/firstinspires/ftc/teamcode/utils/Robot.java | 10 ++++++++++ .../teamcode/utils/functions/DistanceToTarget.java | 4 ++++ .../ftc/teamcode/utils/subsystems/Limelight.java | 7 +++++++ .../Vars.java => variables/HardwareConfig.java} | 6 ++++-- 12 files changed, 46 insertions(+), 15 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{utils/Vars.java => variables/HardwareConfig.java} (56%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java deleted file mode 100644 index 53e7fcb..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - -public class blank { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java index 6cc00c9..82d05a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.libs.RR; + import androidx.annotation.NonNull; import com.acmerobotics.dashboard.canvas.Canvas; @@ -211,6 +212,7 @@ public final class MecanumDrive { headingDelta )); + return twist.velocity().value(); } } @@ -222,7 +224,7 @@ public final class MecanumDrive { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } - // TODO: make sure your config has motors with these names (or change them) + // TODO: make sure your HardwareConfig has motors with these names (or change them) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); leftBack = hardwareMap.get(DcMotorEx.class, "leftBack"); @@ -237,7 +239,7 @@ public final class MecanumDrive { // TODO: reverse motor directions if needed // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - // TODO: make sure your config has an IMU with this name (can be BNO or BHI) + // TODO: make sure your HardwareConfig has an IMU with this name (can be BNO or BHI) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot( PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java index 3345787..ef5b865 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/OTOSLocalizer.java @@ -28,7 +28,7 @@ public class OTOSLocalizer implements Localizer { private Pose2d currentPose; public OTOSLocalizer(HardwareMap hardwareMap, Pose2d initialPose) { - // TODO: make sure your config has an OTOS device with this name + // TODO: make sure your HardwareConfig has an OTOS device with this name // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); currentPose = initialPose; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java index 5d16196..9959566 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/PinpointLocalizer.java @@ -29,7 +29,7 @@ public final class PinpointLocalizer implements Localizer { private Pose2d txPinpointRobot = new Pose2d(0, 0, 0); public PinpointLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) { - // TODO: make sure your config has a Pinpoint device with this name + // TODO: make sure your HardwareConfig has a Pinpoint device with this name // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html driver = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java index c94dea2..a53462b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TankDrive.java @@ -232,7 +232,7 @@ public final class TankDrive { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } - // TODO: make sure your config has motors with these names (or change them) + // TODO: make sure your HardwareConfig has motors with these names (or change them) // add additional motors on each side if you have them // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html leftMotors = Arrays.asList(hardwareMap.get(DcMotorEx.class, "left")); @@ -248,7 +248,7 @@ public final class TankDrive { // TODO: reverse motor directions if needed // leftMotors.get(0).setDirection(DcMotorSimple.Direction.REVERSE); - // TODO: make sure your config has an IMU with this name (can be BNO or BHI) + // TODO: make sure your HardwareConfig has an IMU with this name (can be BNO or BHI) // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html lazyImu = new LazyHardwareMapImu(hardwareMap, "imu", new RevHubOrientationOnRobot( PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java index b3a8220..6ba0d53 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/ThreeDeadWheelLocalizer.java @@ -37,7 +37,7 @@ public final class ThreeDeadWheelLocalizer implements Localizer { private Pose2d pose; public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick, Pose2d initialPose) { - // TODO: make sure your config has **motors** with these names (or change them) + // TODO: make sure your HardwareConfig has **motors** with these names (or change them) // the encoders should be plugged into the slot matching the named motor // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html par0 = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par0"))); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java index d634311..65fabe8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/TwoDeadWheelLocalizer.java @@ -46,7 +46,7 @@ public final class TwoDeadWheelLocalizer implements Localizer { private Pose2d pose; public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick, Pose2d initialPose) { - // TODO: make sure your config has **motors** with these names (or change them) + // TODO: make sure your HardwareConfig has **motors** with these names (or change them) // the encoders should be plugged into the slot matching the named motor // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par"))); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index 26e1cb1..9973b8d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -6,10 +6,10 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.variables.HardwareConfig; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.subsystems.Limelight; - @Autonomous @Config @@ -21,12 +21,21 @@ public class LimelightTest extends LinearOpMode { MultipleTelemetry TELE; + + + + + Robot robot; @Override public void runOpMode() throws InterruptedException { + + HardwareConfig.USING_LL= true; + + robot = new Robot(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); @@ -51,6 +60,7 @@ public class LimelightTest extends LinearOpMode { 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 d42c344..ad1f8b4 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,7 +1,10 @@ package org.firstinspires.ftc.teamcode.utils; +import static org.firstinspires.ftc.teamcode.variables.HardwareConfig.*; + import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; public class Robot { @@ -15,5 +18,12 @@ public class Robot { limelight3A = hardwareMap.get(Limelight3A.class, "limelight"); + if (USING_LL) { + limelight3A = hardwareMap.get(Limelight3A.class, "limelight"); + limelight3A.start(); // This tells Limelight to start looking! + } + + + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java index 326ca4a..17b8c33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java @@ -1,4 +1,8 @@ package org.firstinspires.ftc.teamcode.utils.functions; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + public class DistanceToTarget { + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java index 3bc3fa3..1704e2b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java @@ -11,6 +11,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.robotcore.external.navigation.Position; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.variables.HardwareConfig; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Subsystem; @@ -49,6 +50,9 @@ public class Limelight implements Subsystem { private List colorResults = new ArrayList<>(); public Limelight(Robot robot, MultipleTelemetry tele) { + + HardwareConfig.USING_LL= true; + this.limelight = robot.limelight3A; this.telemetry = tele; limelight.pipelineSwitch(1); @@ -65,6 +69,9 @@ public class Limelight implements Subsystem { /** ✅ MAIN UPDATE LOOP */ @Override public void update() { + + limelight.updateRobotOrientation(); + result = limelight.getLatestResult(); status = limelight.getStatus(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Vars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/variables/HardwareConfig.java similarity index 56% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Vars.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/variables/HardwareConfig.java index e28deec..1a959b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Vars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/variables/HardwareConfig.java @@ -1,10 +1,12 @@ -package org.firstinspires.ftc.teamcode.utils; +package org.firstinspires.ftc.teamcode.variables; import com.acmerobotics.dashboard.config.Config; @Config -public class Vars { +public class HardwareConfig { public static boolean USING_LL = false; + + }