diff --git a/.vscode/settings.json b/.vscode/settings.json index 16429118..3e787966 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -62,5 +62,5 @@ null ], "java.dependency.enableDependencyCheckup": false, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index c73c6835..d787f858 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -34,8 +34,6 @@ import edu.wpi.first.math.util.Units; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.math.geometry.Translation3d; - import edu.wpi.first.wpilibj.RobotBase; /** @@ -103,7 +101,18 @@ public final class Field { .withVoltageClosedLoopRampPeriod(0.02); public final class GenericRoller { - public static final TalonFXConfiguration CONFIG = + public static final TalonFXConfiguration CLOCKWISE_CONFIG = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60)) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake)); + + public static final TalonFXConfiguration COUNTERCLOCKWISE_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -122,11 +131,12 @@ public final class GenericRoller { public final class Intake { public static final int CAN_ID = 41; + public static final int FOLLOWER_CAN_ID = 42; public static final String BUS = "rio"; public static final double MOTOR_TO_MECHANISM = 36.0 / 16.0; public static final double SIM_MOI = 0.001; - public static final TalonFXConfiguration CONFIG = GenericRoller.CONFIG; + public static final TalonFXConfiguration CONFIG = GenericRoller.COUNTERCLOCKWISE_CONFIG.clone(); } public final class IntakePivot { @@ -155,8 +165,8 @@ public final class IntakePivot { new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withSupplyCurrentLimit(120) - .withStatorCurrentLimit(120) + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60) .withSupplyCurrentLimitEnable(true) .withStatorCurrentLimitEnable(true)) .withMotorOutput( @@ -168,15 +178,15 @@ 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() .withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor) .withSensorToMechanismRatio(MOTOR_TO_MECHANISM) .withRotorToSensorRatio(1.0); - } + } public final class Hopper { public static final int CAN_ID = 50; @@ -184,32 +194,33 @@ public final class Hopper { public static final double MOTOR_TO_MECHANISM = (16.0 / 12.0) * (24.0 / 16.0); public static final double SIM_MOI = 0.001; - public static final int BEAM_BREAK_ID = 0; - - public static final TalonFXConfiguration CONFIG = GenericRoller.CONFIG; + public static final TalonFXConfiguration CONFIG = GenericRoller.COUNTERCLOCKWISE_CONFIG.clone(); } public final class Indexer { public final static int CAN_ID = 51; public static final String BUS = "rio"; + // controls two rollers, so reduction is weird public static final double MOTOR_TO_MECHANISM = 36.0 / 16.0; public static final double SIM_MOI = 0.001; - public final static int BEAM_BREAK_ID = 0; - public static final TalonFXConfiguration CONFIG = - GenericRoller.CONFIG + GenericRoller.COUNTERCLOCKWISE_CONFIG.clone() .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive)); } - /* General Subsystems */ - public final class GeneralShooterHood { + public final class ShooterHood { public static final String BUS = "rio"; public static final double SIM_MOI = 0.001; + public static final int CAN_ID = 34; + + /* CANCoder */ + public static final int CANCODER_ID = 36; + public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; // spur = hood driving gear, mechanism = shooter hood gear public static final double MOTOR_TO_CANCODER = (40.0 / 12.0) * (24.0 / 17.0); @@ -220,7 +231,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; @@ -252,25 +263,36 @@ public final class GeneralShooterHood { .withForwardSoftLimitEnable(false) .withReverseSoftLimitEnable(false); - public static final FeedbackConfigs GENERAL_FEEDBACK_CONFIGS = + public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) .withRotorToSensorRatio(MOTOR_TO_CANCODER) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withFeedbackRemoteSensorID(CANCODER_ID) + .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - public static final MagnetSensorConfigs GENERAL_CANCODER_CONFIGS = + public static final MagnetSensorConfigs CANCODER_CONFIGS = new MagnetSensorConfigs() .withAbsoluteSensorDiscontinuityPoint(1.0) - .withSensorDirection(SensorDirectionValue.Clockwise_Positive); + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); } - public final class GeneralFlywheel { + public final class Flywheel { + public static final int CAN_ID = 30; + + public static final int FOLLOWER_CAN_ID = 31; + public static final int OPPOSER_ONE_CAN_ID = 32; + public static final int OPPOSER_TWO_CAN_ID = 33; + + public static final double FOLLOWER_SIM_MOI = 0.001; + public static final double EPSILON_VELOCITY = 2.5; public static final double MOTOR_TO_MECHANISM = 15.0 / 26.0; public static final String BUS = "rio"; public static final double SIM_MOI = 0.001; - public static final TalonFXConfiguration GENERAL_CONFIG = + public static final TalonFXConfiguration CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -287,188 +309,4 @@ public final class GeneralFlywheel { .withSensorToMechanismRatio(MOTOR_TO_MECHANISM) .withRotorToSensorRatio(1.0); } - - public final class GeneralTurret { - public static final String BUS = "rio"; - - public static final double SIM_MOI = 0.001; - - public static final double MOTOR_TO_CANCODER = (20.0 / 12.0); - public static final double CANCODER_TO_MECHANISM = (135.0 / 20.0); - public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_MECHANISM; - - public static final double EPSILON_RADS = Units.degreesToRadians(10.0); - public static final double STATE_TIMEOUT = 1.0; - - /* Configs */ - public static final TalonFXConfiguration GENERAL_CONFIG = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(60) - .withStatorCurrentLimit(10) - .withStatorCurrentLimitEnable(true)) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake)); - - public static final SoftwareLimitSwitchConfigs GENERAL_SOFTWARE_LIMIT_CONFIGS = - new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(false) - .withReverseSoftLimitEnable(false); - - public static final FeedbackConfigs GENERAL_FEEDBACK_CONFIGS = - new FeedbackConfigs() - .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) - .withRotorToSensorRatio(MOTOR_TO_CANCODER) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); - - public static final MagnetSensorConfigs GENERAL_CANCODER_CONFIGS = - new MagnetSensorConfigs() - .withAbsoluteSensorDiscontinuityPoint(1) - .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive); - } - - /* Left */ - public final class ShooterHoodLeft { - public static final int CAN_ID = 34; - - /* CANCoder */ - public static final int CANCODER_ID = 36; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralShooterHood.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralShooterHood.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - public final class FlywheelLeaderLeft { - public static final int CAN_ID = 30; - public static final int BEAM_BREAK_PORT = 0; // TODO: update later - - public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG; - } - - public final class FlywheelFollowerLeft { - public static final int CAN_ID = 31; - public static final double SIM_MOI = 0.001; - public static final int BEAM_BREAK_ID = 0; - - public static final TalonFXConfiguration CONFIG = - GeneralFlywheel.GENERAL_CONFIG - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)); - } - - public final class TurretLeft { - public static final int CAN_ID = 20; - - // +x -> forward; +y -> left - public static final Translation3d ROBOT_TO_TURRET = - new Translation3d( - Units.inchesToMeters(4.5), - Units.inchesToMeters(7.505), - Units.inchesToMeters(15.340)); - - /* CANCoder */ - public static final int CANCODER_ID = 22; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final double MIN_POSITION_ROTATIONS = 0 - Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - public static final double MAX_POSITION_ROTATIONS = 150.0 / 360.0 + Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - - public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_ROTATIONS); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_ROTATIONS); - - public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = - GeneralTurret.GENERAL_SOFTWARE_LIMIT_CONFIGS - .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) - .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS); - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralTurret.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralTurret.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - /* Right */ - public final class ShooterHoodRight { - public static final int CAN_ID = 35; - - /* CANCoder */ - public static final int CANCODER_ID = 37; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralShooterHood.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralShooterHood.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - public final class FlywheelLeaderRight { - public static final int CAN_ID = 32; - 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 TalonFXConfiguration CONFIG = - GeneralFlywheel.GENERAL_CONFIG - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)); - } - - public final class TurretRight { - public static final int CAN_ID = 21; - - // +x -> forward; +y -> left - public static final Translation3d ROBOT_TO_TURRET = - new Translation3d( - Units.inchesToMeters(4.5), - Units.inchesToMeters(-7.505), - Units.inchesToMeters(15.340)); - - /* CANCoder */ - public static final int CANCODER_ID = 23; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final double MIN_POSITION_ROTATIONS = -150.0 / 360.0 - Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - public static final double MAX_POSITION_ROTATIONS = 0.0 + Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - - public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_ROTATIONS); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_ROTATIONS); - - public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = - GeneralTurret.GENERAL_SOFTWARE_LIMIT_CONFIGS - .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) - .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS); - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralTurret.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralTurret.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } } diff --git a/src/main/java/org/team5924/frc2026/Robot.java b/src/main/java/org/team5924/frc2026/Robot.java index d5875b0e..cbe17d50 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; @@ -125,6 +126,12 @@ public Robot() { // Reset alert timers disabledTimer.restart(); + // Low battery alert + if (DriverStation.isEnabled()) { + disabledTimer.reset(); + lowBatteryAlert.set(false); + } + // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); @@ -147,11 +154,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 @@ -161,6 +163,8 @@ public void robotPeriodic() { } else { lowBatteryAlert.set(false); } + + LaunchCalculator.getInstance().clearLaunchingParameters(); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 1007604e..e818f7b0 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -34,29 +34,15 @@ 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,30 +51,50 @@ 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.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; 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; private final Hopper hopper; private final Indexer indexer; - private final ShooterHood shooterHoodRight; - private final Flywheel flywheelRight; - private final Turret turretRight; + private final ShooterHood shooterHood; + private final Flywheel flywheel; + + // Real/IO implementation + private final boolean realDrive = true; + private final boolean realVision = true; + private final boolean realIntake = true; + private final boolean realIntakePivot = true; + private final boolean realHopper = true; + private final boolean realIndexer = true; - private final ShooterHood shooterHoodLeft; - private final Flywheel flywheelLeft; - private final Turret turretLeft; + private final boolean realShooterHood = true; + private final boolean realFlywheel = true; // Controller private final CommandXboxController driveController = new CommandXboxController(0); @@ -102,60 +108,50 @@ public RobotContainer() { switch (Constants.currentMode) { case REAL: // Real robot, instantiate hardware IO implementations - - // -------------------------- real -------------------------- drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - (pose) -> {}); + realDrive + ? new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(TunerConstants.FrontLeft), + new ModuleIOTalonFX(TunerConstants.FrontRight), + new ModuleIOTalonFX(TunerConstants.BackLeft), + new ModuleIOTalonFX(TunerConstants.BackRight), + (pose) -> {}) + : new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + (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()); - intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); - // 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); - - // shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); - // flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); - // turretRight = new Turret(new TurretIOTalonFX(false), false); - - // ---------------------------- IO ---------------------------- - // drive = - // new Drive( - // new GyroIOPigeon2(), - // new ModuleIOTalonFX(TunerConstants.FrontLeft), - // new ModuleIOTalonFX(TunerConstants.FrontRight), - // new ModuleIOTalonFX(TunerConstants.BackLeft), - // new ModuleIOTalonFX(TunerConstants.BackRight), - // (pose) -> {}); - - intake = new Intake(new IntakeIO() {}); - // intakePivot = new IntakePivot(new IntakePivotIO() {}); - 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); - - shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); - flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); + realVision + ? new Vision( + drive::addVisionMeasurement, + new VisionIOPhotonVision( + VisionConstants.FRONT_LEFT_NAME, VisionConstants.FRONT_LEFT_TRANSFORM), + new VisionIOPhotonVision( + VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM)) + : null; + + intake = realIntake ? new Intake(new IntakeIOTalonFX()) : new Intake(new IntakeIO() {}); + intakePivot = + realIntakePivot + ? new IntakePivot(new IntakePivotIOTalonFX()) + : new IntakePivot(new IntakePivotIO() {}); + hopper = realHopper ? new Hopper(new HopperIOTalonFX()) : new Hopper(new HopperIO() {}); + + indexer = + realIndexer ? new Indexer(new IndexerIOTalonFX()) : new Indexer(new IndexerIO() {}); + shooterHood = + realShooterHood + ? new ShooterHood(new ShooterHoodIOTalonFX()) + : new ShooterHood(new ShooterHoodIO() {}); + flywheel = + realFlywheel + ? new Flywheel(new FlywheelIOTalonFX()) + : new Flywheel(new FlywheelIO() {}); break; case SIM: @@ -189,15 +185,10 @@ public RobotContainer() { intake = new Intake(new IntakeIOSim()); intakePivot = new IntakePivot(new IntakePivotIOSim()); hopper = new Hopper(new HopperIOSim()); - indexer = new Indexer(new IndexerIOSim()); - - 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); + indexer = new Indexer(new IndexerIOSim()); + shooterHood = new ShooterHood(new ShooterHoodIOSim()); + flywheel = new Flywheel(new FlywheelIOSim()); break; @@ -212,20 +203,15 @@ public RobotContainer() { new ModuleIO() {}, (pose) -> {}); - vision = null; // no vision implementation for replay + vision = null; intake = new Intake(new IntakeIO() {}); intakePivot = new IntakePivot(new IntakePivotIO() {}); - hopper = new Hopper(new HopperIO() {}); // TODO: Add replay IO implementation - indexer = new Indexer(new IndexerIO() {}); - - shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); - flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); + hopper = new Hopper(new HopperIO() {}); - shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); - flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); + indexer = new Indexer(new IndexerIO() {}); + shooterHood = new ShooterHood(new ShooterHoodIO() {}); + flywheel = new Flywheel(new FlywheelIO() {}); break; } @@ -321,89 +307,40 @@ private void configureButtonBindings() { // [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)); - 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 + /* ### intake ### */ driveController - .leftTrigger() + .rightBumper() .onTrue( Commands.runOnce( () -> { intakePivot.setGoalState(IntakePivotState.DOWN); - intake.setGoalState(IntakeState.SPITOUT); + intake.setGoalState(IntakeState.INTAKE); }, intakePivot, intake)); + driveController - .leftTrigger() - .onFalse( + .rightBumper() + .onTrue( Commands.runOnce( () -> { intakePivot.setGoalState(IntakePivotState.STOW); @@ -412,268 +349,49 @@ private void configureButtonBindings() { intakePivot, intake)); - // // ### indexing - operatorController - .a() + // manual intake + intakePivot.setDefaultCommand( + Commands.run( + () -> { + intakePivot.setGoalState(IntakePivotState.MANUAL); + intakePivot.setInput(operatorController.getLeftX()); + }, + intakePivot)); + + + // shooter + driveController + .leftBumper() .onTrue( Commands.runOnce( () -> { - // intakePivot.setGoalState(IntakePivotState.SHOOTING); + flywheel.setGoalState(Flywheel.FlywheelState.B8); indexer.setGoalState(Indexer.IndexerState.INDEXING); }, - // intakePivot, + flywheel, indexer)); - operatorController - .a() + + driveController + .leftBumper() .onFalse( Commands.runOnce( () -> { - // intakePivot.setGoalState(IntakePivotState.STOW); + flywheel.setGoalState(Flywheel.FlywheelState.OFF); indexer.setGoalState(Indexer.IndexerState.OFF); }, - // intakePivot, + flywheel, 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() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B8); - // flywheelLeft.setGoalState(FlywheelState.B8); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .y() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B12); - // flywheelLeft.setGoalState(FlywheelState.B12); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .rightTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.OFF); - // flywheelLeft.setGoalState(FlywheelState.OFF); - // }, - // flywheelLeft, - // flywheelRight)); - - // 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() - // .onTrue( - // Commands.runOnce( - // () -> { - // turretLeft.setGoalState(TurretState.ZERO); - // }, - // turretLeft)); - - // driveController - // .rightTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // turretLeft.setGoalState(TurretState.NINETY); - // }, - // 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)); - - operatorController - .y() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.SLOW_LAUNCH); - }, - flywheelRight)); - - operatorController - .povDown() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(-5.0); - }, - flywheelRight)); + shooterHood.setDefaultCommand( + Commands.runOnce( + () -> shooterHood.setGoalState(ShooterHood.ShooterHoodState.MANUAL), shooterHood)); - operatorController - .povUp() + driveController + .rightStick() .onTrue( - Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(5.0); - }, - flywheelRight)); - - // // driveController - // // .rightTrigger() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // // shooterHoodLeft.setGoalState(ShooterHoodState.OFF); - // // // shooterHoodRight.setGoalState(ShooterHoodState.OFF); - // // // turretLeft.setGoalState(Turret.TurretState.OFF); - // // // turretRight.setGoalState(Turret.TurretState.OFF); - // // flywheelLeft.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // flywheelRight.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // }, - // // shooterHoodLeft, - // // shooterHoodRight, - // // turretLeft, - // // turretRight, - // // flywheelLeft, - // // flywheelRight)); - - // // operatorController - // // .leftBumper() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // hopper.setGoalState(HopperState.ON); - // // indexer.setGoalState(IndexerState.INDEXING); - // // flywheelLeft.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // flywheelRight.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // })); - - // // operatorController - // // .leftBumper() - // // .onFalse( - // // Commands.runOnce( - // // () -> { - // // hopper.setGoalState(HopperState.OFF); - // // indexer.setGoalState(IndexerState.OFF); - // // flywheelLeft.setGoalState(FlywheelState.OFF); - // // flywheelRight.setGoalState(FlywheelState.OFF); - // // })); - - // // operatorController - // // .leftTrigger() - // // .onTrue(Commands.runOnce(() -> intake.setGoalState(IntakeState.INTAKE))); - // // operatorController - // // .leftTrigger() - // // .onFalse(Commands.runOnce(() -> intake.setGoalState(IntakeState.OFF))); - - // // operatorController - // // .rightBumper() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // flywheelRight.setGoalState(FlywheelState.MANUAL); - // // })); - - // // operatorController - // // .rightBumper() - // // .onFalse( - // // Commands.runOnce( - // // () -> { - // // flywheelRight.setGoalState(FlywheelState.OFF); - // // })); - - // // shooterHoodRight.setDefaultCommand( - // // ShooterCommands.manualShooterHood(shooterHoodRight, () -> - // // operatorController.getRightY())); - // // shooterHoodLeft.setDefaultCommand( - // // ShooterCommands.manualShooterHood(shooterHoodLeft, () -> - // // operatorController.getRightY())); - // // turretRight.setDefaultCommand( - // // ShooterCommands.manualTurret(turretRight, () -> operatorController.getLeftX())); - // // turretLeft.setDefaultCommand( - // // ShooterCommands.manualTurret(turretLeft, () -> operatorController.getRightX())); - - // // shooterHoodLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // shooterHoodRight.setDefaultCommand(getAutonomousCommand()); // todo - // // turretLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // turretRight.setDefaultCommand(getAutonomousCommand()); // todo - // // flywheelLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // flywheelRight.setDefaultCommand(getAutonomousCommand()); // todo + Commands.run(() -> shooterHood.setInput(() -> driveController.getRightY()), shooterHood)); + // TODO: auto shooting } /** diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 0ceb4d9e..a597675c 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -21,14 +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; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; -import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; @Getter public class RobotState { @@ -44,7 +36,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,40 +53,15 @@ 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(); @Getter @Setter private double yawVelocityRadPerSec = 0.0; - /* ### Intake ### */ - @Getter @Setter private IntakeState intakeState = IntakeState.OFF; - - /* ### Intake ### */ - @Getter @Setter private IntakePivotState intakePivotState = IntakePivotState.OFF; - - /* ### Hopper ### */ - @Getter @Setter private HopperState hopperState = HopperState.OFF; - - /* ### Indexer ### */ - @Getter @Setter private IndexerState indexerState = IndexerState.OFF; - - // Turret - @Getter @Setter private TurretState leftTurretState = TurretState.OFF; - @Getter @Setter private TurretState rightTurretState = TurretState.OFF; - - /*### Flywheel ### */ - @Getter @Setter private FlywheelState leftFlywheelState = FlywheelState.OFF; - @Getter @Setter private FlywheelState rightFlywheelState = FlywheelState.OFF; - - /*### Shooter Hood ### */ - @Getter @Setter private ShooterHoodState leftShooterHoodState = ShooterHoodState.OFF; - @Getter @Setter private ShooterHoodState rightShooterHoodState = ShooterHoodState.OFF; + @Getter @Setter private boolean isFlywheelAtSetpoint = false; } diff --git a/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java b/src/main/java/org/team5924/frc2026/commands/AutoBuilder.java index 89481dd1..275bd6e0 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 41cb7713..22b60c95 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,151 @@ 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(); + 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()); + } + } + + if (driveLauncherCORMaxErrorDeg.get() < driveLauncherCORMinErrorDeg.get()) return; + + // 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.robotToLauncher.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 0e956d83..09fe6119 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 78ae3c5a..dc8b8f6f 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java @@ -22,8 +22,6 @@ 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; @@ -43,23 +41,19 @@ public class AutoScoreCommands { // } private AutoScoreCommands() {} - public static Command runTrackTargetCommand( - Turret turret, ShooterHood shooterHood, Flywheel flywheel, boolean isLeft) { + public static Command runTrackTargetCommand(ShooterHood shooterHood, Flywheel flywheel) { return Commands.run( () -> { - LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(isLeft); + LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(); - turret.setAutoInput(launchParams.turretRadians()); shooterHood.setAutoInput(launchParams.hoodAngle()); flywheel.setAutoInput(launchParams.flywheelSpeed()); - turret.setGoalState(TurretState.AUTO); shooterHood.setGoalState(ShooterHoodState.AUTO); flywheel.setGoalState(FlywheelState.AUTO); LaunchCalculator.getInstance().clearLaunchingParameters(); }, - turret, shooterHood, flywheel); } diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java index ee50abf0..4737b7b8 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java @@ -21,8 +21,6 @@ import java.util.function.DoubleSupplier; 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; public class ManualShooterCommands { @@ -32,17 +30,8 @@ public static Command manualShooterHood(ShooterHood hood, DoubleSupplier inputSu return Commands.run( () -> { hood.setGoalState(ShooterHoodState.MANUAL); - hood.setInput(inputSupplier.getAsDouble()); + hood.setInput(inputSupplier); }, hood); } - - public static Command manualTurret(Turret turret, DoubleSupplier inputSupplier) { - return Commands.run( - () -> { - turret.setGoalState(TurretState.MANUAL); - turret.setInput(inputSupplier.getAsDouble()); - }, - turret); - } } 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 e595765b..cbf28600 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,11 +28,11 @@ 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 { private final FlywheelIO io; - private final boolean isLeft; private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); @@ -42,14 +43,12 @@ 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)), AUTO(() -> 0.0), - SETPOINT(() -> 0.0), + MANUAL_SETPOINT(() -> 0.0), B4(() -> 4.0), B6(() -> 6.0), @@ -60,46 +59,59 @@ public enum FlywheelState { private final DoubleSupplier velocityRotationsPerSec; } - private final String side; - - private final Alert flywheelMotorDisconnected; - protected final Alert overheatAlert; + private final Alert[] motorDisconnected = new Alert[4]; + protected final Alert[] overheatAlert = new Alert[4]; @Getter private FlywheelState goalState = FlywheelState.OFF; + private FlywheelState currentState = 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, 100.0); + } - public Flywheel(FlywheelIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; - this.isLeft = isLeft; + public Flywheel(FlywheelIO io) { this.io = io; this.goalState = FlywheelState.OFF; - this.flywheelMotorDisconnected = - new Alert(side + " Flywheel Motor Disconnected!", Alert.AlertType.kWarning); - overheatAlert = new Alert(side + " Flywheel motor overheating!", Alert.AlertType.kWarning); + for (int i = 0; i < 4; ++i) { + int id = + switch (i) { + case 0 -> Constants.Flywheel.CAN_ID; + case 1 -> Constants.Flywheel.FOLLOWER_CAN_ID; + case 2 -> Constants.Flywheel.OPPOSER_ONE_CAN_ID; + case 3 -> Constants.Flywheel.OPPOSER_TWO_CAN_ID; + default -> -1; + }; + + motorDisconnected[i] = + new Alert("Flywheel Motor (id " + id + ") Disconnected!", Alert.AlertType.kWarning); + overheatAlert[i] = + new Alert("Flywheel motor (id " + id + ") overheating!", Alert.AlertType.kWarning); + } } @Override public void periodic() { io.periodicUpdates(); io.updateInputs(inputs); - Logger.processInputs("Flywheel/" + side, inputs); + Logger.processInputs("Flywheel/", inputs); - Logger.recordOutput("Flywheel/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput( - "Flywheel/" + side + "/CurrentState", getRespectiveFlywheelState().toString()); + Logger.recordOutput("Flywheel/GoalState", goalState.toString()); + Logger.recordOutput("Flywheel/CurrentState", currentState.toString()); Logger.recordOutput( - "Flywheel/" + side + "/TargetVelocityRotationsPerSec", getTargetVelocityRotationsPerSec()); - Logger.recordOutput( - "Flywheel/" + side + "/CurrentVelocityRotationsPerSec", inputs.velocityRotationsPerSec); - Logger.recordOutput("Flywheel/" + side + "/IsAtSetpoint", isAtSetpoint); + "Flywheel/TargetVelocityRotationsPerSec", getTargetVelocityRotationsPerSec()); + Logger.recordOutput("Flywheel/CurrentVelocityRotationsPerSec", inputs.velocityRotationsPerSec); + Logger.recordOutput("Flywheel/IsAtSetpoint", isAtSetpoint); - flywheelMotorDisconnected.set(!inputs.motorConnected); + for (int i = 0; i < 4; ++i) { + motorDisconnected[i].set(!inputs.motorConnected[i]); + overheatAlert[i].set(inputs.tempCelsius[i] > Constants.OVERHEAT_THRESHOLD); + } handleCurrentState(); - overheatAlert.set(inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD); } public void runVolts(double volts) { @@ -118,19 +130,15 @@ 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); case MOVING -> DriverStation.reportError( "Flywheel: MOVING is an invalid goal state; it is a transition state!!", null); - case OFF -> setRespectiveFlywheelState(FlywheelState.OFF); - case B4, B6, B8, B12 -> setRespectiveFlywheelState(goalState); - case AUTO -> setRespectiveFlywheelState(FlywheelState.AUTO); - default -> setRespectiveFlywheelState(FlywheelState.MOVING); + case OFF -> currentState = FlywheelState.OFF; + case B4, B6, B8, B12 -> currentState = goalState; + case AUTO -> currentState = FlywheelState.AUTO; + default -> currentState = FlywheelState.MOVING; } } @@ -138,56 +146,36 @@ public boolean isAtSetpoint() { return EqualsUtil.epsilonEquals( getTargetVelocityRotationsPerSec(), inputs.velocityRotationsPerSec, - Constants.GeneralFlywheel.EPSILON_VELOCITY); + Constants.Flywheel.EPSILON_VELOCITY); } private double getTargetVelocityRotationsPerSec() { - return goalState == FlywheelState.AUTO || goalState == FlywheelState.SETPOINT + return goalState == FlywheelState.AUTO || goalState == FlywheelState.MANUAL_SETPOINT ? autoInput : goalState.velocityRotationsPerSec.getAsDouble(); } private void handleCurrentState() { isAtSetpoint = isAtSetpoint(); + RobotState.getInstance().setFlywheelAtSetpoint(isAtSetpoint); - switch (getRespectiveFlywheelState()) { + switch (currentState) { case MOVING -> { setVelocity(getTargetVelocityRotationsPerSec()); - if (isAtSetpoint() && goalState != FlywheelState.AUTO) - setRespectiveFlywheelState(goalState); + if (isAtSetpoint() && goalState != FlywheelState.AUTO) currentState = goalState; } - case MANUAL -> handleManualState(); case OFF -> stop(); case B4, B6, B8, B12 -> runVolts(getTargetVelocityRotationsPerSec()); - case AUTO -> setVelocity(autoInput); + case AUTO -> { + setAutoInput(LaunchCalculator.getInstance().getParameters().flywheelSpeed()); + setVelocity(autoInput); + } default -> setVelocity(getTargetVelocityRotationsPerSec()); } } - private void handleManualState() { - if (!goalState.equals(FlywheelState.MANUAL)) return; - - if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { - stop(); - return; - } - - setVelocity(FlywheelState.MANUAL.getVelocityRotationsPerSec().getAsDouble() * input); - } - public void updateSetpointState(double change) { - autoInput = inputs.setpointVelocityRotationsPerSec + change; - setGoalState(FlywheelState.SETPOINT); - } - - private void setRespectiveFlywheelState(FlywheelState state) { - if (isLeft) RobotState.getInstance().setLeftFlywheelState(state); - else RobotState.getInstance().setRightFlywheelState(state); - } - - private FlywheelState getRespectiveFlywheelState() { - return isLeft - ? RobotState.getInstance().getLeftFlywheelState() - : RobotState.getInstance().getRightFlywheelState(); + setAutoInput(inputs.setpointVelocityRotationsPerSec + change); + setGoalState(FlywheelState.MANUAL_SETPOINT); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIO.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIO.java index ec28f0cf..253acc3e 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIO.java @@ -21,17 +21,15 @@ public interface FlywheelIO { @AutoLog public static class FlywheelIOInputs { - public boolean motorConnected = true; public double position = 0.0; public double positionRads = 0.0; public double velocityRotationsPerSec = 0.0; public double appliedVoltage = 0.0; public double supplyCurrentAmps = 0.0; public double torqueCurrentAmps = 0.0; - public double tempCelsius = 0.0; - public double followerSupplyCurrentAmps; - public double followerTempCelsius; + public boolean[] motorConnected = {true, true, true, true}; + public double[] tempCelsius = new double[4]; public double setpointVelocityRotationsPerSec; diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java index eced248c..43c323b2 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java @@ -34,9 +34,7 @@ public FlywheelIOSim() { sim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralFlywheel.SIM_MOI, - Constants.GeneralFlywheel.MOTOR_TO_MECHANISM), + gearbox, Constants.Flywheel.SIM_MOI, Constants.Flywheel.MOTOR_TO_MECHANISM), gearbox); } @@ -45,13 +43,16 @@ public void updateInputs(FlywheelIOInputs inputs) { if (DriverStation.isDisabled()) stop(); sim.update(Constants.LOOP_PERIODIC_SECONDS); - inputs.motorConnected = true; inputs.positionRads = sim.getAngularPositionRad(); inputs.velocityRotationsPerSec = Units.radiansToRotations(sim.getAngularVelocityRadPerSec()); inputs.appliedVoltage = appliedVoltage; inputs.supplyCurrentAmps = sim.getCurrentDrawAmps(); inputs.setpointVelocityRotationsPerSec = setpoint; - inputs.tempCelsius = 25.0; + + for (int i = 0; i < 4; ++i) { + inputs.motorConnected[i] = true; + inputs.tempCelsius[i] = 25.0; + } } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index 22664cd8..0aa3acff 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -34,13 +34,10 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import java.util.ArrayList; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.FlywheelFollowerLeft; -import org.team5924.frc2026.Constants.FlywheelFollowerRight; -import org.team5924.frc2026.Constants.FlywheelLeaderLeft; -import org.team5924.frc2026.Constants.FlywheelLeaderRight; -import org.team5924.frc2026.Constants.GeneralFlywheel; +import org.team5924.frc2026.Constants.Flywheel; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; @@ -50,45 +47,34 @@ public class FlywheelIOTalonFX implements FlywheelIO { /* Hardware */ private final TalonFX leaderTalon; private final TalonFX followerTalon; + private final TalonFX opposerOneTalon; + private final TalonFX opposerTwoTalon; /* Configurators */ - private TalonFXConfigurator leaderConfig; - private TalonFXConfigurator followerConfig; + private final TalonFXConfigurator leaderConfig; + private final TalonFXConfigurator followerConfig; + private final TalonFXConfigurator opposerOneConfig; + private final TalonFXConfigurator opposerTwoConfig; /* Configs */ private final Slot0Configs slot0Configs; private final MotionMagicConfigs motionMagicConfigs; private double setpointVelocityRotationsPerSec; - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("Flywheel/Left/kP", 0.5); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("Flywheel/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("Flywheel/Left/kD", 0.0); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("Flywheel/Left/kS", 0.25); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("Flywheel/Left/kV", 0.0705); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("Flywheel/Left/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("Flywheel/Left/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("Flywheel/Left/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("Flywheel/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = new LoggedTunableNumber("Flywheel/Right/kP", 0.5); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("Flywheel/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("Flywheel/Right/kD", 0.0); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("Flywheel/Right/kS", 0.25); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("Flywheel/Right/kV", 0.0705); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("Flywheel/Right/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("Flywheel/Right/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("Flywheel/Right/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("Flywheel/Right/MotionJerk", 0.0); + /* Gains */ + private final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel/kP", 0.5); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheel/kI", 0.0); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheel/kD", 0.0); + private final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheel/kS", 0.25); + private final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheel/kV", 0.0705); + private final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheel/kA", 0.0); + + private final LoggedTunableNumber motionCruiseVelocity = + new LoggedTunableNumber("Flywheel/MotionCruiseVelocity", 10.0); + private final LoggedTunableNumber motionAcceleration = + new LoggedTunableNumber("Flywheel/MotionAcceleration", 100.0); + private final LoggedTunableNumber motionJerk = + new LoggedTunableNumber("Flywheel/MotionJerk", 0.0); /* Status Signals */ private final StatusSignal position; @@ -96,7 +82,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final StatusSignal appliedVoltage; private final StatusSignal supplyCurrent; private final StatusSignal torqueCurrent; - private final StatusSignal tempCelsius; + private final ArrayList> tempCelsius = new ArrayList<>(4); private final StatusSignal closedLoopReferenceSlope; private double prevClosedLoopReferenceSlope = 0.0; @@ -105,28 +91,16 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final VoltageOut voltageOut; private final MotionMagicVelocityVoltage motionMagicVelocity; - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; - - private final boolean isLeft; - private final String side; - - public FlywheelIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - - leaderTalon = - new TalonFX( - isLeft ? FlywheelLeaderLeft.CAN_ID : FlywheelLeaderRight.CAN_ID, - new CANBus(GeneralFlywheel.BUS)); - - followerTalon = - new TalonFX( - isLeft ? FlywheelFollowerLeft.CAN_ID : FlywheelFollowerRight.CAN_ID, - new CANBus(GeneralFlywheel.BUS)); + public FlywheelIOTalonFX() { + leaderTalon = new TalonFX(Flywheel.CAN_ID, new CANBus(Flywheel.BUS)); + followerTalon = new TalonFX(Flywheel.FOLLOWER_CAN_ID, new CANBus(Flywheel.BUS)); + opposerOneTalon = new TalonFX(Flywheel.OPPOSER_ONE_CAN_ID, new CANBus(Flywheel.BUS)); + opposerTwoTalon = new TalonFX(Flywheel.OPPOSER_TWO_CAN_ID, new CANBus(Flywheel.BUS)); leaderConfig = leaderTalon.getConfigurator(); followerConfig = followerTalon.getConfigurator(); + opposerOneConfig = opposerOneTalon.getConfigurator(); + opposerTwoConfig = opposerTwoTalon.getConfigurator(); slot0Configs = new Slot0Configs(); updateSlot0Configs(); @@ -134,42 +108,19 @@ public FlywheelIOTalonFX(boolean isLeft) { motionMagicConfigs = new MotionMagicConfigs(); updateMotionMagicConfigs(); - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = leaderConfig.apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("Flywheel/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = leaderConfig.apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("Flywheel/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - // Apply Configs - StatusCode[] statusArray = new StatusCode[10]; + StatusCode[] statusArray = new StatusCode[9]; - statusArray[0] = - leaderConfig.apply(isLeft ? FlywheelLeaderLeft.CONFIG : FlywheelLeaderRight.CONFIG); + statusArray[0] = leaderConfig.apply(Flywheel.CONFIG); statusArray[1] = leaderConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); statusArray[2] = leaderConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[3] = leaderConfig.apply(GeneralFlywheel.FEEDBACK_CONFIGS); + statusArray[3] = leaderConfig.apply(Flywheel.FEEDBACK_CONFIGS); statusArray[4] = leaderConfig.apply(slot0Configs); statusArray[5] = leaderConfig.apply(motionMagicConfigs); - statusArray[6] = - followerConfig.apply(isLeft ? FlywheelFollowerLeft.CONFIG : FlywheelFollowerRight.CONFIG); - statusArray[7] = followerConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); - statusArray[8] = followerConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[9] = followerConfig.apply(GeneralFlywheel.FEEDBACK_CONFIGS); + statusArray[6] = followerConfig.apply(Flywheel.CONFIG); + statusArray[7] = opposerOneConfig.apply(Flywheel.CONFIG); + statusArray[8] = opposerTwoConfig.apply(Flywheel.CONFIG); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -177,16 +128,13 @@ public FlywheelIOTalonFX(boolean isLeft) { if (isErrorPresent) Elastic.sendNotification( new Notification( - NotificationLevel.WARNING, - side + "Flywheel Configs", - "Error in " + side + " shooter flywheel configs!")); + NotificationLevel.WARNING, "Flywheel Configs", "Error in shooter flywheel configs!")); - Logger.recordOutput("Flywheel/" + side + "/InitConfReport", statusArray); + Logger.recordOutput("Flywheel/InitConfReport", statusArray); - followerTalon.setControl( - isLeft - ? new Follower(FlywheelLeaderLeft.CAN_ID, MotorAlignmentValue.Opposed) - : new Follower(FlywheelLeaderRight.CAN_ID, MotorAlignmentValue.Opposed)); + followerTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Aligned)); + opposerOneTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Opposed)); + opposerTwoTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Opposed)); // Get select status signals and set update frequency position = leaderTalon.getPosition(); @@ -194,12 +142,25 @@ public FlywheelIOTalonFX(boolean isLeft) { appliedVoltage = leaderTalon.getMotorVoltage(); supplyCurrent = leaderTalon.getSupplyCurrent(); torqueCurrent = leaderTalon.getTorqueCurrent(); - tempCelsius = leaderTalon.getDeviceTemp(); + + tempCelsius.add(leaderTalon.getDeviceTemp()); + tempCelsius.add(followerTalon.getDeviceTemp()); + tempCelsius.add(opposerOneTalon.getDeviceTemp()); + tempCelsius.add(opposerTwoTalon.getDeviceTemp()); closedLoopReferenceSlope = leaderTalon.getClosedLoopReferenceSlope(); BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius); + 100.0, + position, + velocity, + appliedVoltage, + supplyCurrent, + torqueCurrent, + tempCelsius.get(0), + tempCelsius.get(1), + tempCelsius.get(2), + tempCelsius.get(3)); voltageOut = new VoltageOut(0.0).withEnableFOC(true); motionMagicVelocity = new MotionMagicVelocityVoltage(0.0).withEnableFOC(true).withSlot(0); @@ -209,17 +170,21 @@ public FlywheelIOTalonFX(boolean isLeft) { @Override public void updateInputs(FlywheelIOInputs inputs) { - inputs.motorConnected = + inputs.motorConnected[0] = BaseStatusSignal.refreshAll( position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, - tempCelsius, + tempCelsius.get(0), closedLoopReferenceSlope) .isOK(); + for (int i = 1; i < 4; ++i) { + inputs.motorConnected[i] = BaseStatusSignal.refreshAll(tempCelsius.get(i)).isOK(); + } + inputs.position = BaseStatusSignal.getLatencyCompensatedValueAsDouble(position, velocity); inputs.positionRads = Units.rotationsToRadians(inputs.position); @@ -227,7 +192,10 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); inputs.torqueCurrentAmps = torqueCurrent.getValueAsDouble(); - inputs.tempCelsius = tempCelsius.getValueAsDouble(); + + for (int i = 0; i < 4; ++i) { + inputs.tempCelsius[i] = tempCelsius.get(i).getValueAsDouble(); + } inputs.motionMagicVelocityTarget = motorPositionToRads(leaderTalon.getClosedLoopReferenceSlope().getValueAsDouble()); @@ -250,58 +218,55 @@ public void periodicUpdates() { } private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); } private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); } private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + updateSlot0Configs(); + + StatusCode statusCode = leaderConfig.apply(slot0Configs); + if (!statusCode.isOK()) { + Logger.recordOutput("Flywheel/UpdateSlot0Report", statusCode); + } + }, + kP, + kI, + kD, + kS, + kV, + kA); + + LoggedTunableNumber.ifChanged( + hashCode() + 1, + () -> { + updateMotionMagicConfigs(); + + StatusCode statusCode = leaderConfig.apply(motionMagicConfigs); + if (!statusCode.isOK()) { + Logger.recordOutput("Flywheel/UpdateStatusCodeReport", statusCode); + } + }, + motionAcceleration, + motionCruiseVelocity, + motionJerk); } @Override + // runs motor at volts public void runVolts(double volts) { leaderTalon.setControl(voltageOut.withOutput(volts)); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java index 526a87d8..6ab492cf 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java @@ -26,7 +26,6 @@ import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.FieldState; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -55,10 +54,11 @@ public enum IntakePivotState { private final DoubleSupplier rads; } - private final Alert intakePivotMotorDisconnected; + private final Alert motorDisconnected; protected final Alert overheatAlert; @Getter private IntakePivotState goalState = IntakePivotState.OFF; + private IntakePivotState currentState = IntakePivotState.OFF; private boolean isAtSetpoint = false; private double lastStateChange = 0.0; private double timeSinceLastStateChange = 0.0; @@ -67,7 +67,7 @@ public IntakePivot(IntakePivotIO io) { this.io = io; this.goalState = IntakePivotState.OFF; - this.intakePivotMotorDisconnected = + this.motorDisconnected = new Alert("Intake Pivot Motor Disconnected!", Alert.AlertType.kWarning); overheatAlert = new Alert("intake pivot motor overheating!", Alert.AlertType.kWarning); } @@ -78,14 +78,13 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("IntakePivot", inputs); - intakePivotMotorDisconnected.set(!inputs.motorConnected); + motorDisconnected.set(!inputs.motorConnected); overheatAlert.set(inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD); handleCurrentState(); Logger.recordOutput("IntakePivot/GoalState", goalState.toString()); - Logger.recordOutput( - "IntakePivot/CurrentState", RobotState.getInstance().getIntakePivotState().toString()); + Logger.recordOutput("IntakePivot/CurrentState", currentState.toString()); Logger.recordOutput("IntakePivot/TargetRads", goalState.rads.getAsDouble()); Logger.recordOutput("IntakePivot/CurrentRads", inputs.positionRads); Logger.recordOutput("IntakePivot/IsAtSetpoint", isAtSetpoint); @@ -111,12 +110,12 @@ public void setGoalState(IntakePivotState goalState) { this.goalState = goalState; switch (goalState) { - case MANUAL -> RobotState.getInstance().setIntakePivotState(IntakePivotState.MANUAL); + case MANUAL -> currentState = IntakePivotState.MANUAL; case MOVING -> DriverStation.reportError( "IntakePivot: MOVING is an invalid goal state; it is a transition state!!", null); - case OFF -> RobotState.getInstance().setIntakePivotState(IntakePivotState.OFF); - default -> RobotState.getInstance().setIntakePivotState(IntakePivotState.MOVING); + case OFF -> currentState = IntakePivotState.OFF; + default -> currentState = IntakePivotState.MOVING; } lastStateChange = FieldState.getInstance().getTime(); @@ -134,10 +133,10 @@ private void handleCurrentState() { timeSinceLastStateChange = FieldState.getInstance().getTime() - lastStateChange; isAtSetpoint = isAtSetpoint(); - switch (RobotState.getInstance().getIntakePivotState()) { + switch (currentState) { case MOVING -> { setPosition(goalState.getRads().getAsDouble()); - if (isAtSetpoint) RobotState.getInstance().setIntakePivotState(goalState); + if (isAtSetpoint) currentState = goalState; } case MANUAL -> handleManualState(); case OFF -> stop(); 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 66dda21f..c265ee9b 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 @@ -49,7 +49,7 @@ public class IntakePivotIOTalonFX implements IntakePivotIO { private final TalonFX talon; /* Configurators */ - private TalonFXConfigurator talonConfig; + private final TalonFXConfigurator talonConfig; /* Configs */ private final Slot0Configs slot0Configs; @@ -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 88011da0..b650da59 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; @@ -26,13 +27,12 @@ import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; 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 { private final ShooterHoodIO io; - private final boolean isLeft; private final ShooterHoodIOInputsAutoLogged inputs = new ShooterHoodIOInputsAutoLogged(); @@ -50,6 +50,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), @@ -60,53 +62,59 @@ public enum ShooterHoodState { private final DoubleSupplier rads; } - private final String side; - private final Alert motorDisconnectedAlert; protected final Alert overheatAlert; private final Alert notImplementedAlert; private boolean showNotImplementedAlert; - @Getter private ShooterHoodState goalState; + @Getter private ShooterHoodState goalState = ShooterHoodState.OFF; + private ShooterHoodState currentState = ShooterHoodState.OFF; private boolean isAtSetpoint = false; private double lastStateChange = 0.0; private double timeSinceLastStateChange = 0.0; - @Setter private double input; - @Setter private double autoInput = 0.0; + private double input; + private double autoInput = 0.0; + + public void setInput(DoubleSupplier inputSupplier) { + input = inputSupplier.getAsDouble(); + } + + public void setAutoInput(double inputRads) { + autoInput = + MathUtil.clamp( + inputRads, + Constants.ShooterHood.MIN_POSITION_RADS, + Constants.ShooterHood.MAX_POSITION_RADS); + } - public ShooterHood(ShooterHoodIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; + public ShooterHood(ShooterHoodIO io) { this.io = io; - this.isLeft = isLeft; goalState = ShooterHoodState.OFF; motorDisconnectedAlert = - new Alert(side + " Shooter Hood Motor Disconnected!", Alert.AlertType.kWarning); - notImplementedAlert = - new Alert(side + " Auto Shooting not yet implemented!", Alert.AlertType.kWarning); - overheatAlert = new Alert(side + " Shooter Hood motor overheating!", Alert.AlertType.kWarning); + new Alert("Shooter Hood Motor Disconnected!", Alert.AlertType.kWarning); + notImplementedAlert = new Alert("Auto Shooting not yet implemented!", Alert.AlertType.kWarning); + overheatAlert = new Alert("Shooter Hood motor overheating!", Alert.AlertType.kWarning); } @Override public void periodic() { io.periodicUpdates(); io.updateInputs(inputs); - Logger.processInputs("ShooterHood/" + side, inputs); + Logger.processInputs("ShooterHood", inputs); motorDisconnectedAlert.set(!inputs.motorConnected); overheatAlert.set(inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD); handleCurrentState(); - Logger.recordOutput("ShooterHood/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput( - "ShooterHood/" + side + "/CurrentState", getRespectiveShooterHoodState().toString()); - Logger.recordOutput("ShooterHood/" + side + "/TargetRads", getTargetRads()); - Logger.recordOutput("ShooterHood/" + side + "/CurrentRads", inputs.positionRads); - Logger.recordOutput("ShooterHood/" + side + "/IsAtSetpoint", isAtSetpoint); - Logger.recordOutput( - "ShooterHood/" + side + "/TimeSinceLastStateChange", timeSinceLastStateChange); + Logger.recordOutput("ShooterHood/GoalState", goalState.toString()); + Logger.recordOutput("ShooterHood/CurrentState", currentState.toString()); + Logger.recordOutput("ShooterHood/TargetRads", getTargetRads()); + Logger.recordOutput("ShooterHood/CurrentRads", inputs.positionRads); + Logger.recordOutput("ShooterHood/IsAtSetpoint", isAtSetpoint); + Logger.recordOutput("ShooterHood/TimeSinceLastStateChange", timeSinceLastStateChange); } public void runVolts(double volts) { @@ -128,15 +136,13 @@ public void setGoalState(ShooterHoodState goalState) { this.goalState = goalState; switch (goalState) { - case MANUAL, AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> - setRespectiveShooterHoodState(goalState); + case MANUAL, AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> currentState = goalState; case MOVING -> DriverStation.reportError( - side + " Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", - null); - case AUTO -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); - case OFF -> setRespectiveShooterHoodState(ShooterHoodState.OFF); - default -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); + "Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", null); + case AUTO -> currentState = ShooterHoodState.AUTO; + case OFF -> currentState = ShooterHoodState.OFF; + default -> currentState = ShooterHoodState.MOVING; } lastStateChange = FieldState.getInstance().getTime(); @@ -144,10 +150,10 @@ public void setGoalState(ShooterHoodState goalState) { @SuppressWarnings("unused") public boolean isAtSetpoint() { - return (!Constants.GeneralShooterHood.ENABLE_TIMEOUT - || timeSinceLastStateChange > Constants.GeneralShooterHood.STATE_TIMEOUT) + return (!Constants.ShooterHood.ENABLE_TIMEOUT + || timeSinceLastStateChange > Constants.ShooterHood.STATE_TIMEOUT) && EqualsUtil.epsilonEquals( - inputs.positionRads, getTargetRads(), Constants.GeneralShooterHood.EPSILON_RADS); + inputs.positionRads, getTargetRads(), Constants.ShooterHood.EPSILON_RADS); } private double getTargetRads() { @@ -159,17 +165,20 @@ private void handleCurrentState() { isAtSetpoint = isAtSetpoint(); showNotImplementedAlert = false; - switch (getRespectiveShooterHoodState()) { + switch (currentState) { case MOVING -> { setPosition(getTargetRads()); - if (isAtSetpoint() && goalState != ShooterHoodState.AUTO) - setRespectiveShooterHoodState(goalState); + if (isAtSetpoint() && goalState != ShooterHoodState.AUTO) currentState = goalState; } case AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> { showNotImplementedAlert = true; // TODO: handle this sometime } case MANUAL -> handleManualState(); case OFF -> stop(); + case AUTO -> { + setAutoInput(LaunchCalculator.getInstance().getParameters().hoodAngle()); + if (!isAtSetpoint) setPosition(autoInput); + } default -> { if (!isAtSetpoint) setPosition(getTargetRads()); } @@ -188,15 +197,4 @@ private void handleManualState() { runVolts(ShooterHoodState.MANUAL.getRads().getAsDouble() * input); } - - private void setRespectiveShooterHoodState(ShooterHoodState state) { - if (isLeft) RobotState.getInstance().setLeftShooterHoodState(state); - else RobotState.getInstance().setRightShooterHoodState(state); - } - - private ShooterHoodState getRespectiveShooterHoodState() { - return isLeft - ? RobotState.getInstance().getLeftShooterHoodState() - : RobotState.getInstance().getRightShooterHoodState(); - } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java index b04277ef..5b37ea9d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java @@ -29,13 +29,11 @@ public class ShooterHoodIOSim implements ShooterHoodIO { private double appliedVoltage = 0.0; private double setpoint = 0.0; - public ShooterHoodIOSim(boolean isLeft) { + public ShooterHoodIOSim() { sim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralShooterHood.SIM_MOI, - Constants.GeneralShooterHood.MOTOR_TO_MECHANISM), + gearbox, Constants.ShooterHood.SIM_MOI, Constants.ShooterHood.MOTOR_TO_MECHANISM), gearbox); } @@ -63,9 +61,7 @@ public void runVolts(double volts) { public void setPosition(double rads) { rads = MathUtil.clamp( - rads, - Constants.GeneralShooterHood.MIN_POSITION_RADS, - Constants.GeneralShooterHood.MAX_POSITION_RADS); + rads, Constants.ShooterHood.MIN_POSITION_RADS, Constants.ShooterHood.MAX_POSITION_RADS); setpoint = rads; sim.setAngle(rads); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java index 19e6a16d..2487eff5 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java @@ -38,9 +38,7 @@ import edu.wpi.first.wpilibj.DriverStation; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.GeneralShooterHood; -import org.team5924.frc2026.Constants.ShooterHoodLeft; -import org.team5924.frc2026.Constants.ShooterHoodRight; +import org.team5924.frc2026.Constants.ShooterHood; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; @@ -52,43 +50,27 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { private final CANcoder cancoder; /* Configurators */ - private TalonFXConfigurator talonConfig; + private final TalonFXConfigurator talonConfig; /* Configs */ private final Slot0Configs slot0Configs; private final MotionMagicConfigs motionMagicConfigs; private double setpointRads; - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("ShooterHood/Left/kP", 250.0); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("ShooterHood/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("ShooterHood/Left/kD", 5.0); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("ShooterHood/Left/kS", 0.0); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("ShooterHood/Left/kV", 0.0); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("ShooterHood/Left/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionCruiseVelocity", 90.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionAcceleration", 900.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = - new LoggedTunableNumber("ShooterHood/Right/kP", 250.0); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("ShooterHood/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("ShooterHood/Right/kD", 5.0); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("ShooterHood/Right/kS", 0.0); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("ShooterHood/Right/kV", 0.0); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("ShooterHood/Right/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("ShooterHood/Right/MotionCruiseVelocity", 90.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("ShooterHood/Right/MotionAcceleration", 900.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("ShooterHood/Right/MotionJerk", 0.0); + /* Gains */ + private final LoggedTunableNumber kP = new LoggedTunableNumber("ShooterHood/kP", 250.0); + private final LoggedTunableNumber kI = new LoggedTunableNumber("ShooterHood/kI", 0.0); + private final LoggedTunableNumber kD = new LoggedTunableNumber("ShooterHood/kD", 5.0); + private final LoggedTunableNumber kS = new LoggedTunableNumber("ShooterHood/kS", 0.0); + private final LoggedTunableNumber kV = new LoggedTunableNumber("ShooterHood/kV", 0.0); + private final LoggedTunableNumber kA = new LoggedTunableNumber("ShooterHood/kA", 0.0); + + private final LoggedTunableNumber motionCruiseVelocity = + new LoggedTunableNumber("ShooterHood/MotionCruiseVelocity", 90.0); + private final LoggedTunableNumber motionAcceleration = + new LoggedTunableNumber("ShooterHood/MotionAcceleration", 900.0); + private final LoggedTunableNumber motionJerk = + new LoggedTunableNumber("ShooterHood/MotionJerk", 0.0); /* Status Signals */ private final StatusSignal position; @@ -111,24 +93,9 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { private final PositionVoltage positionOut; private final MotionMagicTorqueCurrentFOC motionMagicCurrent; - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; - - private final boolean isLeft; - private final String side; - - public ShooterHoodIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - - talon = - new TalonFX( - isLeft ? ShooterHoodLeft.CAN_ID : ShooterHoodRight.CAN_ID, - new CANBus(GeneralShooterHood.BUS)); - cancoder = - new CANcoder( - isLeft ? ShooterHoodLeft.CANCODER_ID : ShooterHoodRight.CANCODER_ID, - new CANBus(GeneralShooterHood.BUS)); + public ShooterHoodIOTalonFX() { + talon = new TalonFX(ShooterHood.CAN_ID, new CANBus(ShooterHood.BUS)); + cancoder = new CANcoder(ShooterHood.CANCODER_ID, new CANBus(ShooterHood.BUS)); talonConfig = talon.getConfigurator(); @@ -138,42 +105,17 @@ public ShooterHoodIOTalonFX(boolean isLeft) { motionMagicConfigs = new MotionMagicConfigs(); updateMotionMagicConfigs(); - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = talon.getConfigurator().apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("ShooterHood/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = talon.getConfigurator().apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("ShooterHood/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - // Apply Configs StatusCode[] statusArray = new StatusCode[8]; - statusArray[0] = talonConfig.apply(GeneralShooterHood.CONFIG); + statusArray[0] = talonConfig.apply(ShooterHood.CONFIG); statusArray[1] = talonConfig.apply(slot0Configs); statusArray[2] = talonConfig.apply(motionMagicConfigs); statusArray[3] = talonConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); statusArray[4] = talonConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[5] = talonConfig.apply(GeneralShooterHood.SOFTWARE_LIMIT_CONFIGS); - statusArray[6] = - talonConfig.apply( - isLeft ? ShooterHoodLeft.FEEDBACK_CONFIGS : ShooterHoodRight.FEEDBACK_CONFIGS); - statusArray[7] = - cancoder - .getConfigurator() - .apply(isLeft ? ShooterHoodLeft.CANCODER_CONFIGS : ShooterHoodRight.CANCODER_CONFIGS); + statusArray[5] = talonConfig.apply(ShooterHood.SOFTWARE_LIMIT_CONFIGS); + statusArray[6] = talonConfig.apply(ShooterHood.FEEDBACK_CONFIGS); + statusArray[7] = cancoder.getConfigurator().apply(ShooterHood.CANCODER_CONFIGS); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -182,10 +124,10 @@ public ShooterHoodIOTalonFX(boolean isLeft) { Elastic.sendNotification( new Notification( NotificationLevel.WARNING, - side + " Shooter Hood Configs", - "Error in applying " + side + " Shooter Hood configs!")); + "Shooter Hood Configs", + "Error in applying Shooter Hood configs!")); - Logger.recordOutput("ShooterHood/" + side + "/InitConfReport", statusArray); + Logger.recordOutput("ShooterHood/InitConfReport", statusArray); // Get select status signals and set update frequency position = talon.getPosition(); @@ -287,55 +229,51 @@ public void periodicUpdates() { } private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); } private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); } private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + updateSlot0Configs(); + + StatusCode statusCode = talon.getConfigurator().apply(slot0Configs); + if (!statusCode.isOK()) { + Logger.recordOutput("ShooterHood/UpdateSlot0Report", statusCode); + } + }, + kP, + kI, + kD, + kS, + kV, + kA); + + LoggedTunableNumber.ifChanged( + hashCode() + 1, + () -> { + updateMotionMagicConfigs(); + + StatusCode statusCode = talon.getConfigurator().apply(motionMagicConfigs); + if (!statusCode.isOK()) { + Logger.recordOutput("ShooterHood/UpdateStatusCodeReport", statusCode); + } + }, + motionAcceleration, + motionCruiseVelocity, + motionJerk); } @Override @@ -366,8 +304,7 @@ public void stop() { } private double clampRads(double rads) { - return MathUtil.clamp( - rads, GeneralShooterHood.MIN_POSITION_RADS, GeneralShooterHood.MAX_POSITION_RADS); + return MathUtil.clamp(rads, ShooterHood.MIN_POSITION_RADS, ShooterHood.MAX_POSITION_RADS); } private double radsToMotorPosition(double rads) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerIOTalonFX.java index 707cc26e..e509d69f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerIOTalonFX.java @@ -33,7 +33,7 @@ import org.team5924.frc2026.Constants.GenericRoller; public abstract class GenericRollerIOTalonFX implements GenericRollerIO { - private final TalonFX talon; + protected final TalonFX talon; private final StatusSignal position; private final StatusSignal velocity; 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 9227bfa4..ab9e151c 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 @@ -19,7 +19,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -30,7 +29,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); @@ -38,6 +37,7 @@ public enum HopperState implements VoltageState { } private HopperState goalState = HopperState.OFF; + private HopperState currentState = HopperState.OFF; public Hopper(HopperIO io) { super("Hopper", io); @@ -45,7 +45,7 @@ public Hopper(HopperIO io) { public void setGoalState(HopperState goalState) { this.goalState = goalState; - RobotState.getInstance().setHopperState(goalState); + currentState = goalState; } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/HopperIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/HopperIOTalonFX.java index fed7aa3f..c51cefef 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/HopperIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/HopperIOTalonFX.java @@ -23,9 +23,4 @@ public class HopperIOTalonFX extends GenericRollerIOTalonFX implements HopperIO public HopperIOTalonFX() { super(Hopper.CAN_ID, Hopper.BUS, Hopper.CONFIG, Hopper.MOTOR_TO_MECHANISM); } - - @Override - public void runVolts(double volts) { - super.runVolts(volts); - } } 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 6554f00d..9016dc05 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,12 +30,13 @@ 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; } private IndexerState goalState = IndexerState.OFF; + private IndexerState currentState = IndexerState.OFF; public Indexer(IndexerIO indexerIO) { super("Indexer", indexerIO); @@ -43,7 +44,14 @@ public Indexer(IndexerIO indexerIO) { public void setGoalState(IndexerState goalState) { this.goalState = goalState; - RobotState.getInstance().setIndexerState(goalState); + currentState = goalState; + } + + @Override + protected void handleCurrentState() { + if (RobotState.getInstance().isFlywheelAtSetpoint()) + io.runVolts(getGoalState().getVoltageSupplier().getAsDouble()); + else io.stop(); } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java index 05a88b0a..d4638a3b 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/IndexerIOTalonFX.java @@ -20,18 +20,7 @@ import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIOTalonFX; public class IndexerIOTalonFX extends GenericRollerIOTalonFX implements IndexerIO { - public IndexerIOTalonFX() { super(Indexer.CAN_ID, Indexer.BUS, Indexer.CONFIG, Indexer.MOTOR_TO_MECHANISM); } - - @Override - public void runVolts(double volts) { - super.runVolts(volts); - } - - @Override - public void stop() { - super.stop(); - } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java index 2aba3875..6a55d6a6 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java @@ -19,14 +19,12 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; import org.team5924.frc2026.util.LoggedTunableNumber; @Getter public class Intake extends GenericRoller { - @RequiredArgsConstructor @Getter public enum IntakeState implements VoltageState { @@ -38,13 +36,19 @@ public enum IntakeState implements VoltageState { } private IntakeState goalState = IntakeState.OFF; + private IntakeState currentState = IntakeState.OFF; public Intake(IntakeIO io) { super("Intake", io); } + @Override + public void periodic() { + super.periodic(); + } + public void setGoalState(IntakeState goalState) { this.goalState = goalState; - RobotState.getInstance().setIntakeState(goalState); + currentState = goalState; } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java new file mode 100644 index 00000000..e1e975f4 --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIO.java @@ -0,0 +1,21 @@ +/* + * IntakeFollowerIO.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.rollers.intake; + +import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIO; + +public interface IntakeFollowerIO extends GenericRollerIO {} diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIOTalonFX.java new file mode 100644 index 00000000..37ca92d5 --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeFollowerIOTalonFX.java @@ -0,0 +1,36 @@ +/* + * IntakeFollowerIOTalonFX.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.rollers.intake; + +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import org.team5924.frc2026.Constants.Intake; +import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIOTalonFX; + +public class IntakeFollowerIOTalonFX extends GenericRollerIOTalonFX implements IntakeFollowerIO { + public IntakeFollowerIOTalonFX() { + super(Intake.FOLLOWER_CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM); + + talon.setControl(new Follower(Intake.CAN_ID, MotorAlignmentValue.Opposed)); + } + + @Override + public void runVolts(double volts) {} + + @Override + public void stop() {} +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java index c02aaa41..7e16e0b7 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOSim.java @@ -25,4 +25,7 @@ public IntakeIOSim() { super( DCMotor.getKrakenX60Foc(1), Constants.Intake.SIM_MOI, Constants.Intake.MOTOR_TO_MECHANISM); } + + // @Override + // public void updateFollowerInputs() {} } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java index 2bf5d01a..2d214572 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/IntakeIOTalonFX.java @@ -20,12 +20,10 @@ import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerIOTalonFX; public class IntakeIOTalonFX extends GenericRollerIOTalonFX implements IntakeIO { + private final IntakeFollowerIOTalonFX follower; + public IntakeIOTalonFX() { super(Intake.CAN_ID, Intake.BUS, Intake.CONFIG, Intake.MOTOR_TO_MECHANISM); - } - - @Override - public void runVolts(double volts) { - super.runVolts(volts); + follower = new IntakeFollowerIOTalonFX(); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java b/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java deleted file mode 100644 index ef32c6ad..00000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java +++ /dev/null @@ -1,213 +0,0 @@ -/* - * Turret.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.DoubleSupplier; -import lombok.Getter; -import lombok.Setter; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2026.Constants; -import org.team5924.frc2026.FieldState; -import org.team5924.frc2026.RobotState; -import org.team5924.frc2026.util.EqualsUtil; -import org.team5924.frc2026.util.LoggedTunableNumber; - -public class Turret extends SubsystemBase { - - private final TurretIO io; - private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); - - @Setter private double input; - - private final boolean isLeft; - - public enum TurretState { - OFF(() -> 0.0), - MOVING(() -> 0.0), - - // voltage at which the example subsystem motor moves when controlled by the operator - MANUAL(new LoggedTunableNumber("Turret/Volts/OperatorVoltage", 1.0)), - - NINETY(new LoggedTunableNumber("Turret/Volts/Ninety", Math.PI / 2)), - - ZERO(() -> 0.0), - - AUTO(() -> 0.0); - - @Getter private final DoubleSupplier rads; - - TurretState(DoubleSupplier rads) { - this.rads = rads; - } - } - - @Getter private TurretState goalState = TurretState.OFF; - - private final Alert motorDisconnected; - - protected final Alert overheatAlert; - - private double lastStateChange = 0.0; - - private double exceedBoundsDirection; - private boolean shouldContinue; - - private final String side; - - @Setter private double autoInput = 0.0; - - public Turret(TurretIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; - this.io = io; - this.goalState = TurretState.OFF; - this.motorDisconnected = - new Alert(side + " Turret Motor Disconnected!", Alert.AlertType.kWarning); - this.isLeft = isLeft; - - overheatAlert = new Alert(side + " Turret motor overheating!", Alert.AlertType.kWarning); - } - - @Override - public void periodic() { - io.periodicUpdates(); - io.updateInputs(inputs); - Logger.processInputs("Turret/" + side, inputs); - - Logger.recordOutput("Turret/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput("Turret/" + side + "/CurrentState", getRespectiveTurretState()); - Logger.recordOutput("Turret/" + side + "/TargetRads", goalState.rads.getAsDouble()); - Logger.recordOutput("Turret/" + side + "/CurrentRads", inputs.positionRads); - Logger.recordOutput("Turret/" + side + "/ExceedBoundsDirection", exceedBoundsDirection); - Logger.recordOutput("Turret/" + side + "/ShouldContinue", shouldContinue); - - handleCurrentState(); - - motorDisconnected.set(!inputs.motorConnected); - - boolean isOverheating = inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD; - overheatAlert.set(isOverheating); - } - - public boolean isAtSetpoint() { - // return RobotState.getTime() - lastStateChange > Constants.GeneralTurret.STATE_TIMEOUT - return EqualsUtil.epsilonEquals( - inputs.setpointRads, inputs.positionRads, Constants.GeneralTurret.EPSILON_RADS); - } - - private void handleCurrentState() { - TurretState currentState = - isLeft - ? RobotState.getInstance().getLeftTurretState() - : RobotState.getInstance().getRightTurretState(); - switch (currentState) { - case MOVING -> { - if (isAtSetpoint() && goalState != TurretState.AUTO) setRespectiveTurretState(goalState); - } - case MANUAL -> handleManualState(); - case OFF -> io.stop(); - case NINETY -> { - io.setPosition(goalState.rads.getAsDouble() * (isLeft ? 1 : -1)); - } - default -> io.setPosition(goalState.rads.getAsDouble()); - } - } - - private void handleManualState() { - if (!goalState.equals(TurretState.MANUAL)) return; - - if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { - io.runVolts(0); - return; - } - - tryRunVolts(TurretState.MANUAL.getRads().getAsDouble() * input); - } - - public void tryRunVolts(double volts) { - // if (!(shouldContinue = shouldContinueInDirection(volts, inputs.turretPositionRads))) return; - - io.runVolts(volts); - } - - public boolean shouldContinueInDirection(double volts, double rads) { - double voltDirection = Math.signum(volts); - return (voltDirection != (exceedBoundsDirection = exceedBoundsDirection(rads))); - } - - /** - * @param rads rads - * @return -1 for min bound, 0 for within, 1 for upper bound - */ - public double exceedBoundsDirection(double rads) { - double min = - isLeft ? Constants.TurretLeft.MIN_POSITION_RADS : Constants.TurretRight.MIN_POSITION_RADS; - double max = - isLeft ? Constants.TurretLeft.MAX_POSITION_RADS : Constants.TurretRight.MAX_POSITION_RADS; - if (rads <= min) return -1.0; - if (rads >= max) return 1.0; - return 0.0; - } - - public void setGoalState(TurretState goalState) { - if (this.goalState.equals(goalState)) return; - if (goalState.equals(TurretState.MANUAL) && Math.abs(input) <= Constants.JOYSTICK_DEADZONE) - return; - - if (goalState != TurretState.MOVING) this.goalState = goalState; - switch (goalState) { - case MANUAL: - setRespectiveTurretState(TurretState.MANUAL); - break; - case MOVING: - DriverStation.reportError( - side + " Turret: MOVING is an invalid goal state; it is a transition state!!", null); - break; - case OFF: - setRespectiveTurretState(TurretState.OFF); - io.stop(); - break; - case AUTO: - setRespectiveTurretState(TurretState.MOVING); - io.setPosition(autoInput); - break; - default: - setRespectiveTurretState(TurretState.MOVING); - io.setPosition(goalState.rads.getAsDouble()); - break; - } - - lastStateChange = FieldState.getInstance().getTime(); - } - - private void setRespectiveTurretState(TurretState state) { - if (isLeft) RobotState.getInstance().setLeftTurretState(state); - else RobotState.getInstance().setRightTurretState(state); - } - - private TurretState getRespectiveTurretState() { - return isLeft - ? RobotState.getInstance().getLeftTurretState() - : RobotState.getInstance().getRightTurretState(); - } - - public double getSetpoint() { - return inputs.setpointRads; - } -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java deleted file mode 100644 index 09f847bb..00000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java +++ /dev/null @@ -1,79 +0,0 @@ -/* - * TurretIO.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import org.littletonrobotics.junction.AutoLog; - -public interface TurretIO { - @AutoLog - public static class TurretIOInputs { - public boolean motorConnected = true; - public double position = 0.0; - public double positionRads = 0.0; - public double positionCancoder = 0.0; - public double velocityRadsPerSec = 0.0; - public double appliedVoltage = 0.0; - public double supplyCurrentAmps = 0.0; - public double torqueCurrentAmps = 0.0; - public double tempCelsius = 0.0; - - public double motionMagicVelocityTarget = 0.0; - public double motionMagicPositionTarget = 0.0; - - public double setpointRads = 0.0; - public double acceleration = 0.0; - - public boolean cancoderConnected = true; - public double cancoderAbsolutePosition = 0.0; - public double cancoderVelocity = 0.0; - public double cancoderSupplyVoltage = 0.0; - public double cancoderPositionRotations = 0.0; - } - - /** - * Updates the inputs object with the latest data from hardware - * - * @param inputs Inputs to update - */ - public default void updateInputs(TurretIOInputs inputs) {} - - /** Updates that are be called in turret periodic */ - public default void periodicUpdates() {} - - /** - * Sets the turret motor to the specified voltage - * - * @param volts number of volts - */ - public default void runVolts(double volts) {} - - /** - * Sets the turret motor to a specified angle - * - * @param rads target angle - */ - public default void setPosition(double rads) {} - - /** Holds the turret motor at a set position */ - public default void holdPosition(double rads) {} - - /** stops the motor */ - default void stop() {} - - /** Sets the turret position to specified rads from center */ - default void setPositionSetpoint(double radiansFromCenter, double radsPerSecond) {} -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java deleted file mode 100644 index d8669e28..00000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java +++ /dev/null @@ -1,79 +0,0 @@ -/* - * TurretIOSim.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import org.team5924.frc2026.Constants; - -public class TurretIOSim implements TurretIO { - private final DCMotorSim sim; - private final DCMotor gearbox = DCMotor.getKrakenX60Foc(1); - private double appliedVoltage = 0.0; - private double setpoint = 0.0; - private final double minPositionRads; - private final double maxPositionRads; - - public TurretIOSim(boolean isLeft) { - sim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralTurret.MOTOR_TO_MECHANISM, - Constants.GeneralTurret.SIM_MOI), - gearbox); - minPositionRads = - isLeft ? Constants.TurretLeft.MIN_POSITION_RADS : Constants.TurretRight.MIN_POSITION_RADS; - maxPositionRads = - isLeft ? Constants.TurretLeft.MAX_POSITION_RADS : Constants.TurretRight.MAX_POSITION_RADS; - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - if (DriverStation.isDisabled()) runVolts(0.0); - - sim.update(Constants.LOOP_PERIODIC_SECONDS); - inputs.motorConnected = true; - inputs.positionRads = sim.getAngularPositionRad(); - inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVoltage = appliedVoltage; - inputs.supplyCurrentAmps = sim.getCurrentDrawAmps(); - inputs.setpointRads = setpoint; - inputs.tempCelsius = 25.0; - } - - @Override - public void runVolts(double volts) { - appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); - sim.setInputVoltage(appliedVoltage); - } - - @Override - public void setPosition(double rads) { - rads = MathUtil.clamp(rads, minPositionRads, maxPositionRads); - setpoint = rads; - sim.setAngle(rads); - } - - @Override - public void stop() { - runVolts(0.0); - } -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java deleted file mode 100644 index 7595a429..00000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java +++ /dev/null @@ -1,386 +0,0 @@ -/* - * TurretIOTalonFX.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Temperature; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.GeneralTurret; -import org.team5924.frc2026.Constants.TurretLeft; -import org.team5924.frc2026.Constants.TurretRight; -import org.team5924.frc2026.util.Elastic; -import org.team5924.frc2026.util.Elastic.Notification; -import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; -import org.team5924.frc2026.util.LoggedTunableNumber; - -public class TurretIOTalonFX implements TurretIO { - /* Hardware */ - private final TalonFX turretTalon; - private final CANcoder turretCANCoder; - - /* Configurators */ - private TalonFXConfigurator turretTalonConfig; - - /* Configs */ - private final Slot0Configs slot0Configs; - private final MotionMagicConfigs motionMagicConfigs; - private double setpointRads; - - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("Turret/Left/kP", 25.0); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("Turret/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("Turret/Left/kD", 1.00); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("Turret/Left/kS", 0.0); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("Turret/Left/kV", 0.0); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("Turret/Left/kA", 0.00); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("Turret/Left/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("Turret/Left/MotionAcceleration", 20.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("Turret/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = new LoggedTunableNumber("Turret/Right/kP", 25.0); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("Turret/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("Turret/Right/kD", 1.00); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("Turret/Right/kS", 0.0); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("Turret/Right/kV", 0.0); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("Turret/Right/kA", 0.00); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("Turret/Right/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("Turret/Right/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("Turret/Right/MotionJerk", 0.0); - - /* Status Signals */ - private final StatusSignal turretPosition; - private final StatusSignal turretVelocity; - private final StatusSignal turretAppliedVoltage; - private final StatusSignal turretSupplyCurrent; - private final StatusSignal turretTorqueCurrent; - private final StatusSignal turretTempCelsius; - - private final StatusSignal cancoderAbsolutePosition; - private final StatusSignal cancoderVelocity; - private final StatusSignal cancoderSupplyVoltage; - private final StatusSignal cancoderPositionRotations; - - private final StatusSignal closedLoopReferenceSlope; - private double prevClosedLoopReferenceSlope = 0.0; - private double prevReferenceSlopeTimestamp = 0.0; - - private final TorqueCurrentFOC currentOut; - private final PositionVoltage positionOut; - private final MotionMagicTorqueCurrentFOC motionMagicCurrent; - - private final double minPositionRads; - private final double maxPositionRads; - - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; - - private final boolean isLeft; - private final String side; - - public TurretIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - minPositionRads = isLeft ? TurretLeft.MIN_POSITION_RADS : TurretRight.MIN_POSITION_RADS; - maxPositionRads = isLeft ? TurretLeft.MAX_POSITION_RADS : TurretRight.MAX_POSITION_RADS; - - turretTalon = - new TalonFX(isLeft ? TurretLeft.CAN_ID : TurretRight.CAN_ID, new CANBus(GeneralTurret.BUS)); - turretCANCoder = - new CANcoder( - isLeft ? TurretLeft.CANCODER_ID : TurretRight.CANCODER_ID, - new CANBus(GeneralTurret.BUS)); - - turretTalonConfig = turretTalon.getConfigurator(); - - slot0Configs = new Slot0Configs(); - updateSlot0Configs(); - - motionMagicConfigs = new MotionMagicConfigs(); - updateMotionMagicConfigs(); - - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = turretTalonConfig.apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("Turret/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = turretTalonConfig.apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("Turret/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - - // Apply Configs - StatusCode[] statusArray = new StatusCode[8]; - - statusArray[0] = turretTalonConfig.apply(GeneralTurret.GENERAL_CONFIG); - statusArray[1] = turretTalonConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); - statusArray[2] = turretTalonConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[3] = - turretTalonConfig.apply( - isLeft ? TurretLeft.SOFTWARE_LIMIT_CONFIGS : TurretRight.SOFTWARE_LIMIT_CONFIGS); - statusArray[4] = - turretTalonConfig.apply( - isLeft ? TurretLeft.FEEDBACK_CONFIGS : TurretRight.FEEDBACK_CONFIGS); - statusArray[5] = turretTalonConfig.apply(motionMagicConfigs); - statusArray[6] = turretTalonConfig.apply(slot0Configs); - statusArray[7] = - turretCANCoder - .getConfigurator() - .apply(isLeft ? TurretLeft.CANCODER_CONFIGS : TurretRight.CANCODER_CONFIGS); - - boolean isErrorPresent = false; - for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; - - if (isErrorPresent) - Elastic.sendNotification( - new Notification( - NotificationLevel.WARNING, - side + " Turret Configs", - "Error in applying " + side + " Turret configs!")); - - Logger.recordOutput("Turret/" + side + "/InitConfReport", statusArray); - - // Get select status signals and set update frequency - turretPosition = turretTalon.getPosition(); - turretVelocity = turretTalon.getVelocity(); - turretAppliedVoltage = turretTalon.getMotorVoltage(); - turretSupplyCurrent = turretTalon.getSupplyCurrent(); - turretTorqueCurrent = turretTalon.getTorqueCurrent(); - turretTempCelsius = turretTalon.getDeviceTemp(); - - cancoderAbsolutePosition = turretCANCoder.getAbsolutePosition(); - cancoderVelocity = turretCANCoder.getVelocity(); - cancoderSupplyVoltage = turretCANCoder.getSupplyVoltage(); - cancoderPositionRotations = turretCANCoder.getPosition(); - - closedLoopReferenceSlope = turretTalon.getClosedLoopReferenceSlope(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, - turretPosition, - turretVelocity, - turretAppliedVoltage, - turretSupplyCurrent, - turretTorqueCurrent, - turretTempCelsius, - cancoderAbsolutePosition, - cancoderVelocity, - cancoderSupplyVoltage, - cancoderPositionRotations, - closedLoopReferenceSlope); - - currentOut = new TorqueCurrentFOC(0.0); - positionOut = new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true).withSlot(0); - motionMagicCurrent = new MotionMagicTorqueCurrentFOC(0.0).withSlot(0); - - BaseStatusSignal.waitForAll(0.5, turretPosition, cancoderAbsolutePosition); - - turretTalon.setPosition(0.0); - turretCANCoder.setPosition(0.0); - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - inputs.motorConnected = - BaseStatusSignal.refreshAll( - turretPosition, - turretVelocity, - turretAppliedVoltage, - turretSupplyCurrent, - turretTorqueCurrent, - turretTempCelsius, - closedLoopReferenceSlope) - .isOK(); - - inputs.cancoderConnected = - BaseStatusSignal.refreshAll( - cancoderAbsolutePosition, - cancoderVelocity, - cancoderSupplyVoltage, - cancoderPositionRotations) - .isOK(); - - inputs.position = - BaseStatusSignal.getLatencyCompensatedValueAsDouble(turretPosition, turretVelocity); - inputs.positionRads = Units.rotationsToRadians(inputs.position); - - inputs.velocityRadsPerSec = Units.rotationsToRadians(turretVelocity.getValueAsDouble()); - inputs.appliedVoltage = turretAppliedVoltage.getValueAsDouble(); - inputs.supplyCurrentAmps = turretSupplyCurrent.getValueAsDouble(); - inputs.torqueCurrentAmps = turretTorqueCurrent.getValueAsDouble(); - inputs.tempCelsius = turretTempCelsius.getValueAsDouble(); - - inputs.motionMagicVelocityTarget = - motorPositionToRads(turretTalon.getClosedLoopReferenceSlope().getValueAsDouble()); - inputs.motionMagicPositionTarget = - motorPositionToRads(turretTalon.getClosedLoopReference().getValueAsDouble()); - - inputs.setpointRads = setpointRads; - - double currentTime = closedLoopReferenceSlope.getTimestamp().getTime(); - double timeDiff = currentTime - prevReferenceSlopeTimestamp; - if (timeDiff > 0.0) { - inputs.acceleration = - (inputs.motionMagicVelocityTarget - prevClosedLoopReferenceSlope) / timeDiff; - } - prevClosedLoopReferenceSlope = inputs.motionMagicVelocityTarget; - prevReferenceSlopeTimestamp = currentTime; - - inputs.cancoderAbsolutePosition = cancoderAbsolutePosition.getValueAsDouble(); - inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); - inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); - inputs.cancoderPositionRotations = cancoderPositionRotations.getValueAsDouble(); - - inputs.positionCancoder = inputs.cancoderPositionRotations; - } - - @Override - public void periodicUpdates() { - updateLoggedTunableNumbers(); - } - - private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } - } - - private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } - } - - private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } - } - - @Override - public void runVolts(double volts) { - turretTalon.setControl(currentOut.withOutput(40 * volts)); - } - - @Override - public void setPosition(double rads) { - if (!DriverStation.isEnabled()) { - stop(); - return; - } - - setpointRads = clampRads(rads); - turretTalon.setControl(motionMagicCurrent.withPosition(radsToMotorPosition(setpointRads))); - } - - @Override - public void holdPosition(double rads) { - turretTalon.setControl(positionOut.withPosition(radsToMotorPosition(rads))); - } - - @Override - public void stop() { - turretTalon.stopMotor(); - } - - private double clampRads(double rads) { - return MathUtil.clamp(rads, minPositionRads, maxPositionRads); - } - - private double radsToMotorPosition(double rads) { - while (rads < minPositionRads) rads += 2 * Math.PI; - while (rads > maxPositionRads) rads -= 2 * Math.PI; - - if (rads < minPositionRads) turretPosition.getValueAsDouble(); - return Units.radiansToRotations(rads); - } - - private double motorPositionToRads(double motorPosition) { - return Units.rotationsToRadians(motorPosition); - } -} 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 c3385a32..570c738a 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 82089a34..93cf0e20 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,15 @@ 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 boolean a = true; + public TranslationRotation cameraToTarget; public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; } @@ -36,6 +39,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 2b2e6f29..2b7f01e0 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,12 +51,73 @@ 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())); + + this.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()))); + } + + private void updateLoggedTunableNumbers() { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> + 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()))), + xInches, + yInches, + zInches, + rollDegrees, + pitchDegrees, + yawDegrees); } @Override public void updateInputs(VisionIOInputs inputs) { + updateLoggedTunableNumbers(); inputs.connected = camera.isConnected(); + inputs.cameraToTarget = null; // Read new camera observations Set tagIds = new HashSet<>(); @@ -86,6 +160,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 91c24523..d6aaffc2 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -22,7 +22,6 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; @@ -65,7 +64,6 @@ public record LaunchingParameters( double hoodAngle, double hoodVelocity, double flywheelSpeed, - double turretRadians, double distance, double distanceNoLookahead, double timeOfFlight, @@ -102,22 +100,22 @@ 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( "LaunchCalculator/Presets/HoodMin/HoodAngle", - Units.radiansToDegrees(Constants.GeneralShooterHood.MIN_POSITION_RADS)), + Units.radiansToDegrees(Constants.ShooterHood.MIN_POSITION_RADS)), new LoggedTunableNumber("LaunchCalculator/Presets/HoodMin/FlywheelSpeed", 50)); public static final LaunchPreset hoodMaxPreset = new LaunchPreset( new LoggedTunableNumber( "LaunchCalculator/Presets/HoodMax/HoodAngle", - Units.radiansToDegrees(Constants.GeneralShooterHood.MAX_POSITION_RADS)), + Units.radiansToDegrees(Constants.ShooterHood.MAX_POSITION_RADS)), new LoggedTunableNumber("LaunchCalculator/Presets/HoodMax/FlywheelSpeed", 50)); public static final LoggedTunableNumber passingIdleSpeed = @@ -159,48 +157,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 +255,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 +305,16 @@ public static double getMaxTimeOfFlight() { return timeOfFlightMap.get(maxDistance); } - public LaunchingParameters getParameters(boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; + public LaunchingParameters getParameters() { boolean passing = - AllianceFlipUtil.applyX(RobotState.getInstance().getEstimatedPose().getX()) + AllianceFlipUtil.applyX(RobotState.getInstance().getOdometryPose().getX()) > FieldConstants.LinesVertical.hubCenter; 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( @@ -301,7 +332,7 @@ public LaunchingParameters getParameters(boolean isLeft) { double launcherToTargetDistance = target.getDistance(launcherPosition.getTranslation()); // Calculate field relative launcher velocity - var robotVelocity = RobotState.getInstance().getFieldSetpointVelocity(); + var robotVelocity = new ChassisSpeeds(); // RobotState.getInstance().getFieldSetpointVelocity(); var robotAngle = RobotState.getInstance().getRotation(); ChassisSpeeds launcherVelocity = DriverStation.isAutonomous() @@ -331,12 +362,10 @@ public LaunchingParameters getParameters(boolean isLeft) { lookaheadLauncherToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } - double turretRads = getTurretAngle(lookaheadPose, target, isLeft).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); // Calculate remaining parameters double hoodAngle = @@ -377,7 +406,6 @@ public LaunchingParameters getParameters(boolean isLeft) { hoodAngle + Units.degreesToRadians(hoodAngleOffsetDeg), hoodVelocity, flywheelVelocity, - turretRads, lookaheadLauncherToTargetDistance, launcherToTargetDistance, timeOfFlight, @@ -389,21 +417,13 @@ public LaunchingParameters getParameters(boolean isLeft) { Logger.recordOutput( "LaunchCalculator/LauncherToTargetDistance", lookaheadLauncherToTargetDistance); - return latestParameters; - } - - private static Rotation2d getTurretAngle( - Pose2d turretPose, Translation2d target, boolean isLeft) { + Logger.recordOutput("LaunchCalculator/LaunchParams", latestParameters); - Rotation2d turretToHub = target.minus(turretPose.getTranslation()).getAngle(); - Rotation2d turretRads = turretPose.getRotation().minus(turretToHub); - return turretRads; + return latestParameters; } private static Rotation2d getDriveAngleWithLauncherOffset( - Pose2d robotPose, Translation2d target, boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; - + Pose2d robotPose, Translation2d target) { Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); Rotation2d hubAngle = new Rotation2d( @@ -427,7 +447,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 @@ -461,8 +481,7 @@ public Translation2d getPassingTarget() { * @param forceBlue Always use the blue hub target * @return The target pose for the aimed robot. */ - public static Pose2d getStationaryAimedPose( - Translation2d robotTranslation, boolean forceBlue, boolean isLeft) { + public static Pose2d getStationaryAimedPose(Translation2d robotTranslation, boolean forceBlue) { // Calculate target Translation2d target = FieldConstants.Hub.topCenterPoint.toTranslation2d(); if (!forceBlue) { @@ -470,8 +489,7 @@ public static Pose2d getStationaryAimedPose( } return new Pose2d( - robotTranslation, - getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target, isLeft)); + robotTranslation, getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target)); } /** 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 4879bf42..08e27561 100644 --- a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java +++ b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java @@ -18,16 +18,16 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import org.team5924.frc2026.Constants; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; public class LauncherConstants { - public static Transform3d robotToLauncherLeft = + // TODO: update to be more accurate + public static Transform3d robotToLauncher = new Transform3d( - Constants.TurretLeft.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update - - public static Transform3d robotToLauncherRight = - new Transform3d( - Constants.TurretRight.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update + new Translation3d( + Units.inchesToMeters(5.764), Units.inchesToMeters(0.0), Units.inchesToMeters(21.203)), + new Rotation3d(0.0, 0.0, Math.PI)); private LauncherConstants() {} }