diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index c73c683..4761dd4 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -155,8 +155,8 @@ public final class IntakePivot { new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withSupplyCurrentLimit(120) - .withStatorCurrentLimit(120) + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60) .withSupplyCurrentLimitEnable(true) .withStatorCurrentLimitEnable(true)) .withMotorOutput( @@ -168,8 +168,8 @@ public final class IntakePivot { new SoftwareLimitSwitchConfigs() .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS) - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true); + .withForwardSoftLimitEnable(false) + .withReverseSoftLimitEnable(false); public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() @@ -220,7 +220,7 @@ public final class GeneralShooterHood { public static final double CANCODER_TO_MECHANISM = CANCODER_TO_SPUR * SPUR_TO_MECHANISM; public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_SPUR * SPUR_TO_MECHANISM; - public static final double EPSILON_RADS = Units.degreesToRadians(2.0); + public static final double EPSILON_RADS = Units.degreesToRadians(0.5); public static final double STATE_TIMEOUT = 5.0; public static final boolean ENABLE_TIMEOUT = false; @@ -421,14 +421,14 @@ public final class ShooterHoodRight { } public final class FlywheelLeaderRight { - public static final int CAN_ID = 32; + public static final int CAN_ID = 33; public static final int BEAM_BREAK_PORT = 0; // TODO: update later public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG; } public final class FlywheelFollowerRight { - public static final int CAN_ID = 33; + public static final int CAN_ID = 32; public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG diff --git a/src/main/java/org/team5924/frc2026/Robot.java b/src/main/java/org/team5924/frc2026/Robot.java index d5875b0..d0fe751 100644 --- a/src/main/java/org/team5924/frc2026/Robot.java +++ b/src/main/java/org/team5924/frc2026/Robot.java @@ -37,6 +37,7 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.util.Elastic; +import org.team5924.frc2026.util.LaunchCalculator; public class Robot extends LoggedRobot { private static final double LOW_BATTERY_VOLTAGE = 11.0; @@ -147,11 +148,6 @@ public void robotPeriodic() { // Return to non-RT thread priority (do not modify the first argument) // Threads.setCurrentThreadPriority(false, 10); - // Low battery alert - if (DriverStation.isEnabled()) { - disabledTimer.reset(); - lowBatteryAlert.set(false); - } double batteryVoltage = RobotController.getBatteryVoltage(); if (batteryVoltage > 0.0 && batteryVoltage <= LOW_BATTERY_VOLTAGE @@ -203,12 +199,20 @@ public void teleopInit() { } updateMatchShift.startPeriodic(0.2); + // Low battery alert + if (DriverStation.isEnabled()) { + disabledTimer.reset(); + lowBatteryAlert.set(false); + } + Elastic.selectTab("Teleoperated"); } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + LaunchCalculator.getInstance().clearLaunchingParameters(); + } /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 1007604..d728874 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -31,32 +31,20 @@ import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.team5924.frc2026.commands.drive.DriveCommands; +import org.team5924.frc2026.commands.shooter.AutoScoreCommands; import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.subsystems.drive.Drive; import org.team5924.frc2026.subsystems.drive.GyroIO; +import org.team5924.frc2026.subsystems.drive.GyroIOPigeon2; import org.team5924.frc2026.subsystems.drive.GyroIOSim; import org.team5924.frc2026.subsystems.drive.ModuleIO; +import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFX; import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFXSim; -import org.team5924.frc2026.subsystems.vision.Vision; -import org.team5924.frc2026.subsystems.vision.VisionConstants; -import org.team5924.frc2026.subsystems.vision.VisionIO; -import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision; -import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState; -import org.team5924.frc2026.subsystems.rollers.hopper.HopperIO; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; -import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIO; -import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOTalonFX; -import org.team5924.frc2026.subsystems.rollers.intake.Intake; -import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; -import org.team5924.frc2026.subsystems.rollers.intake.IntakeIO; -import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOSim; import org.team5924.frc2026.subsystems.flywheel.Flywheel; import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.flywheel.FlywheelIO; import org.team5924.frc2026.subsystems.flywheel.FlywheelIOSim; +import org.team5924.frc2026.subsystems.flywheel.FlywheelIOTalonFX; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot.IntakePivotState; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIO; @@ -65,17 +53,32 @@ import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIO; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOSim; +import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.hopper.Hopper; +import org.team5924.frc2026.subsystems.rollers.hopper.HopperIO; import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOSim; +import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.indexer.Indexer; +import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; +import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIO; import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOSim; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.TurretIO; -import org.team5924.frc2026.subsystems.turret.TurretIOSim; +import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.intake.Intake; +import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIO; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOSim; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOTalonFX; +import org.team5924.frc2026.subsystems.vision.Vision; +import org.team5924.frc2026.subsystems.vision.VisionConstants; +import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision; +import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim; +import org.team5924.frc2026.util.LaunchCalculator; public class RobotContainer { // Subsystems private final Drive drive; private SwerveDriveSimulation driveSimulation = null; - private Vision vision; + private final Vision vision; private final Intake intake; private final IntakePivot intakePivot; @@ -84,11 +87,9 @@ public class RobotContainer { private final ShooterHood shooterHoodRight; private final Flywheel flywheelRight; - private final Turret turretRight; private final ShooterHood shooterHoodLeft; private final Flywheel flywheelLeft; - private final Turret turretLeft; // Controller private final CommandXboxController driveController = new CommandXboxController(0); @@ -106,56 +107,68 @@ public RobotContainer() { // -------------------------- real -------------------------- drive = new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, + new GyroIOPigeon2(), + new ModuleIOTalonFX(TunerConstants.FrontLeft), + new ModuleIOTalonFX(TunerConstants.FrontRight), + new ModuleIOTalonFX(TunerConstants.BackLeft), + new ModuleIOTalonFX(TunerConstants.BackRight), (pose) -> {}); - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOPhotonVision( - VisionConstants.FRONT_LEFT_NAME, VisionConstants.FRONT_LEFT_TRANSFORM), - new VisionIOPhotonVision( - VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM)); - - // intake = new Intake(new IntakeIOTalonFX()); + intake = new Intake(new IntakeIOTalonFX()); intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); - // hopper = new Hopper(new HopperIOTalonFX()); - // indexer = new Indexer(new IndexerIOTalonFX()); + hopper = new Hopper(new HopperIOTalonFX()); + indexer = new Indexer(new IndexerIOTalonFX()); - // shooterHoodLeft = new ShooterHood(new ShooterHoodIOTalonFX(true), true); - // flywheelLeft = new Flywheel(new FlywheelIOTalonFX(true), true); - // turretLeft = new Turret(new TurretIOTalonFX(true), true); + shooterHoodLeft = new ShooterHood(new ShooterHoodIOTalonFX(true), true); + flywheelLeft = new Flywheel(new FlywheelIOTalonFX(true), true); // shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); // flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); - // turretRight = new Turret(new TurretIOTalonFX(false), false); + + // -------------------------- sim drive -------------------------- + // driveSimulation = + // new SwerveDriveSimulation(Drive.mapleSimConfig, new Pose2d(3, 3, new Rotation2d())); + // SimulatedArena.getInstance().addDriveTrainSimulation(driveSimulation); + // drive = + // new Drive( + // new GyroIOSim(driveSimulation.getGyroSimulation()), + // new ModuleIOTalonFXSim(TunerConstants.FrontLeft, + // driveSimulation.getModules()[0]), + // new ModuleIOTalonFXSim(TunerConstants.FrontRight, + // driveSimulation.getModules()[1]), + // new ModuleIOTalonFXSim(TunerConstants.BackLeft, driveSimulation.getModules()[2]), + // new ModuleIOTalonFXSim(TunerConstants.BackRight, + // driveSimulation.getModules()[3]), + // driveSimulation::setSimulationWorldPose); // ---------------------------- IO ---------------------------- // drive = // new Drive( - // new GyroIOPigeon2(), - // new ModuleIOTalonFX(TunerConstants.FrontLeft), - // new ModuleIOTalonFX(TunerConstants.FrontRight), - // new ModuleIOTalonFX(TunerConstants.BackLeft), - // new ModuleIOTalonFX(TunerConstants.BackRight), + // new GyroIO() {}, + // new ModuleIO() {}, + // new ModuleIO() {}, + // new ModuleIO() {}, + // new ModuleIO() {}, // (pose) -> {}); - intake = new Intake(new IntakeIO() {}); + // intake = new Intake(new IntakeIO() {}); // intakePivot = new IntakePivot(new IntakePivotIO() {}); - hopper = new Hopper(new HopperIO() {}); - indexer = new Indexer(new IndexerIO() {}); + // hopper = new Hopper(new HopperIO() {}); + // indexer = new Indexer(new IndexerIO() {}); - shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); - flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); + // shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); + // flywheelLeft = new Flywheel(new FlywheelIO() {}, true); shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); + + vision = + new Vision( + drive::addVisionMeasurement, + new VisionIOPhotonVision( + VisionConstants.FRONT_LEFT_NAME, VisionConstants.FRONT_LEFT_TRANSFORM), + new VisionIOPhotonVision( + VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM)); break; case SIM: @@ -193,11 +206,9 @@ public RobotContainer() { shooterHoodLeft = new ShooterHood(new ShooterHoodIOSim(true), true); flywheelLeft = new Flywheel(new FlywheelIOSim(), true); - turretLeft = new Turret(new TurretIOSim(true), true); shooterHoodRight = new ShooterHood(new ShooterHoodIOSim(false), false); flywheelRight = new Flywheel(new FlywheelIOSim(), false); - turretRight = new Turret(new TurretIOSim(false), false); break; @@ -221,11 +232,9 @@ public RobotContainer() { shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); break; } @@ -308,101 +317,51 @@ private void configureButtonBindings() { () -> -driveController.getLeftX() * Constants.SLOW_MODE_MULTI, () -> -driveController.getRightX() * Constants.SLOW_MODE_MULTI)); - // [driver] 0-DEGREE MODE - driveController - .a() - .onTrue( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -driveController.getLeftY(), - () -> -driveController.getLeftX(), - () -> Rotation2d.kZero)); + // // [driver] 0-DEGREE MODE + // driveController + // .a() + // .onTrue( + // DriveCommands.joystickDriveAtAngle( + // drive, + // () -> -driveController.getLeftY(), + // () -> -driveController.getLeftX(), + // () -> Rotation2d.kZero)); - // [driver] Switch to X pattern when X button is pressed - driveController.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - - // [driver] Reset gyro to 0° when B button is pressed - driveController - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), - drive) - .ignoringDisable(true)); + // // [driver] Switch to X pattern when X button is pressed + // driveController.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); final Runnable resetGyro = Constants.currentMode == Constants.Mode.SIM ? () -> drive.setPose( driveSimulation - .getSimulatedDriveTrainPose()) // reset odometry to actual robot pose during - // simulation + .getSimulatedDriveTrainPose()) // reset odometry to actual robot pose + // during simulation : () -> drive.setPose( new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro driveController.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); - // )); + + // [driver] Reset gyro to 0° when B button is pressed + driveController.b().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true)); // ### hopper on by default hopper.setDefaultCommand( Commands.run(() -> hopper.setGoalState(Hopper.HopperState.ON), hopper)); - // flywheelLeft.setDefaultCommand( - // Commands.runOnce( - // () -> flywheelLeft.setGoalState(FlywheelState.OFF), - // flywheelLeft)); - - // flywheelRight.setDefaultCommand( - // Commands.runOnce( - // () -> flywheelRight.setGoalState(FlywheelState.OFF), - // flywheelRight)); - - // // ### intake pivot down/stow - - intakePivot.setDefaultCommand( - Commands.run( - () -> { - intakePivot.setGoalState(IntakePivotState.MANUAL); - intakePivot.setInput(operatorController.getLeftX()); - }, - intakePivot)); - - operatorController - .leftBumper() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.DOWN); - // intake.setGoalState(IntakeState.INTAKE); - }, - intakePivot /*, - intake*/)); - operatorController - .rightBumper() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.STOW); - // intake.setGoalState(IntakeState.OFF); - }, - intakePivot /*, - intake*/)); - - // // // ### intake pivot spit + // // ### intaking driveController - .leftTrigger() + .rightBumper() .onTrue( Commands.runOnce( () -> { intakePivot.setGoalState(IntakePivotState.DOWN); - intake.setGoalState(IntakeState.SPITOUT); + intake.setGoalState(IntakeState.INTAKE); }, intakePivot, intake)); driveController - .leftTrigger() + .rightBumper() .onFalse( Commands.runOnce( () -> { @@ -412,185 +371,103 @@ private void configureButtonBindings() { intakePivot, intake)); - // // ### indexing - operatorController - .a() - .onTrue( - Commands.runOnce( - () -> { - // intakePivot.setGoalState(IntakePivotState.SHOOTING); - indexer.setGoalState(Indexer.IndexerState.INDEXING); - }, - // intakePivot, - indexer)); - operatorController - .a() - .onFalse( - Commands.runOnce( - () -> { - // intakePivot.setGoalState(IntakePivotState.STOW); - indexer.setGoalState(Indexer.IndexerState.OFF); - }, - // intakePivot, - indexer)); - - // // ### shooting voltages - // operatorController - // .a() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B4); - // flywheelLeft.setGoalState(FlywheelState.B4); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .b() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B6); - // flywheelLeft.setGoalState(FlywheelState.B6); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .x() + // driveController + // .leftBumper() // .onTrue( // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B8); - // flywheelLeft.setGoalState(FlywheelState.B8); - // }, - // flywheelLeft, - // flywheelRight)); + // () -> { + // flywheelLeft.setGoalState(FlywheelState.FAST_LAUNCH); + // flywheelRight.setGoalState(FlywheelState.FAST_LAUNCH); + // }, + // flywheelLeft, + // flywheelRight) + // .andThen(Commands.waitSeconds(0.5)) + // .andThen( + // Commands.runOnce( + // () -> { + // indexer.setGoalState(IndexerState.INDEXING); + // }, + // indexer))); - // operatorController - // .y() + // driveController + // .leftTrigger() // .onTrue( // Commands.runOnce( // () -> { - // flywheelRight.setGoalState(FlywheelState.B12); - // flywheelLeft.setGoalState(FlywheelState.B12); + // LaunchCalculator.getInstance().getParameters(true); + // flywheelLeft.setGoalState(FlywheelState.MANUAL); + // shooterHoodLeft.setGoalState(ShooterHoodState.MANUAL_ANGLE); + // indexer.setGoalState(IndexerState.INDEXING); // }, + // shooterHoodLeft, // flywheelLeft, - // flywheelRight)); + // indexer)); - // operatorController - // .rightTrigger() - // .onTrue( + // driveController + // .leftTrigger() + // .onFalse( // Commands.runOnce( // () -> { - // flywheelRight.setGoalState(FlywheelState.OFF); // flywheelLeft.setGoalState(FlywheelState.OFF); + // shooterHoodLeft.setGoalState(ShooterHoodState.OFF); + // indexer.setGoalState(IndexerState.OFF); // }, + // shooterHoodLeft, // flywheelLeft, - // flywheelRight)); + // indexer)); // driveController - // .povUp() - // .onTrue( - // DriveCommands.driveFacingHub( - // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); - - // // AJGAOEIBAWIBAEROBIAEROBIJAROIBHAEBOIAERBIOHAERBHIOAERb - // // operatorController - // // .leftTrigger() - // // .onTrue( - // // AutoScoreCommands.runTrackTargetCommand( - // // turretLeft, shooterHoodLeft, flywheelLeft, true)); - // // operatorController - // // .leftTrigger() - // // .onTrue( - // // AutoScoreCommands.runTrackTargetCommand( - // // turretRight, shooterHoodRight, flywheelRight, false)); - - // // --------------------------------------------------------- - - // // turretLeft.setDefaultCommand( - // // Commands.run( - // // () -> { - // // turretLeft.setGoalState(TurretState.MANUAL); - // // turretLeft.setInput(driveController.getRightX()); - // // }, - // // turretLeft)); - - // driveController - // .leftTrigger() + // .leftBumper() // .onTrue( // Commands.runOnce( // () -> { - // turretLeft.setGoalState(TurretState.ZERO); + // flywheelLeft.setGoalState(FlywheelState.SLOW_LAUNCH); + // indexer.setGoalState(IndexerState.INDEXING); // }, - // turretLeft)); + // flywheelLeft, + // indexer)); // driveController - // .rightTrigger() - // .onTrue( + // .leftBumper() + // .onFalse( // Commands.runOnce( // () -> { - // turretLeft.setGoalState(TurretState.NINETY); + // flywheelLeft.setGoalState(FlywheelState.OFF); + // indexer.setGoalState(IndexerState.OFF); // }, - // turretLeft)); - - // // --------------------------------------------------------- - - // shooterHoodLeft.setDefaultCommand( - // Commands.run( - // () -> { - // shooterHoodLeft.setGoalState(ShooterHoodState.MANUAL); - // shooterHoodLeft.setInput(operatorController.getRightY()); - // }, - // shooterHoodLeft, - // shooterHoodRight)); - - operatorController - .leftTrigger() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.OFF); - }, - flywheelRight)); - - operatorController - .rightTrigger() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.FAST_LAUNCH); - }, - flywheelRight)); + // flywheelLeft, + // indexer)); - operatorController - .y() + driveController + .leftBumper() + .and(() -> LaunchCalculator.getInstance().getParameters().isValid()) .onTrue( + AutoScoreCommands.runTrackTargetCommand(shooterHoodLeft, flywheelLeft, true) + .alongWith( + Commands.runOnce( + () -> { + indexer.setGoalState(IndexerState.INDEXING); + }, + indexer))); + driveController + .leftBumper() + .and(() -> LaunchCalculator.getInstance().getParameters().isValid()) + .onFalse( Commands.runOnce( () -> { - flywheelRight.setGoalState(FlywheelState.SLOW_LAUNCH); + flywheelLeft.setGoalState(FlywheelState.OFF); + indexer.setGoalState(IndexerState.OFF); }, - flywheelRight)); + flywheelLeft, + indexer)); - operatorController - .povDown() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(-5.0); - }, - flywheelRight)); + // driveController + // .rightTrigger() + // .whileTrue( + // DriveCommands.joystickDriveWhileLaunching( + // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); - operatorController - .povUp() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(5.0); - }, - flywheelRight)); + // ------- // // driveController // // .rightTrigger() @@ -673,7 +550,6 @@ private void configureButtonBindings() { // // turretRight.setDefaultCommand(getAutonomousCommand()); // todo // // flywheelLeft.setDefaultCommand(getAutonomousCommand()); // todo // // flywheelRight.setDefaultCommand(getAutonomousCommand()); // todo - } /** diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 0ceb4d9..b5337a2 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -21,7 +21,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import lombok.Getter; import lombok.Setter; -import org.littletonrobotics.junction.AutoLogOutput; import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot.IntakePivotState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; @@ -44,7 +43,7 @@ public static RobotState getInstance() { /** Get the rotation of the estimated pose. */ public Rotation2d getRotation() { - return estimatedPose.getRotation(); + return odometryPose.getRotation(); } public ChassisSpeeds getFieldVelocity() { @@ -61,14 +60,11 @@ public ChassisSpeeds getFieldSetpointVelocity() { // @AutoLogOutput(key = "RobotState/OdometryPose") @Getter @Setter private Pose2d odometryPose = new Pose2d(); - @Getter @Setter @AutoLogOutput private Pose2d estimatedPose = Pose2d.kZero; - public void resetPose(Pose2d pose) { // Gyro offset is the rotation that maps the old gyro rotation (estimated - offset) to the new // frame of rotation gyroOffset = pose.getRotation().minus(odometryPose.getRotation().minus(gyroOffset)); odometryPose = pose; - estimatedPose = pose; } @Getter @Setter private Rotation2d yawPosition = new Rotation2d(); @@ -93,6 +89,7 @@ public void resetPose(Pose2d pose) { /*### Flywheel ### */ @Getter @Setter private FlywheelState leftFlywheelState = FlywheelState.OFF; @Getter @Setter private FlywheelState rightFlywheelState = FlywheelState.OFF; + @Getter @Setter private boolean leftFlywheelAtSetpoint = false; /*### Shooter Hood ### */ @Getter @Setter private ShooterHoodState leftShooterHoodState = ShooterHoodState.OFF; diff --git a/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java b/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java index 89481dd..275bd6e 100644 --- a/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java +++ b/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java @@ -42,13 +42,13 @@ public Command basicDriveAuto() { .resetPose( AllianceFlipUtil.apply( new Pose2d( - RobotState.getInstance().getEstimatedPose().getTranslation(), + RobotState.getInstance().getOdometryPose().getTranslation(), Rotation2d.kPi)))) .andThen( new DriveToPose( drive, - () -> RobotState.getInstance().getEstimatedPose(), - () -> RobotState.getInstance().getEstimatedPose(), + () -> RobotState.getInstance().getOdometryPose(), + () -> RobotState.getInstance().getOdometryPose(), () -> new Translation2d((AllianceFlipUtil.shouldFlip() ? -1.0 : 1.0) * -1.0, 0.0)) .withTimeout(0.6)); diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java index 41cb771..35a6a11 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveCommands.java @@ -16,6 +16,8 @@ package org.team5924.frc2026.commands.drive; +import static edu.wpi.first.units.Units.MetersPerSecond; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.SlewRateLimiter; @@ -37,9 +39,17 @@ import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2026.Constants; import org.team5924.frc2026.FieldConstants; +import org.team5924.frc2026.RobotState; +import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.subsystems.drive.Drive; import org.team5924.frc2026.util.AllianceFlipUtil; +import org.team5924.frc2026.util.GeomUtil; +import org.team5924.frc2026.util.LaunchCalculator; +import org.team5924.frc2026.util.LauncherConstants; +import org.team5924.frc2026.util.LoggedTunableNumber; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -52,6 +62,19 @@ public class DriveCommands { private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 + private static final LoggedTunableNumber driveLaunchKp = + new LoggedTunableNumber("DriveCommands/Launching/kP", 8.0); + private static final LoggedTunableNumber driveLaunchKd = + new LoggedTunableNumber("DriveCommands/Launching/kD", 0.5); + private static final LoggedTunableNumber driveYawLaunchToleranceDeg = + new LoggedTunableNumber("DriveCommands/Launching/YawToleranceDeg", 5.0); + private static final LoggedTunableNumber driveLaunchMaxPolarVelocityRadPerSec = + new LoggedTunableNumber("DriveCommands/Launching/MaxPolarVelocityRadPerSec", 0.6); + private static final LoggedTunableNumber driveLauncherCORMinErrorDeg = + new LoggedTunableNumber("DriveCommands/Launching/DriveLauncherCORMinErrorDeg", 15.0); + private static final LoggedTunableNumber driveLauncherCORMaxErrorDeg = + new LoggedTunableNumber("DriveCommands/Launching/DriveLauncherCORMaxErrorDeg", 30.0); + private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { @@ -174,6 +197,155 @@ public static Command driveFacingHub( .getAngle()); } + public static Command joystickDriveWhileLaunching( + Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier) { + // Create command + return Commands.run( + () -> { + // Run PID controller + final var parameters = LaunchCalculator.getInstance().getParameters(); + + if (!parameters.isValid()) return; + + double omegaOutput = + parameters.driveVelocity() + + (parameters + .driveAngle() + .minus(RobotState.getInstance().getRotation()) + .getRadians() + * driveLaunchKp.get()) + + ((parameters.driveVelocity() + - RobotState.getInstance().getRobotVelocity().omegaRadiansPerSecond) + * driveLaunchKd.get()); + + // Calculate speeds + Translation2d fieldRelativeLinearVelocity = + getLinearVelocityFromJoysticks(xSupplier.getAsDouble(), ySupplier.getAsDouble()) + .times(TunerConstants.kSpeedAt12Volts.in(MetersPerSecond)); + if (AllianceFlipUtil.shouldFlip()) { + fieldRelativeLinearVelocity = fieldRelativeLinearVelocity.times(-1.0); + } + + // Only limit if launching, not passing + if (!LaunchCalculator.getInstance().getParameters().passing()) { + // Calculate max linear velocity magnitude based on the max polar velocity + double maxLinearVelocityMagnitude = Double.POSITIVE_INFINITY; + double robotAngle = + Math.abs( + AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint.toTranslation2d()) + .minus(RobotState.getInstance().getOdometryPose().getTranslation()) + .getAngle() + .minus(fieldRelativeLinearVelocity.getAngle()) + .getRadians()); + double robotHubDistance = + LaunchCalculator.getInstance().getParameters().distanceNoLookahead(); + double hubAngle = + driveLaunchMaxPolarVelocityRadPerSec.get() + * LaunchCalculator.getInstance().getNaiveTOF(robotHubDistance); + double lookaheadAngle = Math.PI - robotAngle - hubAngle; + + // Calculate limit if triangle is valid (otherwise no limit) + if (lookaheadAngle > 0) { + double robotLookaheadDistance = + robotHubDistance * Math.sin(hubAngle) / Math.sin(lookaheadAngle); + maxLinearVelocityMagnitude = + robotLookaheadDistance + / LaunchCalculator.getInstance().getNaiveTOF(robotHubDistance); + } + + // Apply limit to velocity + if (fieldRelativeLinearVelocity.getNorm() > maxLinearVelocityMagnitude) { + fieldRelativeLinearVelocity = + fieldRelativeLinearVelocity.times( + maxLinearVelocityMagnitude / fieldRelativeLinearVelocity.getNorm()); + } + } + + // Apply chassis speeds + double corScalar = + MathUtil.clamp( + (Math.abs( + parameters + .driveAngle() + .minus(RobotState.getInstance().getRotation()) + .getDegrees()) + - driveLauncherCORMinErrorDeg.get()) + / (driveLauncherCORMaxErrorDeg.get() - driveLauncherCORMinErrorDeg.get()), + 0.0, + 1.0); + Translation2d launcherToRobot = + LauncherConstants.robotToLauncherCenter + .getTranslation() + .toTranslation2d() + .unaryMinus(); + ChassisSpeeds fieldRelativeSpeedsWithOffset = + GeomUtil.transformVelocity( + new ChassisSpeeds( + fieldRelativeLinearVelocity.getX(), + fieldRelativeLinearVelocity.getY(), + omegaOutput), + launcherToRobot.times(1.0 - corScalar), + RobotState.getInstance().getRotation()); + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeSpeedsWithOffset, RobotState.getInstance().getRotation())); + + // Override robot setpoint speeds published by drive. We run our calculations using the + // speeds that will ultimately be applied once we are using the full robot-to-launcher + // transform. This prevents the setpoint from changing due to the shifting COR of the + // robot. + ChassisSpeeds fieldRelativeSpeedsWithFullOffset = + GeomUtil.transformVelocity( + new ChassisSpeeds( + fieldRelativeLinearVelocity.getX(), + fieldRelativeLinearVelocity.getY(), + omegaOutput), + launcherToRobot, + RobotState.getInstance().getRotation()); + RobotState.getInstance() + .setRobotSetpointVelocity( + ChassisSpeeds.discretize( + ChassisSpeeds.fromFieldRelativeSpeeds( + fieldRelativeSpeedsWithFullOffset, + RobotState.getInstance().getRotation()), + Constants.LOOP_PERIODIC_SECONDS)); + + // Log data + Logger.recordOutput( + "DriveCommands/Launching/SetpointPose", + new Pose2d( + RobotState.getInstance().getOdometryPose().getTranslation(), + parameters.driveAngle())); + Logger.recordOutput("DriveCommands/Launching/AtGoalTolerance", atLaunchGoal()); + Logger.recordOutput( + "DriveCommands/Launching/ErrorPosition", + parameters.driveAngle().minus(RobotState.getInstance().getRotation())); + Logger.recordOutput( + "DriveCommands/Launching/ErrorVelocityRadPerSec", + parameters.driveVelocity() + - RobotState.getInstance().getRobotVelocity().omegaRadiansPerSecond); + Logger.recordOutput( + "DriveCommands/Launching/MeasuredPosition", RobotState.getInstance().getRotation()); + Logger.recordOutput( + "DriveCommands/Launching/MeasuredVelocityRadPerSec", + RobotState.getInstance().getRobotVelocity().omegaRadiansPerSecond); + Logger.recordOutput("DriveCommands/Launching/SetpointPosition", parameters.driveAngle()); + Logger.recordOutput( + "DriveCommands/Launching/SetpointVelocityRadPerSec", parameters.driveVelocity()); + }, + drive); + } + + public static boolean atLaunchGoal() { + return DriverStation.isEnabled() + && Math.abs( + RobotState.getInstance() + .getRotation() + .minus(LaunchCalculator.getInstance().getParameters().driveAngle()) + .getRadians()) + <= Units.degreesToRadians(driveYawLaunchToleranceDeg.get()); + } + /** * Measures the velocity feedforward constants for the drive motors. * diff --git a/src/main/java/org/team5924/frc2026/commands/drive/DriveToPose.java b/src/main/java/org/team5924/frc2026/commands/drive/DriveToPose.java index 0e956d8..09fe611 100644 --- a/src/main/java/org/team5924/frc2026/commands/drive/DriveToPose.java +++ b/src/main/java/org/team5924/frc2026/commands/drive/DriveToPose.java @@ -31,7 +31,7 @@ public class DriveToPose extends Command { private final Drive drive; private final Supplier target; - private Supplier robot = RobotState.getInstance()::getEstimatedPose; + private Supplier robot = RobotState.getInstance()::getOdometryPose; private Supplier linearFF = () -> Translation2d.kZero; private LoggedTunableNumber omegaFF = new LoggedTunableNumber("DriveToPose/omegaFF", 0.0); diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java index 78ae3c5..392e0ba 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java @@ -22,10 +22,7 @@ import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; import org.team5924.frc2026.util.LaunchCalculator; -import org.team5924.frc2026.util.LaunchCalculator.LaunchingParameters; public class AutoScoreCommands { // TODO Make and auto score program @@ -44,23 +41,26 @@ public class AutoScoreCommands { private AutoScoreCommands() {} public static Command runTrackTargetCommand( - Turret turret, ShooterHood shooterHood, Flywheel flywheel, boolean isLeft) { + ShooterHood shooterHood, Flywheel flywheel, boolean isLeft) { return Commands.run( - () -> { - LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(isLeft); + () -> { + // LaunchingParameters launchParams = + // LaunchCalculator.getInstance().getParameters(isLeft); - turret.setAutoInput(launchParams.turretRadians()); - shooterHood.setAutoInput(launchParams.hoodAngle()); - flywheel.setAutoInput(launchParams.flywheelSpeed()); + // shooterHood.setAutoInput(launchParams.hoodAngle()); + // flywheel.setAutoInput(launchParams.flywheelSpeed()); - turret.setGoalState(TurretState.AUTO); - shooterHood.setGoalState(ShooterHoodState.AUTO); - flywheel.setGoalState(FlywheelState.AUTO); + shooterHood.setGoalState(ShooterHoodState.AUTO); + flywheel.setGoalState(FlywheelState.AUTO); - LaunchCalculator.getInstance().clearLaunchingParameters(); - }, - turret, - shooterHood, - flywheel); + LaunchCalculator.getInstance().clearLaunchingParameters(); + }, + shooterHood, + flywheel) + .finallyDo( + () -> { + shooterHood.setGoalState(ShooterHoodState.OFF); + flywheel.setGoalState(FlywheelState.OFF); + }); } } diff --git a/src/main/java/org/team5924/frc2026/generated/TunerConstants.java b/src/main/java/org/team5924/frc2026/generated/TunerConstants.java index aad94c2..8aaeba9 100644 --- a/src/main/java/org/team5924/frc2026/generated/TunerConstants.java +++ b/src/main/java/org/team5924/frc2026/generated/TunerConstants.java @@ -118,7 +118,7 @@ public class TunerConstants { .withMountPose( new MountPoseConfigs() .withMountPosePitch(87.24925231933594) - .withMountPoseYaw(-0.7844216227531433) + .withMountPoseYaw(-0.7844216227531433 + 180.0) .withMountPoseRoll(-179.69129943847656)); // CAN bus that the devices are located on; diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index e595765..cfd1ffd 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -16,6 +16,7 @@ package org.team5924.frc2026.subsystems.flywheel; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -27,6 +28,7 @@ import org.team5924.frc2026.Constants; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; +import org.team5924.frc2026.util.LaunchCalculator; import org.team5924.frc2026.util.LoggedTunableNumber; public class Flywheel extends SubsystemBase { @@ -42,11 +44,11 @@ public class Flywheel extends SubsystemBase { public enum FlywheelState { OFF(() -> 0.0), MOVING(() -> 0.0), - FAST_LAUNCH(new LoggedTunableNumber("Flywheel/FastLaunch", 150)), + FAST_LAUNCH(new LoggedTunableNumber("Flywheel/FastLaunch", 100)), SLOW_LAUNCH(new LoggedTunableNumber("Flywheel/SlowLaunch", 50)), // current at which the example subsystem motor moves when controlled by the operator - MANUAL(new LoggedTunableNumber("Flywheel/OperatorCurrent", 200)), + MANUAL(new LoggedTunableNumber("Flywheel/OperatorCurrent", 135)), AUTO(() -> 0.0), SETPOINT(() -> 0.0), @@ -68,7 +70,11 @@ public enum FlywheelState { @Getter private FlywheelState goalState = FlywheelState.OFF; private boolean isAtSetpoint = false; - @Setter private double autoInput = 0.0; + private double autoInput = 0.0; + + public void setAutoInput(double inputRads) { + autoInput = MathUtil.clamp(inputRads, 0.0, 150.0); + } public Flywheel(FlywheelIO io, boolean isLeft) { side = isLeft ? "Left" : "Right"; @@ -118,9 +124,6 @@ public void stop() { public void setGoalState(FlywheelState goalState) { if (this.goalState.equals(goalState)) return; - if (goalState.equals(FlywheelState.MANUAL) && Math.abs(input) <= Constants.JOYSTICK_DEADZONE) - return; - this.goalState = goalState; switch (goalState) { case MANUAL -> setRespectiveFlywheelState(FlywheelState.MANUAL); @@ -149,6 +152,7 @@ private double getTargetVelocityRotationsPerSec() { private void handleCurrentState() { isAtSetpoint = isAtSetpoint(); + if (isLeft) RobotState.getInstance().setLeftFlywheelAtSetpoint(isAtSetpoint); switch (getRespectiveFlywheelState()) { case MOVING -> { @@ -159,7 +163,10 @@ private void handleCurrentState() { case MANUAL -> handleManualState(); case OFF -> stop(); case B4, B6, B8, B12 -> runVolts(getTargetVelocityRotationsPerSec()); - case AUTO -> setVelocity(autoInput); + case AUTO -> { + setAutoInput(LaunchCalculator.getInstance().getParameters(isLeft).flywheelSpeed()); + setVelocity(autoInput); + } default -> setVelocity(getTargetVelocityRotationsPerSec()); } } @@ -167,16 +174,16 @@ private void handleCurrentState() { private void handleManualState() { if (!goalState.equals(FlywheelState.MANUAL)) return; - if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { - stop(); - return; - } + // if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { + // stop(); + // return; + // } - setVelocity(FlywheelState.MANUAL.getVelocityRotationsPerSec().getAsDouble() * input); + setVelocity(FlywheelState.MANUAL.getVelocityRotationsPerSec().getAsDouble()); } public void updateSetpointState(double change) { - autoInput = inputs.setpointVelocityRotationsPerSec + change; + setAutoInput(inputs.setpointVelocityRotationsPerSec + change); setGoalState(FlywheelState.SETPOINT); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java index 66dda21..4da9666 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java @@ -59,16 +59,16 @@ public class IntakePivotIOTalonFX implements IntakePivotIO { /* Gains */ private final LoggedTunableNumber kP = new LoggedTunableNumber("IntakePivot/kP", 40.0); private final LoggedTunableNumber kI = new LoggedTunableNumber("IntakePivot/kI", 0.0); - private final LoggedTunableNumber kD = new LoggedTunableNumber("IntakePivot/kD", 0.5); - private final LoggedTunableNumber kS = new LoggedTunableNumber("IntakePivot/kS", .2); + private final LoggedTunableNumber kD = new LoggedTunableNumber("IntakePivot/kD", 0.0); + private final LoggedTunableNumber kS = new LoggedTunableNumber("IntakePivot/kS", 0.2); private final LoggedTunableNumber kV = new LoggedTunableNumber("IntakePivot/kV", 0.0); private final LoggedTunableNumber kG = new LoggedTunableNumber("IntakePivot/kG", 2.8); private final LoggedTunableNumber kA = new LoggedTunableNumber("IntakePivot/kA", 0.0); private final LoggedTunableNumber motionCruiseVelocity = - new LoggedTunableNumber("IntakePivot/MotionCruiseVelocity", 10.0); + new LoggedTunableNumber("IntakePivot/MotionCruiseVelocity", 100.0); private final LoggedTunableNumber motionAcceleration = - new LoggedTunableNumber("IntakePivot/MotionAcceleration", 100.0); + new LoggedTunableNumber("IntakePivot/MotionAcceleration", 1000.0); private final LoggedTunableNumber motionJerk = new LoggedTunableNumber("IntakePivot/MotionJerk", 0.0); diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java index 88011da..6853431 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java @@ -16,6 +16,7 @@ package org.team5924.frc2026.subsystems.pivots.shooterHood; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,9 +26,11 @@ import lombok.Setter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; +import org.team5924.frc2026.Constants.GeneralShooterHood; import org.team5924.frc2026.FieldState; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; +import org.team5924.frc2026.util.LaunchCalculator; import org.team5924.frc2026.util.LoggedTunableNumber; public class ShooterHood extends SubsystemBase { @@ -50,6 +53,8 @@ public enum ShooterHoodState { NEUTRAL_SHUFFLING(() -> 0.0), OPPONENT_SHUFFLING(() -> 0.0), + MANUAL_ANGLE(new LoggedTunableNumber("ShooterHood/ManualAngle", Math.toRadians(0.0))), + MAX(new LoggedTunableNumber("ShooterHood/Max", Math.toRadians(30))), CENTER(new LoggedTunableNumber("ShooterHood/Center", Math.toRadians(15))), AUTO(() -> 0.0), @@ -73,7 +78,13 @@ public enum ShooterHoodState { private double timeSinceLastStateChange = 0.0; @Setter private double input; - @Setter private double autoInput = 0.0; + private double autoInput = 0.0; + + public void setAutoInput(double inputRads) { + autoInput = + MathUtil.clamp( + inputRads, GeneralShooterHood.MIN_POSITION_RADS, GeneralShooterHood.MAX_POSITION_RADS); + } public ShooterHood(ShooterHoodIO io, boolean isLeft) { side = isLeft ? "Left" : "Right"; @@ -134,7 +145,7 @@ public void setGoalState(ShooterHoodState goalState) { DriverStation.reportError( side + " Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", null); - case AUTO -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); + case AUTO -> setRespectiveShooterHoodState(ShooterHoodState.AUTO); case OFF -> setRespectiveShooterHoodState(ShooterHoodState.OFF); default -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); } @@ -170,6 +181,10 @@ private void handleCurrentState() { } case MANUAL -> handleManualState(); case OFF -> stop(); + case AUTO -> { + setAutoInput(LaunchCalculator.getInstance().getParameters(isLeft).hoodAngle()); + if (!isAtSetpoint) setPosition(autoInput); + } default -> { if (!isAtSetpoint) setPosition(getTargetRads()); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java index 9227bfa..040be11 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java @@ -30,7 +30,7 @@ public class Hopper extends GenericRoller { @RequiredArgsConstructor @Getter public enum HopperState implements VoltageState { - ON(new LoggedTunableNumber("Hopper/OnVoltage", 4.0)), + ON(new LoggedTunableNumber("Hopper/OnVoltage", 6.0)), SPIT(new LoggedTunableNumber("Hopper/SpitVoltage", -4.0)), OFF(() -> 0.0); diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java index 6554f00..5b6aac9 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java @@ -30,7 +30,7 @@ public class Indexer extends GenericRoller { @Getter public enum IndexerState implements VoltageState { OFF(() -> 0.0), - INDEXING(new LoggedTunableNumber("Indexer/IndexingVoltage", 4.0)); + INDEXING(new LoggedTunableNumber("Indexer/IndexingVoltage", 8.0)); private final DoubleSupplier voltageSupplier; } @@ -46,6 +46,13 @@ public void setGoalState(IndexerState goalState) { RobotState.getInstance().setIndexerState(goalState); } + @Override + protected void handleCurrentState() { + if (RobotState.getInstance().isLeftFlywheelAtSetpoint()) + io.runVolts(getGoalState().getVoltageSupplier().getAsDouble()); + else io.stop(); + } + @Override public void periodic() { super.periodic(); diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java index c3385a3..570c738 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java @@ -35,17 +35,23 @@ public class VisionConstants { public static final Transform3d FRONT_LEFT_TRANSFORM = new Transform3d( new Translation3d( - Units.inchesToMeters(10.402), - Units.inchesToMeters(12.042), - Units.inchesToMeters(8.401)), - new Rotation3d(0.0, Units.degreesToRadians(55.0), Units.degreesToRadians(-15.0))); + Units.inchesToMeters(10.402), + Units.inchesToMeters(12.042), + Units.inchesToMeters(8.401)), + new Rotation3d( + Units.degreesToRadians(8.0), + Units.degreesToRadians(-55.0), + Units.degreesToRadians(-15.0))); public static final Transform3d FRONT_RIGHT_TRANSFORM = new Transform3d( new Translation3d( - Units.inchesToMeters(10.402), - Units.inchesToMeters(-12.042), - Units.inchesToMeters(8.711)), - new Rotation3d(0.0, Units.degreesToRadians(55.0), Units.degreesToRadians(15.0))); + Units.inchesToMeters(10.402), + Units.inchesToMeters(-12.042), + Units.inchesToMeters(8.711)), + new Rotation3d( + Units.degreesToRadians(-8.0), + Units.degreesToRadians(-55.0), + Units.degreesToRadians(15.0))); // public static final ArrayList IGNORE_IDS = // new ArrayList(List.of()); diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java index 82089a3..c2e350f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIO.java @@ -17,12 +17,14 @@ package org.team5924.frc2026.subsystems.vision; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; import org.littletonrobotics.junction.AutoLog; public interface VisionIO { @AutoLog public static class VisionIOInputs { public boolean connected = false; + public TranslationRotation cameraToTarget; public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; } @@ -36,6 +38,9 @@ public static record PoseObservation( double averageTagDistance, PoseObservationType type) {} + public static record TranslationRotation( + Translation3d translation, double roll, double pitch, double yaw) {} + public static enum PoseObservationType { MEGATAG_1, MEGATAG_2, diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java index 2b2e6f2..4460382 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionIOPhotonVision.java @@ -17,18 +17,31 @@ package org.team5924.frc2026.subsystems.vision; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; +import lombok.Setter; import org.photonvision.PhotonCamera; import org.team5924.frc2026.Constants; +import org.team5924.frc2026.util.LoggedTunableNumber; /** IO implementation for real PhotonVision hardware. */ public class VisionIOPhotonVision implements VisionIO { protected final PhotonCamera camera; - protected final Transform3d robotToCamera; + @Setter protected Transform3d robotToCamera; + + private final LoggedTunableNumber rollDegrees; + private final LoggedTunableNumber pitchDegrees; + private final LoggedTunableNumber yawDegrees; + + private final LoggedTunableNumber xInches; + private final LoggedTunableNumber yInches; + private final LoggedTunableNumber zInches; /** * Creates a new VisionIOPhotonVision. @@ -38,11 +51,65 @@ public class VisionIOPhotonVision implements VisionIO { */ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { camera = new PhotonCamera(name); - this.robotToCamera = robotToCamera; + // this.robotToCamera = robotToCamera; + + rollDegrees = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/rotation roll degrees", + Units.radiansToDegrees(robotToCamera.getRotation().getX())); + pitchDegrees = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/rotation pitch degrees", + Units.radiansToDegrees(robotToCamera.getRotation().getY())); + yawDegrees = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/rotation yaw degrees", + Units.radiansToDegrees(robotToCamera.getRotation().getZ())); + + xInches = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/position x inches", + Units.metersToInches(robotToCamera.getX())); + yInches = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/position y inches", + Units.metersToInches(robotToCamera.getY())); + zInches = + new LoggedTunableNumber( + "Vision/Camera" + camera.getName() + "/position z inches", + Units.metersToInches(robotToCamera.getZ())); + + updateRobotToCamera(); + } + + private void updateLoggedTunableNumbers() { + LoggedTunableNumber.ifChanged( + hashCode(), + this::updateRobotToCamera, + xInches, + yInches, + zInches, + rollDegrees, + pitchDegrees, + yawDegrees); + } + + private void updateRobotToCamera() { + robotToCamera = + new Transform3d( + new Translation3d( + Units.inchesToMeters(xInches.getAsDouble()), + Units.inchesToMeters(yInches.getAsDouble()), + Units.inchesToMeters(zInches.getAsDouble())), + new Rotation3d( + Units.degreesToRadians(rollDegrees.getAsDouble()), + Units.degreesToRadians(pitchDegrees.getAsDouble()), + Units.degreesToRadians(yawDegrees.getAsDouble()))); } @Override public void updateInputs(VisionIOInputs inputs) { + updateLoggedTunableNumbers(); inputs.connected = camera.isConnected(); // Read new camera observations @@ -58,6 +125,9 @@ public void updateInputs(VisionIOInputs inputs) { Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + // no single target, so log invalid cameraToTarget value + inputs.cameraToTarget = new TranslationRotation(Translation3d.kZero, -1, -1, -1); + // Calculate average tag distance double totalTagDistance = 0.0; for (var target : result.targets) { @@ -86,6 +156,15 @@ public void updateInputs(VisionIOInputs inputs) { Transform3d fieldToTarget = new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); Transform3d cameraToTarget = target.bestCameraToTarget; + + Rotation3d cameraToTargetRotation = cameraToTarget.getRotation(); + inputs.cameraToTarget = + new TranslationRotation( + cameraToTarget.getTranslation(), + Units.radiansToDegrees(cameraToTargetRotation.getX()), + Units.radiansToDegrees(cameraToTargetRotation.getY()), + Units.radiansToDegrees(cameraToTargetRotation.getZ())); + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index 91c2452..fe7cded 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -102,11 +102,11 @@ public record LaunchingParameters( public static final double trenchPresetDistance = 3.03; public static final double outpostPresetDistance = 4.84; public static final double passingPresetDistance = 7.0; - public static final LaunchPreset passingPreset; - public static final LaunchPreset hubPreset; - public static final LaunchPreset towerPreset; - public static final LaunchPreset trenchPreset; - public static final LaunchPreset outpostPreset; + // public static final LaunchPreset passingPreset; + // public static final LaunchPreset hubPreset; + // public static final LaunchPreset towerPreset; + // public static final LaunchPreset trenchPreset; + // public static final LaunchPreset outpostPreset; public static final LaunchPreset hoodMinPreset = new LaunchPreset( new LoggedTunableNumber( @@ -159,48 +159,82 @@ public static record LaunchPreset( phaseDelay = 0.03; // if (true) { - hoodAngleMap.put(0.96, Rotation2d.fromDegrees(10.0)); - hoodAngleMap.put(1.16, Rotation2d.fromDegrees(12.0)); - hoodAngleMap.put(1.58, Rotation2d.fromDegrees(14.0)); - hoodAngleMap.put(2.07, Rotation2d.fromDegrees(18.5)); - hoodAngleMap.put(2.37, Rotation2d.fromDegrees(22.0)); - hoodAngleMap.put(2.47, Rotation2d.fromDegrees(23.0)); - hoodAngleMap.put(2.70, Rotation2d.fromDegrees(24.0)); - hoodAngleMap.put(2.94, Rotation2d.fromDegrees(25.0)); - hoodAngleMap.put(3.48, Rotation2d.fromDegrees(27.0)); - hoodAngleMap.put(3.92, Rotation2d.fromDegrees(32.0)); - hoodAngleMap.put(4.35, Rotation2d.fromDegrees(34.0)); - hoodAngleMap.put(4.84, Rotation2d.fromDegrees(38.0)); - - flywheelSpeedMap.put(0.96, 150.0); - flywheelSpeedMap.put(1.16, 155.0); - flywheelSpeedMap.put(1.58, 160.0); - flywheelSpeedMap.put(2.07, 165.0); - flywheelSpeedMap.put(2.37, 170.0); - flywheelSpeedMap.put(2.47, 170.0); - flywheelSpeedMap.put(2.70, 170.0); - flywheelSpeedMap.put(2.94, 175.0); - flywheelSpeedMap.put(3.48, 175.0); - flywheelSpeedMap.put(3.92, 180.0); - flywheelSpeedMap.put(4.35, 185.0); - flywheelSpeedMap.put(4.84, 190.0); - - timeOfFlightMap.put(5.68, 1.16); - timeOfFlightMap.put(4.55, 1.12); - timeOfFlightMap.put(3.15, 1.11); - timeOfFlightMap.put(1.88, 1.09); - timeOfFlightMap.put(1.38, 0.90); - - passingHoodAngleMap.put(5.46, Rotation2d.fromDegrees(38.0)); - passingHoodAngleMap.put(6.62, Rotation2d.fromDegrees(38.0)); - passingHoodAngleMap.put(7.80, Rotation2d.fromDegrees(38.0)); - - passingFlywheelSpeedMap.put(5.46, 160.0); - passingFlywheelSpeedMap.put(6.62, 180.0); - passingFlywheelSpeedMap.put(7.80, 200.0); - - passingTimeOfFlightMap.put(passingMinDistance, 0.0); - passingTimeOfFlightMap.put(passingMaxDistance, 0.0); + hoodAngleMap.put(2.388, Rotation2d.fromRadians(0.0)); + hoodAngleMap.put(2.450, Rotation2d.fromRadians(0.031)); + hoodAngleMap.put(2.570, Rotation2d.fromRadians(0.040)); + hoodAngleMap.put(2.730, Rotation2d.fromRadians(0.052)); + hoodAngleMap.put(3.050, Rotation2d.fromRadians(0.052)); + hoodAngleMap.put(3.550, Rotation2d.fromRadians(0.110)); + hoodAngleMap.put(4.000, Rotation2d.fromRadians(0.139)); + hoodAngleMap.put(4.470, Rotation2d.fromRadians(0.181)); + + flywheelSpeedMap.put(2.388, 75.0); + flywheelSpeedMap.put(2.450, 75.0); + flywheelSpeedMap.put(2.570, 77.5); + flywheelSpeedMap.put(2.730, 77.5); + flywheelSpeedMap.put(3.050, 80.0); + flywheelSpeedMap.put(3.550, 85.0); + flywheelSpeedMap.put(4.000, 87.5); + flywheelSpeedMap.put(4.470, 92.5); + + timeOfFlightMap.put(2.388, 0.814); + timeOfFlightMap.put(3.050, 0.879); + timeOfFlightMap.put(4.000, 0.935); + timeOfFlightMap.put(4.470, 0.975); + + // ------------------------ + + // minDistance = 0.9; + // maxDistance = 4.9; + // passingMinDistance = 0.0; + // passingMaxDistance = 12.0; + // phaseDelay = 0.03; + + // // if (true) { + // hoodAngleMap.put(0.96, Rotation2d.fromDegrees(10.0 / 2.0)); + // hoodAngleMap.put(1.16, Rotation2d.fromDegrees(12.0 / 2.0)); + // hoodAngleMap.put(1.58, Rotation2d.fromDegrees(14.0 / 2.0)); + // hoodAngleMap.put(2.07, Rotation2d.fromDegrees(18.5 / 2.0)); + // hoodAngleMap.put(2.37, Rotation2d.fromDegrees(22.0 / 2.0)); + // hoodAngleMap.put(2.47, Rotation2d.fromDegrees(23.0 / 2.0)); + // hoodAngleMap.put(2.70, Rotation2d.fromDegrees(24.0 / 2.0)); + // hoodAngleMap.put(2.94, Rotation2d.fromDegrees(25.0 / 2.0)); + // hoodAngleMap.put(3.48, Rotation2d.fromDegrees(27.0 / 2.0)); + // hoodAngleMap.put(3.92, Rotation2d.fromDegrees(32.0 / 2.0)); + // hoodAngleMap.put(4.35, Rotation2d.fromDegrees(34.0 / 2.0)); + // hoodAngleMap.put(4.84, Rotation2d.fromDegrees(38.0 / 2.0)); + + // flywheelSpeedMap.put(0.96, 150.0); + // flywheelSpeedMap.put(1.16, 155.0); + // flywheelSpeedMap.put(1.58, 160.0); + // flywheelSpeedMap.put(2.07, 165.0); + // flywheelSpeedMap.put(2.37, 170.0); + // flywheelSpeedMap.put(2.47, 170.0); + // flywheelSpeedMap.put(2.70, 170.0); + // flywheelSpeedMap.put(2.94, 175.0); + // flywheelSpeedMap.put(3.48, 175.0); + // flywheelSpeedMap.put(3.92, 180.0); + // flywheelSpeedMap.put(4.35, 185.0); + // flywheelSpeedMap.put(4.84, 190.0); + + // timeOfFlightMap.put(5.68, 1.16); + // timeOfFlightMap.put(4.55, 1.12); + // timeOfFlightMap.put(3.15, 1.11); + // timeOfFlightMap.put(1.88, 1.09); + // timeOfFlightMap.put(1.38, 0.90); + + // { + // passingHoodAngleMap.put(5.46, Rotation2d.fromDegrees(38.0)); + // passingHoodAngleMap.put(6.62, Rotation2d.fromDegrees(38.0)); + // passingHoodAngleMap.put(7.80, Rotation2d.fromDegrees(38.0)); + + // passingFlywheelSpeedMap.put(5.46, 160.0); + // passingFlywheelSpeedMap.put(6.62, 180.0); + // passingFlywheelSpeedMap.put(7.80, 200.0); + + // passingTimeOfFlightMap.put(passingMinDistance, 0.0); + // passingTimeOfFlightMap.put(passingMaxDistance, 0.0); + // } // } else { // // Full field maps @@ -223,46 +257,46 @@ public static record LaunchPreset( // passingTimeOfFlightMap.put(passingMaxDistance, 0.0); // } - passingPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Passing/HoodAngle", - passingHoodAngleMap.get(passingPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Passing/FlywheelSpeed", - passingFlywheelSpeedMap.get(passingPresetDistance))); - hubPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Hub/HoodAngle", - hoodAngleMap.get(hubPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Hub/FlywheelSpeed", - flywheelSpeedMap.get(hubPresetDistance))); - towerPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Tower/HoodAngle", - hoodAngleMap.get(towerPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Tower/FlywheelSpeed", - flywheelSpeedMap.get(towerPresetDistance))); - trenchPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Trench/HoodAngle", - hoodAngleMap.get(trenchPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Trench/FlywheelSpeed", - flywheelSpeedMap.get(trenchPresetDistance))); - outpostPreset = - new LaunchPreset( - new LoggedTunableNumber( - "LaunchCalculator/Presets/Outpost/HoodAngle", - hoodAngleMap.get(outpostPresetDistance).getDegrees()), - new LoggedTunableNumber( - "LaunchCalculator/Presets/Outpost/FlywheelSpeed", - flywheelSpeedMap.get(outpostPresetDistance))); + // passingPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Passing/HoodAngle", + // passingHoodAngleMap.get(passingPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Passing/FlywheelSpeed", + // passingFlywheelSpeedMap.get(passingPresetDistance))); + // hubPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Hub/HoodAngle", + // hoodAngleMap.get(hubPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Hub/FlywheelSpeed", + // flywheelSpeedMap.get(hubPresetDistance))); + // towerPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Tower/HoodAngle", + // hoodAngleMap.get(towerPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Tower/FlywheelSpeed", + // flywheelSpeedMap.get(towerPresetDistance))); + // trenchPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Trench/HoodAngle", + // hoodAngleMap.get(trenchPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Trench/FlywheelSpeed", + // flywheelSpeedMap.get(trenchPresetDistance))); + // outpostPreset = + // new LaunchPreset( + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Outpost/HoodAngle", + // hoodAngleMap.get(outpostPresetDistance).getDegrees()), + // new LoggedTunableNumber( + // "LaunchCalculator/Presets/Outpost/FlywheelSpeed", + // flywheelSpeedMap.get(outpostPresetDistance))); } public static double getMinTimeOfFlight() { @@ -273,17 +307,30 @@ public static double getMaxTimeOfFlight() { return timeOfFlightMap.get(maxDistance); } + public LaunchingParameters getParameters() { + return getParameters(true); + } + public LaunchingParameters getParameters(boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; + return getParameters(isLeft ? robotToLauncherLeft : robotToLauncherRight); + } + + public LaunchingParameters getParameters(Transform3d robotToLauncher) { boolean passing = - AllianceFlipUtil.applyX(RobotState.getInstance().getEstimatedPose().getX()) + AllianceFlipUtil.applyX(RobotState.getInstance().getOdometryPose().getX()) > FieldConstants.LinesVertical.hubCenter; + + // TODO: passing not implemented yet + if (passing) + return new LaunchingParameters( + false, Rotation2d.kZero, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, true); + if (latestParameters != null) { return latestParameters; } // Calculate estimated pose while accounting for phase delay - Pose2d estimatedPose = RobotState.getInstance().getEstimatedPose(); + Pose2d estimatedPose = RobotState.getInstance().getOdometryPose(); ChassisSpeeds robotRelativeVelocity = RobotState.getInstance().getRobotVelocity(); estimatedPose = estimatedPose.exp( @@ -331,12 +378,13 @@ public LaunchingParameters getParameters(boolean isLeft) { lookaheadLauncherToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } - double turretRads = getTurretAngle(lookaheadPose, target, isLeft).getRadians(); + double turretRads = getTurretAngle(lookaheadPose, target).getRadians(); // Account for launcher being off center Pose2d lookaheadRobotPose = lookaheadPose.transformBy(robotToLauncher.toTransform2d().inverse()); - Rotation2d driveAngle = getDriveAngleWithLauncherOffset(lookaheadRobotPose, target, isLeft); + Rotation2d driveAngle = + getDriveAngleWithLauncherOffset(lookaheadRobotPose, target, robotToLauncher); // Calculate remaining parameters double hoodAngle = @@ -389,11 +437,12 @@ public LaunchingParameters getParameters(boolean isLeft) { Logger.recordOutput( "LaunchCalculator/LauncherToTargetDistance", lookaheadLauncherToTargetDistance); + Logger.recordOutput("LaunchCalculator/LaunchParams", latestParameters); + return latestParameters; } - private static Rotation2d getTurretAngle( - Pose2d turretPose, Translation2d target, boolean isLeft) { + private static Rotation2d getTurretAngle(Pose2d turretPose, Translation2d target) { Rotation2d turretToHub = target.minus(turretPose.getTranslation()).getAngle(); Rotation2d turretRads = turretPose.getRotation().minus(turretToHub); @@ -401,8 +450,7 @@ private static Rotation2d getTurretAngle( } private static Rotation2d getDriveAngleWithLauncherOffset( - Pose2d robotPose, Translation2d target, boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; + Pose2d robotPose, Translation2d target, Transform3d robotToLauncher) { Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); Rotation2d hubAngle = @@ -427,7 +475,7 @@ public void clearLaunchingParameters() { } public Translation2d getPassingTarget() { - double flippedY = AllianceFlipUtil.apply(RobotState.getInstance().getEstimatedPose()).getY(); + double flippedY = AllianceFlipUtil.apply(RobotState.getInstance().getOdometryPose()).getY(); boolean mirror = flippedY > FieldConstants.LinesHorizontal.center; // Check if we need to interpolate @@ -471,7 +519,10 @@ public static Pose2d getStationaryAimedPose( return new Pose2d( robotTranslation, - getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target, isLeft)); + getDriveAngleWithLauncherOffset( + robotTranslation.toPose2d(), + target, + isLeft ? robotToLauncherLeft : robotToLauncherRight)); } /** Adjusts the hood angle offset up or down the specified amount. */ diff --git a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java index 4879bf4..ee61b32 100644 --- a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java +++ b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java @@ -29,5 +29,8 @@ public class LauncherConstants { new Transform3d( Constants.TurretRight.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update + public static Transform3d robotToLauncherCenter = + robotToLauncherLeft.plus(robotToLauncherRight).div(2); + private LauncherConstants() {} }