diff --git a/build.gradle b/build.gradle index da5895a..3789561 100644 --- a/build.gradle +++ b/build.gradle @@ -213,6 +213,7 @@ spotless { exclude "src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhoton.java" exclude "src/main/java/org/team2342/lib/util/Elastic.java" exclude "src/main/java/org/team2342/frc/util/FieldConstants.java" + exclude "src/main/java/org/team2342/frc/util/HubShiftUtil.java" } licenseHeader("// Copyright (c) $currentYear Team 2342\n// https://github.com/FRCTeamPhoenix\n//\n// This source code is licensed under the MIT License.\n// See the LICENSE file in the root directory of this project.\n\n", "package ") diff --git a/src/main/deploy/calibrations/left_arducam_800.json b/src/main/deploy/calibrations/swerve_arducam_800.json similarity index 100% rename from src/main/deploy/calibrations/left_arducam_800.json rename to src/main/deploy/calibrations/swerve_arducam_800.json diff --git a/src/main/deploy/pathplanner/autos/LeftTrench.auto b/src/main/deploy/pathplanner/autos/LeftTrench.auto new file mode 100644 index 0000000..f6f570e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftTrench.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "autoShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "L1" + } + }, + { + "type": "path", + "data": { + "pathName": "L2" + } + }, + { + "type": "named", + "data": { + "name": "autoIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "L3" + } + }, + { + "type": "named", + "data": { + "name": "autoShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RightTrench.auto b/src/main/deploy/pathplanner/autos/RightTrench.auto new file mode 100644 index 0000000..5cdc8e0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RightTrench.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "autoShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "R1" + } + }, + { + "type": "path", + "data": { + "pathName": "R2" + } + }, + { + "type": "named", + "data": { + "name": "autoIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "R3" + } + }, + { + "type": "named", + "data": { + "name": "autoShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L1.path b/src/main/deploy/pathplanner/paths/L1.path new file mode 100644 index 0000000..6677534 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.188614457831325, + "y": 4.843650602409639 + }, + "prevControl": null, + "nextControl": { + "x": 3.4642092444429213, + "y": 4.547552372815045 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.282, + "y": 5.969204819277109 + }, + "prevControl": { + "x": 2.1223114027842978, + "y": 6.161557495159928 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 142.56500000000003 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 142.56500000000003 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L2.path b/src/main/deploy/pathplanner/paths/L2.path new file mode 100644 index 0000000..cf9a7ab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L2.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.282, + "y": 5.969 + }, + "prevControl": null, + "nextControl": { + "x": 2.502228276547937, + "y": 6.08731950899461 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8224216867469876, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 2.9992886550228137, + "y": 7.24829220211798 + }, + "nextControl": { + "x": 4.067638556570527, + "y": 7.460331806686578 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.898686746987952, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 4.792781990522218, + "y": 7.409964962466848 + }, + "nextControl": { + "x": 6.561335127600884, + "y": 7.412679890366718 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.221, + "y": 6.078 + }, + "prevControl": { + "x": 6.9961203530394185, + "y": 6.319179213587149 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -2.2493064773545726 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 145.38922380390605 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L3.path b/src/main/deploy/pathplanner/paths/L3.path new file mode 100644 index 0000000..98c8388 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L3.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.221, + "y": 6.078 + }, + "prevControl": null, + "nextControl": { + "x": 7.338850958716274, + "y": 5.857520632417328 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8440481927710834, + "y": 7.378879518072289 + }, + "prevControl": { + "x": 6.450791877215048, + "y": 7.126510358553121 + }, + "nextControl": { + "x": 5.613219480999081, + "y": 7.474890485267073 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2978915662650605, + "y": 7.171253012048193 + }, + "prevControl": { + "x": 3.6947172730944753, + "y": 7.449264616345176 + }, + "nextControl": { + "x": 2.740325263749712, + "y": 6.780628363502267 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.587590361445783, + "y": 5.269831325301205 + }, + "prevControl": { + "x": 2.6197177782925976, + "y": 5.60770395147218 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.263577656602 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -5.020341700004992 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R1.path b/src/main/deploy/pathplanner/paths/R1.path new file mode 100644 index 0000000..6103521 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.314, + "y": 3.2522111269614835 + }, + "prevControl": null, + "nextControl": { + "x": 3.102094518399991, + "y": 3.0505431690468168 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.072, + "y": 2.0618544935805994 + }, + "prevControl": { + "x": 2.6503900810408965, + "y": 2.6146340039316636 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -149.189 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -149.189 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R2.path b/src/main/deploy/pathplanner/paths/R2.path new file mode 100644 index 0000000..56d968f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R2.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.072, + "y": 2.062 + }, + "prevControl": null, + "nextControl": { + "x": 2.183090085894316, + "y": 1.8380379656816952 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9919156626506025, + "y": 0.84410843373494 + }, + "prevControl": { + "x": 2.5473352340379964, + "y": 1.1596555660285341 + }, + "nextControl": { + "x": 3.2312215506910658, + "y": 0.6742577568893806 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.281156626506024, + "y": 0.84410843373494 + }, + "prevControl": { + "x": 5.764583687019719, + "y": 0.5070260819383028 + }, + "nextControl": { + "x": 6.661872155520445, + "y": 1.0925389574346511 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.871, + "y": 2.062 + }, + "prevControl": { + "x": 6.725101421617429, + "y": 1.8283617076130094 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 2.606 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -149.189 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/R3.path b/src/main/deploy/pathplanner/paths/R3.path new file mode 100644 index 0000000..4f3d9a9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/R3.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.871, + "y": 2.062 + }, + "prevControl": null, + "nextControl": { + "x": 6.628907859032533, + "y": 1.418560826553372 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.2265180722891555, + "y": 0.7020481927710842 + }, + "prevControl": { + "x": 6.776606455949892, + "y": 0.907849549399763 + }, + "nextControl": { + "x": 5.982340608807217, + "y": 0.61069550339358 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.751506024096386, + "y": 0.7020481927710842 + }, + "prevControl": { + "x": 3.044562187542487, + "y": 0.619758952159487 + }, + "nextControl": { + "x": 2.131443827357652, + "y": 0.8761596896991247 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.735, + "y": 2.062 + }, + "prevControl": { + "x": 1.9184873757368757, + "y": 1.8921989901526062 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -149.189 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 2.606 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 6d83b54..30ae940 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -25,7 +25,7 @@ public final class Constants { public static final Mode CURRENT_MODE = Mode.REAL; - public static final boolean TUNING = false; + public static final boolean TUNING = true; public static enum Mode { /** Running on a real robot. */ @@ -134,52 +134,25 @@ public static final class DriveConstants { public static final double ODOMETRY_FREQUENCY = IS_CANFD ? 250.0 : 100.0; } - public static final class KickerConstants { - public static final double RUN_VOLTAGE = 7.0; - - public static final MotorConfig KICKER_CONFIG = - new MotorConfig() - .withMotorInverted(true) - .withSupplyCurrentLimit(30.0) - .withStatorCurrentLimit(40.0) - .withIdleMode(MotorConfig.IdleMode.BRAKE); - } - public static final class ConductorConstants { public static final double TRENCH_BUFFER = 0.1; } public static final class IndexerConstants { - public static final double RUN_VOLTAGE = 8.0; - public static final double FEEDER_VOLTAGE = 10.0; - + public static final double RUN_VOLTAGE = 6; public static final double RUN_CURRENT = 30.0; - public static final double FEEDER_CURRENT = 30.0; - public static final MotorConfig INDEXER_BELT_CONFIG = + public static final MotorConfig INDEXER_MOTOR_CONFIG = new MotorConfig() - .withMotorInverted(false) - .withSupplyCurrentLimit(30.0) - .withStatorCurrentLimit(70.0) - .withIdleMode(MotorConfig.IdleMode.BRAKE); - public static final MotorConfig INDEXER_FEEDER_CONFIG = - new MotorConfig() - .withMotorInverted(false) - .withSupplyCurrentLimit(30.0) + .withMotorInverted(true) + .withSupplyCurrentLimit(40.0) .withStatorCurrentLimit(70.0) .withIdleMode(MotorConfig.IdleMode.BRAKE); - public static final DCMotor INDEXER_BELT_SIM_MOTOR = DCMotor.getKrakenX60(1); - public static final DCMotorSim INDEXER_BELT_SIM = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(INDEXER_BELT_SIM_MOTOR, 0.003, 1), - INDEXER_BELT_SIM_MOTOR); - - public static final DCMotor INDEXER_FEEDER_SIM_MOTOR = DCMotor.getKrakenX60(1); - public static final DCMotorSim INDEXER_FEEDER_SIM = + public static final DCMotor INDEXER_SIM_MOTOR = DCMotor.getKrakenX60(1); + public static final DCMotorSim INDEXER_SIM = new DCMotorSim( - LinearSystemId.createDCMotorSystem(INDEXER_FEEDER_SIM_MOTOR, 0.0025, 1), - INDEXER_FEEDER_SIM_MOTOR); + LinearSystemId.createDCMotorSystem(INDEXER_SIM_MOTOR, 0.003, 1), INDEXER_SIM_MOTOR); } public static final class IntakeConstants { @@ -221,23 +194,25 @@ public static final class IntakeConstants { } public static final class ShooterConstants { - public static final double FLYWHEEL_GEAR_RATIO = 23.0 / 24.0; + public static final double FLYWHEEL_GEAR_RATIO = 1.0; public static final double FLYWHEEL_RADIUS_METERS = Units.inchesToMeters(2.0); public static final double IDLE_SPEED = 15.0; + public static final double FLYWHEEL_AT_GOAL_TOLERANCE = 3.0; public static final PIDFFConfigs FLYWHEEL_PID_CONFIGS = new PIDFFConfigs() - .withKP(0.5) - .withKI(0.0) - .withKS(Units.radiansToRotations(0.1824)) - .withKV(Units.radiansToRotations(0.020077)) - .withKA(Units.radiansToRotations(0.0037398)); + .withKP(0.575) + .withKI(0.6) + .withKS(0.15989) + .withKV(0.019968) + .withKA(0.005977); + public static final SmartMotorConfig FLYWHEEL_CONFIG = new SmartMotorConfig() .withControlType(ControlType.VELOCITY) .withGearRatio(FLYWHEEL_GEAR_RATIO) - .withMotorInverted(false) + .withMotorInverted(true) .withSupplyCurrentLimit(40) .withStatorCurrentLimit(70) .withProfileConstraintsRad(new TrapezoidProfile.Constraints(1000, 1000)); @@ -251,53 +226,42 @@ public static final class ShooterConstants { public static final double ENCODER_TO_HOOD = 344.0 / 22.0; public static final double MAX_ANGLE = 0.273; public static final double TARGET_TOLERANCE = 0.01; + } - public static final PIDFFConfigs HOOD_MOTOR_PID_CONFIGS = - new PIDFFConfigs().withKP(400).withKI(100).withKD(30); - public static final SmartMotorConfig HOOD_MOTOR_CONFIG = - new SmartMotorConfig() - .withGearRatio(ENCODER_TO_HOOD) - .withControlType(ControlType.PROFILED_POSITION) - .withIdleMode(IdleMode.BRAKE) - .withSupplyCurrentLimit(30.0) - .withFeedbackConfig( - FeedbackConfig.fused( - CANConstants.HOOD_ENCODER_ID, KRAKEN_TO_ENCODER, 0.662 / 2, false)) - .withProfileConstraintsRad( - new TrapezoidProfile.Constraints( - Units.degreesToRadians(1800), Units.degreesToRadians(1800))); + public static final class KickerConstants { + public static final double RUN_VOLTAGE = 7.5; - public static final DCMotor HOOD_SIM_MOTOR = DCMotor.getKrakenX60(1); - public static final DCMotorSim HOOD_SIM = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - HOOD_SIM_MOTOR, 0.01, (KRAKEN_TO_ENCODER * ENCODER_TO_HOOD)), - HOOD_SIM_MOTOR); + public static final MotorConfig KICKER_CONFIG = + new MotorConfig() + .withMotorInverted(true) + .withSupplyCurrentLimit(30.0) + .withStatorCurrentLimit(40.0) + .withIdleMode(MotorConfig.IdleMode.BRAKE); } public static final class TurretConstants { public static final Transform3d TURRET_TRANSFORM = new Transform3d( new Translation3d( - Units.inchesToMeters(-4.960), - Units.inchesToMeters(5.997), - Units.inchesToMeters(14.823)), + Units.inchesToMeters(-6), + Units.inchesToMeters(0.002), + Units.inchesToMeters(14.165)), new Rotation3d(Rotation2d.k180deg)); public static final double AT_POSITION_THRESHOLD = 0.01; - public static final double STARTING_ANGLE = Units.degreesToRadians(0); - public static final double MIN_TURRET_ANGLE = Units.degreesToRadians(-90); - public static final double MAX_TURRET_ANGLE = Units.degreesToRadians(90); + public static final double STARTING_ANGLE = Units.degreesToRadians(90); + public static final double MIN_TURRET_ANGLE = Units.degreesToRadians(0); + public static final double MAX_TURRET_ANGLE = Units.degreesToRadians(359); - public static final double GEAR_RATIO = (46.0 / 12.0) * (100.0 / 10.0); + public static final double GEAR_RATIO = (48.0 / 12.0) * (100.0 / 10.0); public static final SmartMotorConfig TURRET_CONFIG = new SmartMotorConfig() .withControlType(ControlType.PROFILED_POSITION) .withGearRatio(GEAR_RATIO) .withIdleMode(IdleMode.BRAKE) - .withProfileConstraintsRad(new TrapezoidProfile.Constraints(Math.PI, Math.PI)) + .withProfileConstraintsRad(new TrapezoidProfile.Constraints(12 * Math.PI, 10 * Math.PI)) .withSupplyCurrentLimit(40); public static final PIDFFConfigs PID_CONFIG = new PIDFFConfigs().withKP(100).withKI(10); @@ -317,20 +281,15 @@ public static final class CANConstants { public static final int[] BR_IDS = {4, 8, 12}; public static final int FLYWHEEL_MOTOR_ID = 14; - public static final int HOOD_MOTOR_ID = 15; - public static final int HOOD_ENCODER_ID = 16; + public static final int KICKER_ID = 15; + public static final int TURRET_ID = 16; + public static final int INDEXER_MOTOR_ID = 17; public static final int INTAKE_WHEEL_MOTOR_ID = 18; public static final int INTAKE_PIVOT_MOTOR_ID = 19; public static final int INTAKE_PIVOT_ENCODER_ID = 20; - public static final int INDEXER_WHEEL_ID = 20; - public static final int INDEXER_BELT_ID = 21; - - public static final int KICKER_ID = 22; - - public static final int TURRET_ID = 23; - + public static final int CANDLE_ID = 61; public static final int PDH_ID = 62; } } diff --git a/src/main/java/org/team2342/frc/Robot.java b/src/main/java/org/team2342/frc/Robot.java index 0bafef0..6d9845e 100644 --- a/src/main/java/org/team2342/frc/Robot.java +++ b/src/main/java/org/team2342/frc/Robot.java @@ -30,6 +30,7 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.team2342.frc.util.FiringSolver; +import org.team2342.frc.util.HubShiftUtil; import org.team2342.frc.util.PhoenixUtils; import org.team2342.lib.logging.ExecutionLogger; @@ -158,6 +159,9 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); ExecutionLogger.log("Commands"); + Logger.recordOutput("ShiftUtil/Official", HubShiftUtil.getOfficialShiftInfo()); + Logger.recordOutput("ShiftUtil/Shifted", HubShiftUtil.getShiftedShiftInfo()); + robotContainer.updateAlerts(); FiringSolver.getInstance().clearCachedSolution(); @@ -171,7 +175,9 @@ public void disabledInit() {} public void disabledPeriodic() {} @Override - public void disabledExit() {} + public void disabledExit() { + robotContainer.getPivot().freeze(); + } @Override public void autonomousInit() { diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 411623e..838c377 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -10,13 +10,15 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import lombok.Getter; @@ -24,10 +26,10 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.team2342.frc.Constants.CANConstants; -import org.team2342.frc.Constants.ConductorConstants; import org.team2342.frc.Constants.DriveConstants; import org.team2342.frc.Constants.IndexerConstants; import org.team2342.frc.Constants.IntakeConstants; +import org.team2342.frc.Constants.KickerConstants; import org.team2342.frc.Constants.ShooterConstants; import org.team2342.frc.Constants.TurretConstants; import org.team2342.frc.Constants.VisionConstants; @@ -44,8 +46,9 @@ import org.team2342.frc.subsystems.indexer.Indexer; import org.team2342.frc.subsystems.intake.Pivot; import org.team2342.frc.subsystems.intake.Wheels; +import org.team2342.frc.subsystems.leds.LEDSubsystem; import org.team2342.frc.subsystems.shooter.Flywheel; -import org.team2342.frc.subsystems.shooter.Hood; +import org.team2342.frc.subsystems.shooter.Kicker; import org.team2342.frc.subsystems.shooter.Turret; import org.team2342.frc.subsystems.vision.Vision; import org.team2342.frc.subsystems.vision.VisionIO; @@ -53,8 +56,13 @@ import org.team2342.frc.subsystems.vision.VisionIOSim; import org.team2342.frc.util.FieldConstants; import org.team2342.frc.util.FiringSolver; +import org.team2342.frc.util.HubShiftUtil; +import org.team2342.lib.leds.LedIO; +import org.team2342.lib.leds.LedIOCANdle; import org.team2342.lib.motors.dumb.DumbMotorIO; import org.team2342.lib.motors.dumb.DumbMotorIOSim; +import org.team2342.lib.motors.dumb.DumbMotorIOTalonFX; +import org.team2342.lib.motors.dumb.DumbMotorIOTalonFXFOC; import org.team2342.lib.motors.smart.SmartMotorIO; import org.team2342.lib.motors.smart.SmartMotorIOSim; import org.team2342.lib.motors.smart.SmartMotorIOTalonFX; @@ -67,10 +75,11 @@ public class RobotContainer { @Getter private final Vision vision; @Getter private final Pivot pivot; @Getter private final Indexer indexer; + @Getter private final Kicker kicker; @Getter private final Wheels wheels; @Getter private final Flywheel flywheel; - @Getter private final Hood hood; @Getter private final Turret turret; + @Getter private final LEDSubsystem leds; @Getter private final Conductor conductor; @@ -89,8 +98,9 @@ public class RobotContainer { private final Alert operatorControllerAlert = new Alert("Operator controller is disconnected!", AlertType.kError); - private final Trigger trenchTrigger; private final Trigger allianceZoneTrigger; + private final Trigger shiftAboutToEnd; + private final Trigger activeOrPassing; public RobotContainer() { switch (Constants.CURRENT_MODE) { @@ -108,26 +118,46 @@ public RobotContainer() { drive::getTimestampedHeading, new VisionIOPhoton( VisionConstants.SWERVE_CAMERA_PARAMETERS, - PoseStrategy.CONSTRAINED_SOLVEPNP, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); turret = new Turret( new SmartMotorIOTalonFX( CANConstants.TURRET_ID, TurretConstants.TURRET_CONFIG.withPIDFFConfigs(TurretConstants.PID_CONFIG))); - flywheel = new Flywheel(new SmartMotorIO() {}); - hood = new Hood(new SmartMotorIO() {}); - indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}); - wheels = new Wheels(new DumbMotorIO() {}); + flywheel = + new Flywheel( + new SmartMotorIOTalonFX( + CANConstants.FLYWHEEL_MOTOR_ID, + ShooterConstants.FLYWHEEL_CONFIG.withPIDFFConfigs( + ShooterConstants.FLYWHEEL_PID_CONFIGS))); + kicker = + new Kicker( + new DumbMotorIOTalonFXFOC(CANConstants.KICKER_ID, KickerConstants.KICKER_CONFIG)); + + indexer = + new Indexer( + new DumbMotorIOTalonFXFOC( + CANConstants.INDEXER_MOTOR_ID, IndexerConstants.INDEXER_MOTOR_CONFIG)); pivot = new Pivot( new SmartMotorIOTalonFX( CANConstants.INTAKE_PIVOT_MOTOR_ID, IntakeConstants.PIVOT_MOTOR_CONFIG.withPIDFFConfigs( IntakeConstants.PIVOT_MOTOR_PID_CONFIGS))); - new SmartMotorIOTalonFX( - CANConstants.TURRET_ID, - TurretConstants.TURRET_CONFIG.withPIDFFConfigs(TurretConstants.PID_CONFIG)); + wheels = + new Wheels( + new DumbMotorIOTalonFX( + CANConstants.INTAKE_WHEEL_MOTOR_ID, + IntakeConstants.INTAKE_WHEELS_MOTOR_CONFIG)); + + conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + leds = + new LEDSubsystem( + new LedIOCANdle(CANConstants.CANDLE_ID, 32), + "CANdle", + vision::hasTags, + conductor::getCurrentState); LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev); break; @@ -157,10 +187,7 @@ public RobotContainer() { indexer = new Indexer( new DumbMotorIOSim( - IndexerConstants.INDEXER_BELT_SIM_MOTOR, IndexerConstants.INDEXER_BELT_SIM), - new DumbMotorIOSim( - IndexerConstants.INDEXER_FEEDER_SIM_MOTOR, - IndexerConstants.INDEXER_FEEDER_SIM)); + IndexerConstants.INDEXER_SIM_MOTOR, IndexerConstants.INDEXER_SIM)); wheels = new Wheels( @@ -173,17 +200,14 @@ public RobotContainer() { ShooterConstants.FLYWHEEL_SIM_MOTOR, ShooterConstants.FLYWHEEL_SIM, 1)); - hood = - new Hood( - new SmartMotorIOSim( - ShooterConstants.HOOD_MOTOR_CONFIG.withPIDFFConfigs( - new PIDFFConfigs().withKP(1)), - ShooterConstants.HOOD_SIM_MOTOR, - ShooterConstants.HOOD_SIM, - 1)); + kicker = new Kicker(new DumbMotorIO() {}); turret = new Turret(new SmartMotorIO() {}); pivot = new Pivot(new SmartMotorIO() {}); + conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + leds = + new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState); + break; default: @@ -200,73 +224,35 @@ public RobotContainer() { drive::getTimestampedHeading, new VisionIO() {}, new VisionIO() {}); - indexer = new Indexer(new DumbMotorIO() {}, new DumbMotorIO() {}); + indexer = new Indexer(new DumbMotorIO() {}); wheels = new Wheels(new DumbMotorIO() {}); pivot = new Pivot(new SmartMotorIO() {}); flywheel = new Flywheel(new SmartMotorIO() {}); - hood = new Hood(new SmartMotorIO() {}); turret = new Turret(new SmartMotorIO() {}); + kicker = new Kicker(new DumbMotorIO() {}); + + conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + leds = + new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState); break; } - conductor = new Conductor(flywheel, hood, drive::getPose, drive::getChassisSpeeds); - configureNamedCommands(); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); autoChooser.get(); - - if (Constants.TUNING) setupDevelopmentRoutines(); - autoChooser.addOption( - "scuffed wall", - Commands.run(() -> drive.runVelocity(new ChassisSpeeds(1, 0, 0)), drive) - .alongWith(conductor.runState(ConductorState.WARM_UP)) - .withTimeout(1) - .andThen( - conductor - .goToState(ConductorState.TRACKED_FIRING) - .andThen(conductor.runState(ConductorState.TRACKED_FIRING)) - .alongWith(indexer.feed()) - .alongWith( - DriveCommands.joystickDriveAtAngle( - drive, - () -> 0, - () -> 0, - () -> - FiringSolver.getInstance() - .calculate(drive.getChassisSpeeds(), drive.getPose()) - .turretAngle())))); - - autoChooser.addOption( - "wait n fire", + "Point and fire", conductor .runState(ConductorState.WARM_UP) - .alongWith( - DriveCommands.joystickDriveAtAngle( - drive, - () -> 0, - () -> 0, - () -> - FiringSolver.getInstance() - .calculate(drive.getChassisSpeeds(), drive.getPose()) - .turretAngle())) - .withTimeout(1) - .andThen( - conductor - .goToState(ConductorState.TRACKED_FIRING) - .andThen(conductor.runState(ConductorState.TRACKED_FIRING)) - .alongWith(indexer.feed()) - .alongWith( - DriveCommands.joystickDriveAtAngle( - drive, - () -> 0, - () -> 0, - () -> - FiringSolver.getInstance() - .calculate(drive.getChassisSpeeds(), drive.getPose()) - .turretAngle())))); + .withTimeout(2.0) + .alongWith(pivot.holdAngle(0)) + .alongWith(wheels.in()) + .alongWith(indexer.in()) + .alongWith(kicker.in())); + + if (Constants.TUNING) setupDevelopmentRoutines(); SmartDashboard.putData( "Calculate Vision Heading Offset", @@ -274,29 +260,50 @@ public RobotContainer() { .alongWith(Commands.print("Calculated Vision Offset")) .ignoringDisable(true)); - trenchTrigger = - new Trigger( - () -> - withinBounds( - drive.getPose().getX(), - AllianceUtils.flipToAlliance(FieldConstants.LeftBump.nearLeftCorner).getX() - + ConductorConstants.TRENCH_BUFFER, - AllianceUtils.flipToAlliance(FieldConstants.LeftBump.farLeftCorner).getX() - - ConductorConstants.TRENCH_BUFFER)); allianceZoneTrigger = new Trigger( () -> withinBounds( drive.getPose().getX(), AllianceUtils.flipToAlliance(Pose2d.kZero).getX(), - AllianceUtils.flipToAlliance(FieldConstants.LeftBump.nearLeftCorner).getX() - + ConductorConstants.TRENCH_BUFFER)); + AllianceUtils.flipToAlliance(FieldConstants.LeftBump.nearLeftCorner).getX())); + shiftAboutToEnd = new Trigger(() -> (HubShiftUtil.getShiftedShiftInfo().remainingTime() < 1.0)); + activeOrPassing = + new Trigger( + () -> + HubShiftUtil.getShiftedShiftInfo().active() + || FiringSolver.getInstance() + .calculate(drive.getChassisSpeeds(), drive.getPose()) + .passing()); configureBindings(); } private void configureNamedCommands() { NamedCommands.registerCommand("Named Command Test", Commands.print("Named Command Test")); + NamedCommands.registerCommand( + "autoShoot", + conductor + .runState(ConductorState.TRACKED_FIRING) + .alongWith(indexer.in()) + .alongWith(kicker.in()) + .withTimeout(2.0) + .finallyDo( + () -> { + indexer.stop(); + kicker.stop(); + })); + + NamedCommands.registerCommand("autoIntake", wheels.in().finallyDo(() -> wheels.stop())); + + NamedCommands.registerCommand( + "stopAll", + new InstantCommand( + () -> { + wheels.stop(); + indexer.stop(); + kicker.stop(); + })); } private void configureBindings() { @@ -308,10 +315,12 @@ private void configureBindings() { () -> -driverController.getLeftX(), () -> -driverController.getRightX())); + // X-Stop driverController.x().whileTrue(Commands.run(drive::stopWithX, drive)); + // Reset Gyro driverController - .b() + .povDown() .onTrue( Commands.runOnce( () -> @@ -320,53 +329,72 @@ private void configureBindings() { drive) .ignoringDisable(true)); - driverController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); - - // Shooting Controls + // Trench Drive driverController - .rightBumper() + .b() .whileTrue( - conductor - .goToState(ConductorState.BUMPER_SHOT) - .andThen(conductor.runState(ConductorState.BUMPER_SHOT).alongWith(indexer.feed()))) - .onFalse(indexer.stop()); + DriveCommands.joystickDriveAtAngle( + drive, + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> Rotation2d.kZero)); + + // Run Intake + driverController + .leftTrigger() + .whileTrue(wheels.in().alongWith(pivot.holdAngle(IntakeConstants.MIN_ANGLE))) + .onFalse(wheels.stop().alongWith(pivot.stop())); + // Retract Intake + driverController + .leftBumper() + .whileTrue(pivot.holdAngle(IntakeConstants.MAX_ANGLE)) + .onFalse(pivot.stop()); + + driverController + .povUp() + .whileTrue(pivot.holdAngle(IntakeConstants.MAX_ANGLE)) + .onFalse(pivot.stop()); + + driverController + .povDown() + .whileTrue(pivot.agitate().alongWith(wheels.runIntake(5))) + .onFalse(wheels.stop().alongWith(pivot.stop())); + + // Auto Shoot driverController .rightTrigger() - .whileTrue( - conductor - .goToState(ConductorState.TRACKED_FIRING) - .andThen( - conductor.runState(ConductorState.TRACKED_FIRING).alongWith(indexer.feed())) - .alongWith( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -driverController.getLeftY(), - () -> -driverController.getLeftX(), - () -> - FiringSolver.getInstance() - .calculate(drive.getChassisSpeeds(), drive.getPose()) - .turretAngle()))) - .onFalse(indexer.stop()); - - // Operator Overrides - operatorController.povRight().whileTrue(indexer.feed()).onFalse(indexer.stop()); - operatorController.povLeft().whileTrue(indexer.out()).onFalse(indexer.stop()); - - operatorController.leftBumper().whileTrue(wheels.out()).onFalse(wheels.stop()); - operatorController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); - - operatorController.rightTrigger().whileTrue(conductor.runState(ConductorState.OVERRIDE_25)); - - operatorController.rightBumper().whileTrue(conductor.runState(ConductorState.OVERRIDE_23)); + .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) + .and(activeOrPassing) + .whileTrue(Commands.parallel(indexer.in(), kicker.in())) + .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); + + // Firing during inactive period + driverController + .rightTrigger() + .and(() -> !HubShiftUtil.getShiftedShiftInfo().active()) + .onTrue(driverController.rumble(RumbleType.kBothRumble, 1.0).withTimeout(0.5)); + + // Shift Timer Override + driverController + .rightBumper() + .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) + .whileTrue(Commands.parallel(indexer.in(), kicker.in())) + .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); // Location Triggers - trenchTrigger - .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) - .whileTrue(conductor.runState(ConductorState.TRENCH)); allianceZoneTrigger .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) .whileTrue(conductor.runState(ConductorState.WARM_UP)); + + // Shift Util Resets + RobotModeTriggers.teleop().onTrue(Commands.runOnce(HubShiftUtil::initialize)); + RobotModeTriggers.autonomous().onTrue(Commands.runOnce(HubShiftUtil::initialize)); + + // Shift Ending Rumble + shiftAboutToEnd + .and(RobotModeTriggers.teleop()) + .onTrue(driverController.rumble(RumbleType.kRightRumble, 1.0).withTimeout(0.25)); } public Command getAutonomousCommand() { @@ -410,7 +438,7 @@ public void updateAlerts() { operatorControllerAlert.set(!operatorController.isConnected()); } - private boolean withinBounds(double value, double bound1, double bound2) { + public static boolean withinBounds(double value, double bound1, double bound2) { return value <= Math.max(bound1, bound2) && value >= Math.min(bound1, bound2); } } diff --git a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java index f239eb2..bd18dcf 100644 --- a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java +++ b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java @@ -26,8 +26,9 @@ public class RotationLockedDrive extends Command { private final Drive drive; private final Timer inputTimer = new Timer(); - private final ProfiledPIDController rotationPID = + private final ProfiledPIDController angleController = new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(8.0, 20.0)); + private Rotation2d rotationLock; public RotationLockedDrive( @@ -40,7 +41,7 @@ public RotationLockedDrive( this.ySupplier = ySupplier; this.rotationSupplier = rotationSupplier; - rotationPID.enableContinuousInput(-Math.PI, Math.PI); + angleController.enableContinuousInput(-Math.PI, Math.PI); super.setName("RotationLockedDrive"); super.addRequirements(drive); @@ -50,7 +51,7 @@ public RotationLockedDrive( public void initialize() { inputTimer.restart(); rotationLock = drive.getRawOdometryPose().getRotation(); - rotationPID.reset(drive.getRotation().getRadians()); + angleController.reset(drive.getRotation().getRadians()); } @Override @@ -69,13 +70,13 @@ public void execute() { Logger.recordOutput("Drive/RotationLock/Engaged", true); Logger.recordOutput("Drive/RotationLock/LockedHeading", rotationLock); omega = - rotationPID.calculate( + angleController.calculate( drive.getRawOdometryPose().getRotation().getRadians(), rotationLock.getRadians()); } else { Logger.recordOutput("Drive/RotationLock/Engaged", false); omega = controllerOmega * drive.getMaxAngularSpeedRadPerSec(); rotationLock = drive.getRawOdometryPose().getRotation(); - rotationPID.reset(drive.getRotation().getRadians()); + angleController.reset(drive.getRotation().getRadians()); } // Calculate new linear velocity diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index 7b3e666..cffb130 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -9,26 +9,29 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.Supplier; import lombok.experimental.Delegate; +import org.team2342.frc.Constants; import org.team2342.frc.subsystems.shooter.Flywheel; -import org.team2342.frc.subsystems.shooter.Hood; +import org.team2342.frc.subsystems.shooter.Turret; import org.team2342.frc.util.FiringSolver; import org.team2342.lib.fsm.StateMachine; import org.team2342.lib.logging.ExecutionLogger; +import org.team2342.lib.logging.tunable.TunableNumber; public class Conductor extends SubsystemBase { + private TunableNumber flywheelSpeed = + new TunableNumber("FlywheelSpeedMPS", 0, () -> Constants.TUNING); + public enum ConductorState { UNDETERMINED, DISABLED, - TRENCH, - BUMPER_SHOT, WARM_UP, TRACKED_FIRING, - OVERRIDE_25, - OVERRIDE_23; + TUNING, } @Delegate(types = FSMDelegate.class) @@ -40,18 +43,18 @@ public enum ConductorState { ConductorState.class); private final Flywheel flywheel; - private final Hood hood; + private final Turret turret; private final Supplier poseSupplier; private final Supplier velocitySupplier; public Conductor( Flywheel flywheel, - Hood hood, + Turret turret, Supplier poseSupplier, Supplier velocitySupplier) { this.flywheel = flywheel; - this.hood = hood; + this.turret = turret; this.poseSupplier = poseSupplier; this.velocitySupplier = velocitySupplier; @@ -59,7 +62,7 @@ public Conductor( setupStateCommands(); setupTransitions(); - enable(); + fsm.enable(); setDefaultCommand(runState(ConductorState.DISABLED)); } @@ -71,7 +74,7 @@ public void periodic() { } public Command runState(ConductorState state) { - return run(() -> fsm.requestTransition(state)); + return run(() -> fsm.requestTransition(state)).withName("Conductor Run " + state.name()); } public Command goToState(ConductorState state) { @@ -83,102 +86,70 @@ public Command waitForState(ConductorState state) { } private void setupStateCommands() { - fsm.addStateCommand(ConductorState.DISABLED, hood.stop().alongWith(flywheel.stop())); - - fsm.addStateCommand(ConductorState.TRENCH, hood.holdAngle(0.0)); - - fsm.addStateCommand( - ConductorState.BUMPER_SHOT, - flywheel - .shoot(FiringSolver.BUMPER_SHOT.wheelSpeed()) - .alongWith(hood.holdAngle(FiringSolver.BUMPER_SHOT.hoodAngle()))); + fsm.addStateCommand(ConductorState.DISABLED, Commands.parallel(turret.stop(), flywheel.stop())); fsm.addStateCommand( ConductorState.WARM_UP, - hood.holdAngle( - () -> - FiringSolver.getInstance() - .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .deadlineFor(flywheel.warmUp())); - - fsm.addStateCommand( - ConductorState.TRACKED_FIRING, - hood.holdAngle( + turret + .runPositionCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .deadlineFor( - flywheel.shoot( + .turretAngle()) + .alongWith( + flywheel.warmUpCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) .wheelSpeed()))); - fsm.addStateCommand(ConductorState.OVERRIDE_25, flywheel.shoot(25.0)); - - fsm.addStateCommand(ConductorState.OVERRIDE_23, flywheel.shoot(23.0)); - } - - private void setupTransitions() { - fsm.addOmniTransition(ConductorState.DISABLED); - - fsm.addOmniTransition(ConductorState.TRENCH, hood.goToAngle(0.0)); - - fsm.addOmniTransition( - ConductorState.BUMPER_SHOT, - hood.holdAngle(FiringSolver.BUMPER_SHOT.hoodAngle()) - .withTimeout(0.5) - .deadlineFor(flywheel.shoot(FiringSolver.BUMPER_SHOT.wheelSpeed()))); - - fsm.addOmniTransition( - ConductorState.WARM_UP, - hood.holdAngle( - () -> - FiringSolver.getInstance() - .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .withTimeout(0.5) - .deadlineFor(flywheel.warmUp())); - - fsm.addOmniTransition( + fsm.addStateCommand( ConductorState.TRACKED_FIRING, - hood.holdAngle( + turret + .runPositionCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .withTimeout(0.5) - .deadlineFor( + .turretAngle()) + .alongWith( flywheel.shoot( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) .wheelSpeed()))); - fsm.addOmniTransition( - ConductorState.OVERRIDE_25, - hood.holdAngle( + + fsm.addStateCommand( + ConductorState.TUNING, + turret + .runPositionCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .withTimeout(0.5) - .deadlineFor(flywheel.shoot(25.0))); + .turretAngle()) + .alongWith(flywheel.shoot(flywheelSpeed))); + } - fsm.addOmniTransition(ConductorState.OVERRIDE_23, flywheel.shoot(23.0)); + public Command disable() { + return Commands.runOnce(() -> fsm.disable()); } - public Command makeShooterCommand(double flywheelMetersPerSec, double hoodAngle) { - return hood.holdAngle(hoodAngle).alongWith(flywheel.shoot(flywheelMetersPerSec)); + public Command enable() { + return Commands.runOnce(() -> fsm.enable()); } - public interface FSMDelegate { - String dot(); + public ConductorState getCurrentState() { + return fsm.getCurrentState(); + } - void enable(); + private void setupTransitions() { + fsm.addOmniTransition(ConductorState.DISABLED); + fsm.addOmniTransition(ConductorState.TRACKED_FIRING); + fsm.addOmniTransition(ConductorState.WARM_UP); + fsm.addOmniTransition(ConductorState.TUNING); + } - void disable(); + public interface FSMDelegate { + String dot(); boolean isEnabled(); } diff --git a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java index b2b6f4a..36edbf4 100644 --- a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; import org.team2342.frc.Constants.IndexerConstants; @@ -17,63 +18,43 @@ import org.team2342.lib.motors.dumb.DumbMotorIOInputsAutoLogged; public class Indexer extends SubsystemBase { - private final DumbMotorIO beltMotor; - private final DumbMotorIOInputsAutoLogged wheelMotorInputs = new DumbMotorIOInputsAutoLogged(); - private final DumbMotorIOInputsAutoLogged beltMotorInputs = new DumbMotorIOInputsAutoLogged(); + private final DumbMotorIO motor; + private final DumbMotorIOInputsAutoLogged inputs = new DumbMotorIOInputsAutoLogged(); - private final Alert wheelMotorAlert = - new Alert("Indexer Wheel Motor is diconnected", AlertType.kError); - private final Alert beltMotorAlert = - new Alert("Indexer Belt Motor is diconnected", AlertType.kError); + private final Alert motorAlert = new Alert("Indexer Motor is diconnected", AlertType.kError); - public Indexer(DumbMotorIO wheelMotor, DumbMotorIO beltMotor) { - this.beltMotor = beltMotor; + public Indexer(DumbMotorIO motor) { + this.motor = motor; setName("Indexer"); - setDefaultCommand( - run( - () -> { - beltMotor.runVoltage(0.0); - })); + setDefaultCommand(run(() -> motor.runVoltage(0.0))); } @Override public void periodic() { - beltMotor.updateInputs(beltMotorInputs); + motor.updateInputs(inputs); - Logger.processInputs("Indexer/BeltMotor", beltMotorInputs); + Logger.processInputs("Indexer/BeltMotor", inputs); - beltMotorAlert.set(!beltMotorInputs.connected); + motorAlert.set(!inputs.connected); ExecutionLogger.log("Indexer"); } - public Command load() { - return run(() -> { - beltMotor.runVoltage(IndexerConstants.RUN_VOLTAGE); - }) - .withName("Indexer Feed"); + public Command in() { + return run(() -> motor.runVoltage(IndexerConstants.RUN_VOLTAGE)).withName("Indexer Feed"); } - public Command feed() { - return run(() -> { - beltMotor.runTorqueCurrent(IndexerConstants.RUN_CURRENT); - }) - .withName("Indexer Feed"); + public Command out() { + return run(() -> motor.runVoltage(-IndexerConstants.RUN_VOLTAGE)).withName("Indexer Out"); } - public Command out() { - return run(() -> { - beltMotor.runVoltage(-IndexerConstants.RUN_VOLTAGE); - }) - .withName("Indexer Out"); + public Command pulseIn() { + return Commands.repeatingSequence( + in().withTimeout(0.25), stop().andThen(Commands.waitSeconds(0.25))); } public Command stop() { - return runOnce( - () -> { - beltMotor.runVoltage(0.0); - }) - .withName("Indexer Stop"); + return runOnce(() -> motor.runVoltage(0.0)).withName("Indexer Stop"); } } diff --git a/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java b/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java index 9dbc222..a7f09f6 100644 --- a/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java +++ b/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -24,14 +25,14 @@ public class Pivot extends SubsystemBase { private final SmartMotorIOInputsAutoLogged pivotMotorInputs = new SmartMotorIOInputsAutoLogged(); @AutoLogOutput(key = "Intake/Pivot/TargetAngle") - private double goal = 2.23; + private double goal = IntakeConstants.MIN_ANGLE; private final Alert pivotMotorAlert = new Alert("Pivot motor is disconnected!", AlertType.kError); public Pivot(SmartMotorIO pivotMotor) { this.pivotMotor = pivotMotor; setName("Intake/Pivot"); - setDefaultCommand(run(() -> pivotMotor.runVoltage(0.0))); + setDefaultCommand(holdAngle(goal)); } @Override @@ -53,6 +54,10 @@ public Command goToAngle(double angle) { .withName("Pivot Go To Angle"); } + public void freeze() { + goal = pivotMotorInputs.positionRad; + } + public Command runVoltage(double voltage) { return run(() -> pivotMotor.runVoltage(voltage)).withName("Pivot Run Voltage"); } @@ -64,4 +69,8 @@ public Command holdAngle(double angle) { public Command stop() { return runOnce(() -> pivotMotor.runVoltage(0.0)).withName("Pivot Stop"); } + + public Command agitate() { + return Commands.repeatingSequence(goToAngle(0.8), goToAngle(0.9)); + } } diff --git a/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java b/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java new file mode 100644 index 0000000..b5a7ecb --- /dev/null +++ b/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java @@ -0,0 +1,95 @@ +// Copyright (c) 2026 Team 2342 +// https://github.com/FRCTeamPhoenix +// +// This source code is licensed under the MIT License. +// See the LICENSE file in the root directory of this project. + +package org.team2342.frc.subsystems.leds; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import org.team2342.frc.subsystems.Conductor.ConductorState; +import org.team2342.lib.leds.LedIO; +import org.team2342.lib.leds.LedIO.Half; +import org.team2342.lib.leds.LedIO.LEDAnimation; +import org.team2342.lib.leds.LedIO.LEDEffect; +import org.team2342.lib.leds.LedIOInputsAutoLogged; +import org.team2342.lib.logging.ExecutionLogger; + +public class LEDSubsystem extends SubsystemBase { + private final LedIO io; + private final String name; + private final LedIOInputsAutoLogged inputs = new LedIOInputsAutoLogged(); + + private final BooleanSupplier hasTags; + private final Supplier stateSupplier; + + private boolean hasEnabled = false; + private Debouncer visionDebouncer = new Debouncer(0.5, DebounceType.kBoth); + + public LEDSubsystem( + LedIO io, String name, BooleanSupplier hasTags, Supplier stateSupplier) { + this.io = io; + this.name = name; + this.hasTags = hasTags; + this.stateSupplier = stateSupplier; + setName(name); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs(name, inputs); + if (DriverStation.isEStopped()) { + setAll(new LEDEffect(LEDAnimation.SOLID, Color.kBlack)); + return; + } + + if (DriverStation.isTeleopEnabled()) { + hasEnabled = true; + LEDEffect activeEffect = + switch (stateSupplier.get()) { + case TRACKED_FIRING -> new LEDEffect(LEDAnimation.SOLID, Color.kCyan); + case WARM_UP -> new LEDEffect(LEDAnimation.SOLID, Color.kYellow); + case TUNING -> new LEDEffect(LEDAnimation.SOLID, Color.kPurple); + default -> new LEDEffect(LEDAnimation.SOLID, Color.kOrange); + }; + setAll(activeEffect); + } else if (DriverStation.isAutonomousEnabled()) { + hasEnabled = true; + setAll(new LEDEffect(LEDAnimation.RAINBOW, Color.kWhite)); + } else { + if (!visionDebouncer.calculate(hasTags.getAsBoolean()) && !hasEnabled) { + setAll(new LEDEffect(LEDAnimation.SOLID, Color.kWhite)); + } else { + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().orElse(Alliance.Red) == Alliance.Red) { + setAll(new LEDEffect(LEDAnimation.LARSON, Color.kRed)); + } else { + setAll(new LEDEffect(LEDAnimation.LARSON, Color.kBlue)); + } + } + } + + ExecutionLogger.log(name); + } + + public void setFirst(LEDEffect effect) { + io.setEffect(LedIO.Half.FIRST, effect); + } + + public void setSecond(LEDEffect effect) { + io.setEffect(LedIO.Half.SECOND, effect); + } + + public void setAll(LEDEffect effect) { + io.setEffect(Half.ALL, effect); + } +} diff --git a/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java b/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java index 835b79d..0826551 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java @@ -30,6 +30,8 @@ public class Flywheel extends SubsystemBase { private final SysIdRoutine sysId; + private boolean atGoal; + public Flywheel(SmartMotorIO motor) { this.motor = motor; @@ -62,18 +64,24 @@ public void periodic() { public void runVelocity(double metersPerSec) { double radPerSec = metersPerSec / ShooterConstants.FLYWHEEL_RADIUS_METERS; + atGoal = + Math.abs(metersPerSec - getVelocityMetersPerSec()) + <= ShooterConstants.FLYWHEEL_AT_GOAL_TOLERANCE; Logger.recordOutput("Shooter/Flywheel/SetpointMetersPerSec", metersPerSec); motor.runVelocity(radPerSec); } public void runVelocity(DoubleSupplier metersPerSec) { double radPerSec = metersPerSec.getAsDouble() / ShooterConstants.FLYWHEEL_RADIUS_METERS; + atGoal = + Math.abs(radPerSec - motorInputs.velocityRadPerSec) + <= ShooterConstants.FLYWHEEL_AT_GOAL_TOLERANCE; Logger.recordOutput("Shooter/Flywheel/SetpointMetersPerSec", metersPerSec); motor.runVelocity(radPerSec); } - private void warmUp(double idleSpeed) { - if (getVelocityMetersPerSec() > idleSpeed) { + private void warmUp(DoubleSupplier idleSpeed) { + if (getVelocityMetersPerSec() > idleSpeed.getAsDouble()) { motor.runVoltage(0.0); } else { runVelocity(idleSpeed); @@ -88,8 +96,12 @@ public Command shoot(DoubleSupplier metersPerSec) { return run(() -> runVelocity(metersPerSec)).withName("Run Shooter"); } - public Command warmUp() { - return run(() -> warmUp(ShooterConstants.IDLE_SPEED)).withName("Warm Up Shooter"); + public Command warmUpFixed() { + return run(() -> warmUp(() -> ShooterConstants.IDLE_SPEED)).withName("Warm Up Shooter"); + } + + public Command warmUpCommand(DoubleSupplier metersPerSec) { + return run(() -> warmUp(metersPerSec)).withName("Warm Up Shooter"); } public Command runVoltage(double volts) { @@ -100,6 +112,10 @@ public Command stop() { return runOnce(() -> motor.runVoltage(0.0)).withName("Shooter Stop"); } + public boolean atGoal() { + return atGoal; + } + /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return run(() -> runVoltage(0.0)) diff --git a/src/main/java/org/team2342/frc/subsystems/kicker/Kicker.java b/src/main/java/org/team2342/frc/subsystems/shooter/Kicker.java similarity index 91% rename from src/main/java/org/team2342/frc/subsystems/kicker/Kicker.java rename to src/main/java/org/team2342/frc/subsystems/shooter/Kicker.java index 49dfafe..0095e55 100644 --- a/src/main/java/org/team2342/frc/subsystems/kicker/Kicker.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Kicker.java @@ -4,7 +4,7 @@ // This source code is licensed under the MIT License. // See the LICENSE file in the root directory of this project. -package org.team2342.frc.subsystems.kicker; +package org.team2342.frc.subsystems.shooter; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; @@ -20,8 +20,7 @@ public class Kicker extends SubsystemBase { private final DumbMotorIO kickerMotor; private final DumbMotorIOInputsAutoLogged kickerMotorInputs = new DumbMotorIOInputsAutoLogged(); - private final Alert kickerMotorAlert = - new Alert("Indexer Feeder Motor is diconnected", AlertType.kError); + private final Alert kickerMotorAlert = new Alert("Kicker Motor is diconnected", AlertType.kError); public Kicker(DumbMotorIO kickerMotor) { this.kickerMotor = kickerMotor; diff --git a/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java index a130571..90cbd21 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.team2342.frc.Constants.TurretConstants; @@ -29,6 +30,7 @@ public class Turret extends SubsystemBase { public Turret(SmartMotorIO turretMotor) { this.turretMotor = turretMotor; setName("Shooter/Turret"); + turretMotor.setPosition(TurretConstants.STARTING_ANGLE); setDefaultCommand(run(() -> turretMotor.runVoltage(0.0))); } @@ -46,17 +48,27 @@ public void runPosition(Rotation2d target) { turretMotor.runPosition(goal); } + public void runPosition(Supplier target) { + this.goal = calculateTurretAngle(target.get()); + turretMotor.runPosition(goal); + } + public Command runPositionCommand(Rotation2d target) { return run(() -> runPosition(target)).withName("Turret RunPosition"); } - public void goToPosition(Rotation2d target) { - this.goal = calculateTurretAngle(target); - turretMotor.runPosition(goal); + public Command runPositionCommand(Supplier target) { + return run(() -> runPosition(target)).withName("Turret RunPosition"); } public Command goToPositionCommand(Rotation2d target) { - return run(() -> goToPosition(target)) + return run(() -> runPosition(target)) + .until(() -> Math.abs(inputs.positionRad - goal) <= TurretConstants.AT_POSITION_THRESHOLD) + .withName("Turret GoToPosition"); + } + + public Command goToPositionCommand(Supplier target) { + return run(() -> runPosition(target)) .until(() -> Math.abs(inputs.positionRad - goal) <= TurretConstants.AT_POSITION_THRESHOLD) .withName("Turret GoToPosition"); } @@ -83,14 +95,18 @@ public Rotation2d getTurretSetpoint() { return new Rotation2d(goal); } + public boolean atGoal() { + return Math.abs(inputs.positionRad - goal) <= TurretConstants.AT_POSITION_THRESHOLD; + } + public void zeroTurret() { turretMotor.setPosition(0.0); } private double calculateTurretAngle(Rotation2d angle) { double calculatedAngle = MathUtil.inputModulus(angle.getRadians(), -Math.PI, Math.PI); - // if (calculatedAngle < 0) calculatedAngle += 2 * Math.PI; - // if (calculatedAngle > Math.PI * 2) calculatedAngle -= 2 * Math.PI; + if (calculatedAngle < 0) calculatedAngle += 2 * Math.PI; + if (calculatedAngle > Math.PI * 2) calculatedAngle -= 2 * Math.PI; return MathUtil.clamp( calculatedAngle, TurretConstants.MIN_TURRET_ANGLE, TurretConstants.MAX_TURRET_ANGLE); } diff --git a/src/main/java/org/team2342/frc/subsystems/vision/Vision.java b/src/main/java/org/team2342/frc/subsystems/vision/Vision.java index fbc086f..0e67477 100644 --- a/src/main/java/org/team2342/frc/subsystems/vision/Vision.java +++ b/src/main/java/org/team2342/frc/subsystems/vision/Vision.java @@ -34,6 +34,7 @@ public class Vision extends SubsystemBase { private final Alert[] disconnectedAlerts; private final Supplier> timestampedHeading; + private boolean hasTags = false; public Vision( VisionConsumer consumer, @@ -68,6 +69,10 @@ public Rotation2d getTargetX(int cameraIndex) { return inputs[cameraIndex].latestTargetObservation.tx(); } + public boolean hasTags() { + return hasTags; + } + @Override public void periodic() { Timestamped heading = timestampedHeading.get(); @@ -79,6 +84,8 @@ public void periodic() { Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); } + hasTags = false; + // Initialize logging values List allTagPoses = new LinkedList<>(); List allRobotPoses = new LinkedList<>(); @@ -90,6 +97,8 @@ public void periodic() { // Update disconnected alert disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + hasTags = hasTags || inputs[cameraIndex].poseObservations.length > 0; + // Initialize logging values List tagPoses = new LinkedList<>(); List robotPoses = new LinkedList<>(); diff --git a/src/main/java/org/team2342/frc/util/FieldConstants.java b/src/main/java/org/team2342/frc/util/FieldConstants.java index 579e44a..f9c9b8e 100644 --- a/src/main/java/org/team2342/frc/util/FieldConstants.java +++ b/src/main/java/org/team2342/frc/util/FieldConstants.java @@ -161,18 +161,22 @@ public static class RightBump { // Relevant reference points on alliance side public static final Translation2d nearLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + new Translation2d( + LinesVertical.hubCenter + width / 2, fieldWidth - Units.inchesToMeters(255)); public static final Translation2d nearRightCorner = Hub.nearLeftCorner; public static final Translation2d farLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + new Translation2d( + LinesVertical.hubCenter - width / 2, fieldWidth - Units.inchesToMeters(255)); public static final Translation2d farRightCorner = Hub.farLeftCorner; // Relevant reference points on opposing side public static final Translation2d oppNearLeftCorner = - new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + new Translation2d( + LinesVertical.hubCenter + width / 2, fieldWidth - Units.inchesToMeters(255)); public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; public static final Translation2d oppFarLeftCorner = - new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + new Translation2d( + LinesVertical.hubCenter - width / 2, fieldWidth - Units.inchesToMeters(255)); public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; } diff --git a/src/main/java/org/team2342/frc/util/FiringSolver.java b/src/main/java/org/team2342/frc/util/FiringSolver.java index ffd7eca..50f8c7f 100644 --- a/src/main/java/org/team2342/frc/util/FiringSolver.java +++ b/src/main/java/org/team2342/frc/util/FiringSolver.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import org.littletonrobotics.junction.Logger; import org.team2342.frc.Constants.TurretConstants; +import org.team2342.frc.RobotContainer; import org.team2342.lib.util.AllianceUtils; public class FiringSolver { @@ -21,35 +22,35 @@ public class FiringSolver { private static final int ITERATIONS = 5; - public static final FiringSolution BUMPER_SHOT = new FiringSolution(new Rotation2d(), 23.5, 0.0); + public static final double MIN_TOF, MAX_TOF; private FiringSolution lastSolution = null; - private static final InterpolatingDoubleTreeMap angleMap = new InterpolatingDoubleTreeMap(); private static final InterpolatingDoubleTreeMap speedMap = new InterpolatingDoubleTreeMap(); private static final InterpolatingDoubleTreeMap tofMap = new InterpolatingDoubleTreeMap(); // TODO: tune real maps static { - angleMap.put(1.859, 0.0); - angleMap.put(2.203, 0.0); - angleMap.put(2.510, 0.0); - angleMap.put(2.844, 0.0); - angleMap.put(3.046, 0.0); - angleMap.put(3.315, 0.0); - angleMap.put(3.973, 0.0); - angleMap.put(5.042, 0.0); - - speedMap.put(1.859, 23.0); - speedMap.put(2.203, 23.0); - speedMap.put(2.510, 25.0); - speedMap.put(2.844, 25.5); - speedMap.put(3.046, 26.0); - speedMap.put(3.315, 26.5); - speedMap.put(3.973, 28.5); - speedMap.put(5.042, 30.0); - - tofMap.put(1.859, 1.0); + speedMap.put(1.942, 14.2); + speedMap.put(2.712, 14.2); + speedMap.put(2.847, 15.0); + speedMap.put(3.065, 16.0); + speedMap.put(3.442, 17.0); + speedMap.put(3.8, 17.5); + speedMap.put(4.16, 19.0); + speedMap.put(4.341, 20.5); + + MIN_TOF = 3.89 - 3.08; + MAX_TOF = 1.0; + + tofMap.put(1.942, 3.89 - 3.08); + tofMap.put(2.712, 4.11 - 3.30); + tofMap.put(2.847, 2.28 - 1.24); + tofMap.put(3.065, 8.70 - 7.62); + tofMap.put(3.442, 3.95 - 2.83); + tofMap.put(3.8, 3.74 - 2.50); + tofMap.put(4.16, 12.27 - 10.91); + tofMap.put(4.341, 11.30 - 9.83); } public static FiringSolver getInstance() { @@ -64,6 +65,46 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { return lastSolution; } + boolean outsideAllianceZone = + !RobotContainer.withinBounds( + position.getX(), + AllianceUtils.flipToAlliance(Pose2d.kZero).getX(), + AllianceUtils.flipToAlliance(FieldConstants.LeftBump.nearLeftCorner).getX()); + + if (outsideAllianceZone) { + Pose2d turretPose = + new Pose3d(position).transformBy(TurretConstants.TURRET_TRANSFORM).toPose2d(); + + Translation2d turretTranslation = turretPose.getTranslation(); + + Translation2d leftBump = AllianceUtils.flipToAlliance(FieldConstants.LeftBump.nearLeftCorner); + + Translation2d rightBump = + AllianceUtils.flipToAlliance(FieldConstants.RightBump.farLeftCorner); + + Logger.recordOutput("FiringSolver/LeftTarget", new Pose2d(leftBump, Rotation2d.kZero)); + Logger.recordOutput("FiringSolver/RightTarget", new Pose2d(rightBump, Rotation2d.kZero)); + + Translation2d passTarget = + (turretTranslation.getDistance(leftBump) < turretTranslation.getDistance(rightBump) + ? leftBump + : rightBump); + + Logger.recordOutput("FiringSolver/Target", new Pose2d(passTarget, Rotation2d.kZero)); + + Rotation2d turretAngle = + passTarget + .minus(turretTranslation) + .getAngle() + .minus(position.getRotation()) + .minus(Rotation2d.kCCW_Pi_2); + + // TODO: tune real passing speed + lastSolution = new FiringSolution(turretAngle, 15.0, true); + + return lastSolution; + } + Translation2d hub = AllianceUtils.flipToAlliance(FieldConstants.Hub.topCenterPoint).toTranslation2d(); Pose2d turretPose = @@ -85,8 +126,10 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { Pose2d predictedPose = turretPose; double predictedDistance = hub.getDistance(turretPose.getTranslation()); + Logger.recordOutput("FiringSolver/Distance", predictedDistance); for (int i = 0; i < ITERATIONS; i++) { tof = tofMap.get(predictedDistance); + Logger.recordOutput("FiringSolver/PredictedTOF", tof); predictedPose = new Pose2d( turretPose.getTranslation().plus(new Translation2d(velX * tof, velY * tof)), @@ -96,13 +139,15 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { Logger.recordOutput("FiringSolver/PredictedPose", predictedPose); Logger.recordOutput("FiringSolver/PredictedDistance", predictedDistance); - Rotation2d turretAngle = hub.minus(predictedPose.getTranslation()).getAngle(); - turretAngle = turretAngle.minus(position.getRotation()).minus(Rotation2d.k180deg); + Rotation2d turretAngle = + hub.minus(predictedPose.getTranslation()) + .getAngle() + .minus(position.getRotation()) + .minus(Rotation2d.kCCW_Pi_2); - double hoodAngle = angleMap.get(predictedDistance); double wheelSpeed = speedMap.get(predictedDistance); - lastSolution = new FiringSolution(turretAngle, wheelSpeed, hoodAngle); + lastSolution = new FiringSolution(turretAngle, wheelSpeed, false); return lastSolution; } @@ -111,5 +156,5 @@ public void clearCachedSolution() { lastSolution = null; } - public record FiringSolution(Rotation2d turretAngle, double wheelSpeed, double hoodAngle) {} + public record FiringSolution(Rotation2d turretAngle, double wheelSpeed, boolean passing) {} } diff --git a/src/main/java/org/team2342/frc/util/HubShiftUtil.java b/src/main/java/org/team2342/frc/util/HubShiftUtil.java new file mode 100644 index 0000000..84c8099 --- /dev/null +++ b/src/main/java/org/team2342/frc/util/HubShiftUtil.java @@ -0,0 +1,190 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.team2342.frc.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; + +public class HubShiftUtil { + + public enum ShiftEnum { + TRANSITION, + SHIFT1, + SHIFT2, + SHIFT3, + SHIFT4, + ENDGAME, + AUTO, + DISABLED; + } + + public record ShiftInfo( + ShiftEnum currentShift, double elapsedTime, double remainingTime, boolean active) {} + + private static Timer shiftTimer = new Timer(); + private static final ShiftEnum[] shiftsEnums = ShiftEnum.values(); + + private static final double[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + private static final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + + private static final double minFuelCountDelay = 1.0; + private static final double maxFuelCountDelay = 2.0; + private static final double shiftEndFuelCountExtension = 3.0; + private static final double minTimeOfFlight = FiringSolver.MIN_TOF; + private static final double maxTimeOfFlight = FiringSolver.MAX_TOF; + private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay); + private static final double endingActiveFudge = + shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); + + public static final double autoEndTime = 20.0; + public static final double teleopDuration = 140.0; + private static final boolean[] activeSchedule = {true, true, false, true, false, true}; + private static final boolean[] inactiveSchedule = {true, false, true, false, true, true}; + private static final double timeResetThreshold = 3.0; + private static double shiftTimerOffset = 0.0; + + public static Alliance getFirstActiveAlliance() { + var alliance = DriverStation.getAlliance().orElse(Alliance.Red); + + // Return FMS value + String message = DriverStation.getGameSpecificMessage(); + if (message.length() > 0) { + char character = message.charAt(0); + if (character == 'R') { + return Alliance.Blue; + } else if (character == 'B') { + return Alliance.Red; + } + } + + // Return default value + return alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue; + } + + /** Starts the timer at the begining of teleop. */ + public static void initialize() { + shiftTimerOffset = 0; + shiftTimer.restart(); + } + + private static boolean[] getSchedule() { + boolean[] currentSchedule; + Alliance startAlliance = getFirstActiveAlliance(); + currentSchedule = + startAlliance == DriverStation.getAlliance().orElse(Alliance.Blue) + ? activeSchedule + : inactiveSchedule; + return currentSchedule; + } + + private static ShiftInfo getShiftInfo( + boolean[] currentSchedule, double[] shiftStartTimes, double[] shiftEndTimes) { + double timerValue = shiftTimer.get(); + double currentTime = timerValue - shiftTimerOffset; + double stateTimeElapsed = currentTime; + double stateTimeRemaining = 0.0; + boolean active = false; + ShiftEnum currentShift = ShiftEnum.DISABLED; + double fieldTeleopTime = 140.0 - DriverStation.getMatchTime(); + + if (DriverStation.isAutonomousEnabled()) { + stateTimeElapsed = currentTime; + stateTimeRemaining = autoEndTime - currentTime; + active = true; + currentShift = ShiftEnum.AUTO; + } else if (DriverStation.isEnabled()) { + // Adjust the current offset if the time difference above the theshold + if (Math.abs(fieldTeleopTime - currentTime) >= timeResetThreshold + && fieldTeleopTime <= 135 + && DriverStation.isFMSAttached()) { + shiftTimerOffset += currentTime - fieldTeleopTime; + currentTime = timerValue + shiftTimerOffset; + } + int currentShiftIndex = -1; + for (int i = 0; i < shiftStartTimes.length; i++) { + if (currentTime >= shiftStartTimes[i] && currentTime < shiftEndTimes[i]) { + currentShiftIndex = i; + break; + } + } + if (currentShiftIndex < 0) { + // After last shift, so assume endgame + currentShiftIndex = shiftStartTimes.length - 1; + } + + // Calculate elapsed and remaining time in the current shift, ignoring combined shifts + stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex]; + stateTimeRemaining = shiftEndTimes[currentShiftIndex] - currentTime; + + // If the state is the same as the last shift, combine the elapsed time + if (currentShiftIndex > 0) { + if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex - 1]) { + stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex - 1]; + } + } + + // If the state is the same as the next shift, combine the remaining time + if (currentShiftIndex < shiftEndTimes.length - 1) { + if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex + 1]) { + stateTimeRemaining = shiftEndTimes[currentShiftIndex + 1] - currentTime; + } + } + + active = currentSchedule[currentShiftIndex]; + currentShift = shiftsEnums[currentShiftIndex]; + } + ShiftInfo shiftInfo = new ShiftInfo(currentShift, stateTimeElapsed, stateTimeRemaining, active); + return shiftInfo; + } + + public static ShiftInfo getOfficialShiftInfo() { + return getShiftInfo(getSchedule(), shiftStartTimes, shiftEndTimes); + } + + public static ShiftInfo getShiftedShiftInfo() { + boolean[] shiftSchedule = getSchedule(); + // Starting active + if (shiftSchedule[1] == true) { + double[] shiftedShiftStartTimes = { + 0.0, + 10.0, + 35.0 + endingActiveFudge, + 60.0 + approachingActiveFudge, + 85.0 + endingActiveFudge, + 110.0 + approachingActiveFudge + }; + double[] shiftedShiftEndTimes = { + 10.0, + 35.0 + endingActiveFudge, + 60.0 + approachingActiveFudge, + 85.0 + endingActiveFudge, + 110.0 + approachingActiveFudge, + 140.0 + }; + return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); + } + double[] shiftedShiftStartTimes = { + 0.0, + 10.0 + endingActiveFudge, + 35.0 + approachingActiveFudge, + 60.0 + endingActiveFudge, + 85.0 + approachingActiveFudge, + 110.0 + }; + double[] shiftedShiftEndTimes = { + 10.0 + endingActiveFudge, + 35.0 + approachingActiveFudge, + 60.0 + endingActiveFudge, + 85.0 + approachingActiveFudge, + 110.0, + 140.0 + }; + return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); + } +} diff --git a/src/main/java/org/team2342/lib b/src/main/java/org/team2342/lib index 6c362e3..90d6efc 160000 --- a/src/main/java/org/team2342/lib +++ b/src/main/java/org/team2342/lib @@ -1 +1 @@ -Subproject commit 6c362e39189b227918ae923db2404b37c2f5c8a1 +Subproject commit 90d6efce9c4bedbf1580d897b97b19a1f9c3c73f