From 67db1c3d3cc1e4bd410036c6f88cd79e68ad72e7 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Thu, 26 Feb 2026 18:53:41 -0800 Subject: [PATCH] Code go brrr --- .../drive_comm/DefaultDriveCommand.java | 6 ++--- .../frc/robot/constants/VisionConstants.java | 24 +++++++++---------- .../constants/swerve/DriveConstants.java | 2 +- .../frc/robot/subsystems/turret/Turret.java | 2 ++ 4 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index d9af074..119fa14 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -79,10 +79,10 @@ public class DefaultDriveCommand extends Command { double yawDegrees = swerve.getYaw().getDegrees(); double snappedDeg; - if (swerve.getPose().getX() < FieldConstants.FIELD_LENGTH / 2) { - snappedDeg = (swerve.getYaw().getDegrees() > 135 || swerve.getYaw().getDegrees() < 225) ? 180 : 0; + if (swerve.getPose().getX() < swerve.getDesiredPose().getX()) { + snappedDeg = (yawDegrees > 135 || yawDegrees < 225) ? 180 : 0; } else { - snappedDeg = (swerve.getYaw().getDegrees() < 45 || swerve.getYaw().getDegrees() > 315) ? 0 : 180; + snappedDeg = (yawDegrees < 45 || yawDegrees > 315) ? 0 : 180; } swerve.setAlignAngle(Units.degreesToRadians(snappedDeg)); diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index ef784cc..854aaf9 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -155,33 +155,33 @@ public class VisionConstants { new Pair( "CameraFrontLeft", new Transform3d( - new Translation3d(Units.inchesToMeters(10.965663), - Units.inchesToMeters(-8.602606), - Units.inchesToMeters(19.337757)), + new Translation3d(Units.inchesToMeters(8.47), + Units.inchesToMeters(-11.54), + Units.inchesToMeters(19.7)), new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(55.0)))), new Pair( "CameraFrontRight", new Transform3d( - new Translation3d(Units.inchesToMeters(-10.965663), - Units.inchesToMeters(-8.602606), - Units.inchesToMeters(19.337757)), + new Translation3d(Units.inchesToMeters(-8.47), + Units.inchesToMeters(-11.54), + Units.inchesToMeters(19.7)), new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-55.0)))), new Pair( "CameraBackLeft", new Transform3d( - new Translation3d(Units.inchesToMeters(10.518135), - Units.inchesToMeters(-11.140663), - Units.inchesToMeters(19.337757)), + new Translation3d(Units.inchesToMeters(10.91), + Units.inchesToMeters(-12), + Units.inchesToMeters(19.66)), new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(145.0)))), new Pair( "CameraBackRight", new Transform3d( - new Translation3d(Units.inchesToMeters(-10.518135), - Units.inchesToMeters(-11.140663), - Units.inchesToMeters(19.337757)), + new Translation3d(Units.inchesToMeters(-10.91), + Units.inchesToMeters(-12), + Units.inchesToMeters(19.66)), new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-145.0)))) )); diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java index c9f6591..5cb0da5 100644 --- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java @@ -214,7 +214,7 @@ public class DriveConstants { // Gear ratios //DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); //R2 Ratio - DRIVE_GEAR_RATIO = (54.0 / 14.0) * (25.0 / 32.0) * (30.0 / 15.0); //R1 Ratio + DRIVE_GEAR_RATIO = (54.0 / 12.0) * (25.0 / 32.0) * (30.0 / 15.0); //R1 Ratio STEER_GEAR_RATIO = 287.0 / 11.0; // Gyro is mounted under the robot diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 057224c..d48f879 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -122,6 +122,8 @@ public class Turret extends SubsystemBase implements TurretIO{ //Sets the initial motor position motor.setPosition(motorRotations); + motor.setPosition(0.0); + SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);})); SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);})); SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);})); -- 2.39.5