From 836be403008350044cf1ab001b7973fe79cf874f Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Fri, 27 Feb 2026 15:24:51 -0800 Subject: [PATCH] a --- .../frc/robot/commands/gpm/Superstructure.java | 12 +++++++----- .../frc/robot/constants/FieldConstants.java | 4 ++-- .../frc/robot/constants/ShotInterpolation.java | 17 ++++++++++++++--- .../frc/robot/constants/VisionConstants.java | 8 ++++---- .../controls/PS5ControllerDriverConfig.java | 10 +++++----- .../frc/robot/subsystems/Intake/Intake.java | 17 +++++++++++++---- .../frc/robot/subsystems/turret/Turret.java | 4 +++- 7 files changed, 48 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index d896b0d..72aa526 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -62,7 +62,8 @@ public class Superstructure extends Command { private final double phaseDelay = 0.03; // Extrapolation delay due to latency - private Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); + //TODO: Change this back + private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d(); private PhaseManager phaseManager = new PhaseManager(); @@ -122,9 +123,10 @@ public class Superstructure extends Command { goalState = ShooterPhysics.getShotParams( target3d.minus(lookahead3d), target == FieldConstants.getHubTranslation().toTranslation2d() ? - 2.0 : 3.0); + 2.0 : 5.0); - timeOfFlight = goalState.timeOfFlight(); + double TOFAdjustment = 0.75; + timeOfFlight = goalState.timeOfFlight() * TOFAdjustment; double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; lookaheadPose = @@ -208,7 +210,7 @@ public class Superstructure extends Command { SmartDashboard.putNumber("Hood Offset", hoodOffset); // Phase manager stuff phaseManager.update(drivepose, shooter, turret); - //target = phaseManager.getTarget(); + target = phaseManager.getTarget(); updateDrivePose(); updateSetpoints(drivepose); @@ -221,7 +223,7 @@ public class Superstructure extends Command { if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){ hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE - hoodOffset), 0.0); } else{ - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(hoodSetpoint), hoodVelocity); + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity); } shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index a9f60e7..1e68d07 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -134,13 +134,13 @@ public class FieldConstants { || x > FIELD_LENGTH/2 + Units.inchesToMeters(120.0) && x < RED_ALLIANCE_LINE){ return FieldZone.TRENCH_BUMP; } - if(x < FieldConstants.RED_ALLIANCE_LINE) { // inside red + if(x > FieldConstants.RED_ALLIANCE_LINE) { // inside red if (Robot.getAlliance() == Alliance.Red) { return FieldZone.ALLIANCE; } else { return FieldZone.OPPOSITION; } - } else if (x > FieldConstants.BLUE_ALLIANCE_LINE) { + } else if (x < FieldConstants.BLUE_ALLIANCE_LINE) { if (Robot.getAlliance() == Alliance.Blue) { return FieldZone.ALLIANCE; } else { diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java index 194b48b..da32329 100644 --- a/src/main/java/frc/robot/constants/ShotInterpolation.java +++ b/src/main/java/frc/robot/constants/ShotInterpolation.java @@ -2,6 +2,7 @@ package frc.robot.constants; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.hood.HoodConstants; public class ShotInterpolation { public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); @@ -17,12 +18,22 @@ public class ShotInterpolation { shooterPowerMap.put(0.0, 1.0); shooterPowerMap.put(1.0, 1.0); - hoodAngleMap.put(0.0, Units.degreesToRadians(90)); - hoodAngleMap.put(1.0, Units.degreesToRadians(90)); + //hoodAngleMap.put(HoodConstants.MAX_ANGLE, HoodConstants.MAX_ANGLE); + hoodAngleMap.put(81.3, 70.25); + hoodAngleMap.put(79.0, 65.9); + hoodAngleMap.put(58.5, 48.5); + //hoodAngleMap.put(1.0, Units.degreesToRadians(90)); - //TODO: find actual values from video motion exitVelocityMap.put(0.0, 0.0); exitVelocityMap.put(1.0, 2.2); exitVelocityMap.put(2.0, 4.4); + exitVelocityMap.put(7.0, 12.0); + exitVelocityMap.put(7.8, 15.2); + exitVelocityMap.put(7.78, 16.8); + exitVelocityMap.put(7.9, 17.1); + exitVelocityMap.put(8.0, 17.9); + exitVelocityMap.put(8.08, 19.0); + exitVelocityMap.put(25.0, 25.0* 3.2); + //exitVelocityMap.put(null, null); } } diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 854aaf9..6a48023 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -155,8 +155,8 @@ public class VisionConstants { new Pair( "CameraFrontLeft", new Transform3d( - new Translation3d(Units.inchesToMeters(8.47), - Units.inchesToMeters(-11.54), + 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)))), @@ -171,8 +171,8 @@ public class VisionConstants { new Pair( "CameraBackLeft", new Transform3d( - new Translation3d(Units.inchesToMeters(10.91), - Units.inchesToMeters(-12), + 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/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 64825eb..6041c96 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -176,11 +176,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { // Hood if (hood != null) { // Calibration - controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { - hood.calibrate(); - })).onFalse(new InstantCommand(() -> { - hood.stopCalibrating(); - })); + // controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { + // hood.calibrate(); + // })).onFalse(new InstantCommand(() -> { + // hood.stopCalibrating(); + // })); // Set the hood down -- for safety measures under trench controller.get(DPad.RIGHT).whileTrue(new InstantCommand(()->{ diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 1a7e5e3..ca0a840 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -183,12 +183,12 @@ public class Intake extends SubsystemBase implements IntakeIO{ robotExtension.setLength(inchExtension); if(calibrating){ - leftMotor.set(0.1); + leftMotor.set(-0.1); rightMotor.set(-0.1); boolean atHardStop = Math.abs((leftMotor.getStatorCurrent().getValueAsDouble() + rightMotor.getStatorCurrent().getValueAsDouble()) / 2) >= IntakeConstants.CALIBRATING_CURRENT_THRESHOLD; - if(calibrationDebouncer.calculate(atHardStop)){ - stopCalibrating(); - } + // if(calibrationDebouncer.calculate(atHardStop)){ + // stopCalibrating(); + // } } updateInputs(); @@ -230,6 +230,15 @@ public class Intake extends SubsystemBase implements IntakeIO{ * @param setpoint in inches */ public void setPosition(double setpoint) { + leftMotor.getConfigurator().apply( + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast) + ); + + rightMotor.getConfigurator().apply( + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast) + ); + double motorRotations = inchesToRotations(setpoint); rightMotor.setControl(voltageRequest.withPosition(motorRotations)); leftMotor.setControl(voltageRequest.withPosition(motorRotations)); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 1dbf65f..e589622 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -122,7 +122,9 @@ public class Turret extends SubsystemBase implements TurretIO{ //Sets the initial motor position motor.setPosition(motorRotations); - motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO); + //motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO); + + 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);})); -- 2.39.5