From 1fc7b6f0ade942cf8e4d6a3a19a4c53eb623251a Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Sat, 7 Mar 2026 13:10:28 -0500 Subject: [PATCH 01/16] modifying conductor --- ...ducam_800.json => swerve_arducam_800.json} | 0 src/main/java/org/team2342/frc/Constants.java | 93 ++++------- src/main/java/org/team2342/frc/Robot.java | 2 + .../java/org/team2342/frc/RobotContainer.java | 147 ++++-------------- .../team2342/frc/subsystems/Conductor.java | 122 ++++++--------- .../frc/subsystems/indexer/Indexer.java | 51 ++---- .../frc/subsystems/shooter/Flywheel.java | 2 +- .../{kicker => shooter}/Kicker.java | 5 +- .../frc/subsystems/shooter/Turret.java | 19 ++- .../org/team2342/frc/util/FiringSolver.java | 33 ++-- src/main/java/org/team2342/lib | 2 +- 11 files changed, 149 insertions(+), 327 deletions(-) rename src/main/deploy/calibrations/{left_arducam_800.json => swerve_arducam_800.json} (100%) rename src/main/java/org/team2342/frc/subsystems/{kicker => shooter}/Kicker.java (91%) 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/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 6d83b54..b1dda5e 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -19,13 +19,12 @@ import org.team2342.lib.motors.MotorConfig.IdleMode; import org.team2342.lib.motors.smart.SmartMotorConfig; import org.team2342.lib.motors.smart.SmartMotorConfig.ControlType; -import org.team2342.lib.motors.smart.SmartMotorConfig.FeedbackConfig; import org.team2342.lib.pidff.PIDFFConfigs; import org.team2342.lib.util.CameraParameters; 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 +133,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_CURRENT = 30.0; - public static final double FEEDER_CURRENT = 30.0; - public static final MotorConfig INDEXER_BELT_CONFIG = - new MotorConfig() - .withMotorInverted(false) - .withSupplyCurrentLimit(30.0) - .withStatorCurrentLimit(70.0) - .withIdleMode(MotorConfig.IdleMode.BRAKE); - public static final MotorConfig INDEXER_FEEDER_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 DCMotor INDEXER_BELT_SIM_MOTOR = DCMotor.getKrakenX60(1); - public static final DCMotorSim INDEXER_BELT_SIM = + public static final DCMotor INDEXER_SIM_MOTOR = DCMotor.getKrakenX60(1); + public static final DCMotorSim INDEXER_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 = - 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,7 +193,7 @@ 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; @@ -230,14 +202,15 @@ public static final class ShooterConstants { new PIDFFConfigs() .withKP(0.5) .withKI(0.0) - .withKS(Units.radiansToRotations(0.1824)) - .withKV(Units.radiansToRotations(0.020077)) - .withKA(Units.radiansToRotations(0.0037398)); + .withKS(Units.radiansToRotations(0.10346)) + .withKV(Units.radiansToRotations(0.057928)) + .withKA(Units.radiansToRotations(0.0080087)); + 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,37 +224,26 @@ 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 = 8.0; - 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; @@ -290,7 +252,7 @@ public static final class TurretConstants { public static final double MIN_TURRET_ANGLE = Units.degreesToRadians(-90); public static final double MAX_TURRET_ANGLE = Units.degreesToRadians(90); - 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() @@ -317,15 +279,14 @@ 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 HOOD_MOTOR_ID = 15; // Kicker motor ID + public static final int HOOD_ENCODER_ID = 16; // Turret ID 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 INDEXER_MOTOR_ID = 20; public static final int KICKER_ID = 22; diff --git a/src/main/java/org/team2342/frc/Robot.java b/src/main/java/org/team2342/frc/Robot.java index 0bafef0..923f566 100644 --- a/src/main/java/org/team2342/frc/Robot.java +++ b/src/main/java/org/team2342/frc/Robot.java @@ -158,6 +158,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); ExecutionLogger.log("Commands"); + robotContainer.getFlywheel().runVoltage(12); + robotContainer.updateAlerts(); FiringSolver.getInstance().clearCachedSolution(); diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 411623e..13390e8 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -10,7 +10,6 @@ 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.PowerDistribution.ModuleType; @@ -24,10 +23,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; @@ -45,16 +44,16 @@ import org.team2342.frc.subsystems.intake.Pivot; import org.team2342.frc.subsystems.intake.Wheels; 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; import org.team2342.frc.subsystems.vision.VisionIOPhoton; import org.team2342.frc.subsystems.vision.VisionIOSim; import org.team2342.frc.util.FieldConstants; -import org.team2342.frc.util.FiringSolver; import org.team2342.lib.motors.dumb.DumbMotorIO; import org.team2342.lib.motors.dumb.DumbMotorIOSim; +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,9 +66,9 @@ 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 Conductor conductor; @@ -89,7 +88,6 @@ public class RobotContainer { private final Alert operatorControllerAlert = new Alert("Operator controller is disconnected!", AlertType.kError); - private final Trigger trenchTrigger; private final Trigger allianceZoneTrigger; public RobotContainer() { @@ -108,26 +106,25 @@ 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() {}); - pivot = - new Pivot( + flywheel = + new Flywheel( 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)); + 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 DumbMotorIO() {}); + wheels = new Wheels(new DumbMotorIO() {}); LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev); break; @@ -157,10 +154,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,14 +167,7 @@ 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() {}); @@ -200,17 +187,17 @@ 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() {}); break; } - conductor = new Conductor(flywheel, hood, drive::getPose, drive::getChassisSpeeds); + conductor = new Conductor(flywheel, turret, kicker, drive::getPose, drive::getChassisSpeeds); configureNamedCommands(); @@ -219,54 +206,16 @@ public RobotContainer() { 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", 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())))); + .alongWith(indexer.in()))); SmartDashboard.putData( "Calculate Vision Heading Offset", @@ -274,23 +223,13 @@ 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())); configureBindings(); } @@ -323,50 +262,26 @@ private void configureBindings() { driverController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); // Shooting Controls - driverController - .rightBumper() - .whileTrue( - conductor - .goToState(ConductorState.BUMPER_SHOT) - .andThen(conductor.runState(ConductorState.BUMPER_SHOT).alongWith(indexer.feed()))) - .onFalse(indexer.stop()); - 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()); + .goToState(ConductorState.TUNING) + .andThen(conductor.runState(ConductorState.TUNING))); + + driverController.a().whileTrue(flywheel.runVoltage(12)).whileFalse(flywheel.stop()); // Operator Overrides - operatorController.povRight().whileTrue(indexer.feed()).onFalse(indexer.stop()); + operatorController.povRight().whileTrue(indexer.in()).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)); - // 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)); + // allianceZoneTrigger + // .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) + // .whileTrue(conductor.runState(ConductorState.WARM_UP)); } public Command getAutonomousCommand() { diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index 7b3e666..1b5a3f6 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -9,26 +9,30 @@ 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.Kicker; +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 +44,21 @@ public enum ConductorState { ConductorState.class); private final Flywheel flywheel; - private final Hood hood; + private final Turret turret; + private final Kicker kicker; private final Supplier poseSupplier; private final Supplier velocitySupplier; public Conductor( Flywheel flywheel, - Hood hood, + Turret turret, + Kicker kicker, Supplier poseSupplier, Supplier velocitySupplier) { this.flywheel = flywheel; - this.hood = hood; + this.turret = turret; + this.kicker = kicker; this.poseSupplier = poseSupplier; this.velocitySupplier = velocitySupplier; @@ -59,7 +66,7 @@ public Conductor( setupStateCommands(); setupTransitions(); - enable(); + fsm.enable(); setDefaultCommand(runState(ConductorState.DISABLED)); } @@ -83,103 +90,66 @@ 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()))); + ConductorState.DISABLED, turret.stop().alongWith(flywheel.stop()).alongWith(kicker.stop())); fsm.addStateCommand( ConductorState.WARM_UP, - hood.holdAngle( + turret + .runPositionCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .deadlineFor(flywheel.warmUp())); + .turretAngle()) + .alongWith(flywheel.warmUp()) + .alongWith(kicker.stop())); fsm.addStateCommand( ConductorState.TRACKED_FIRING, - hood.holdAngle( + turret + .goToPositionCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .deadlineFor( + .turretAngle()) + .alongWith( flywheel.shoot( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) - .wheelSpeed()))); - - fsm.addStateCommand(ConductorState.OVERRIDE_25, flywheel.shoot(25.0)); + .wheelSpeed())) + .alongWith(kicker.in())); - 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( + fsm.addStateCommand( + ConductorState.TUNING, + turret + .goToPositionCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .withTimeout(0.5) - .deadlineFor(flywheel.warmUp())); + .turretAngle()) + .alongWith(flywheel.shoot(flywheelSpeed)) + .alongWith(kicker.in())); + } - fsm.addOmniTransition( - ConductorState.TRACKED_FIRING, - hood.holdAngle( - () -> - FiringSolver.getInstance() - .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .withTimeout(0.5) - .deadlineFor( - flywheel.shoot( - () -> - FiringSolver.getInstance() - .calculate(velocitySupplier.get(), poseSupplier.get()) - .wheelSpeed()))); - fsm.addOmniTransition( - ConductorState.OVERRIDE_25, - hood.holdAngle( - () -> - FiringSolver.getInstance() - .calculate(velocitySupplier.get(), poseSupplier.get()) - .hoodAngle()) - .withTimeout(0.5) - .deadlineFor(flywheel.shoot(25.0))); + public Command disable() { + return Commands.runOnce(() -> fsm.disable()); + } - fsm.addOmniTransition(ConductorState.OVERRIDE_23, flywheel.shoot(23.0)); + public Command enable() { + return Commands.runOnce(() -> fsm.enable()); } - public Command makeShooterCommand(double flywheelMetersPerSec, double hoodAngle) { - return hood.holdAngle(hoodAngle).alongWith(flywheel.shoot(flywheelMetersPerSec)); + private void setupTransitions() { + fsm.addOmniTransition(ConductorState.DISABLED); + fsm.addOmniTransition(ConductorState.TRACKED_FIRING); + fsm.addOmniTransition(ConductorState.WARM_UP); + fsm.addOmniTransition(ConductorState.TUNING); } public interface FSMDelegate { String dot(); - void enable(); - - void disable(); - 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..dca8844 100644 --- a/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team2342/frc/subsystems/indexer/Indexer.java @@ -17,63 +17,38 @@ 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 feed() { - return run(() -> { - beltMotor.runTorqueCurrent(IndexerConstants.RUN_CURRENT); - }) - .withName("Indexer Feed"); + public Command in() { + return run(() -> motor.runVoltage(IndexerConstants.RUN_VOLTAGE)).withName("Indexer Feed"); } public Command out() { - return run(() -> { - beltMotor.runVoltage(-IndexerConstants.RUN_VOLTAGE); - }) - .withName("Indexer Out"); + return run(() -> motor.runVoltage(-IndexerConstants.RUN_VOLTAGE)).withName("Indexer Out"); } 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/shooter/Flywheel.java b/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java index 835b79d..08b7fd4 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java @@ -47,7 +47,7 @@ public Flywheel(SmartMotorIO motor) { this)); setName("Shooter/Flywheel"); - setDefaultCommand(run(() -> motor.runVoltage(0.0))); + // setDefaultCommand(run(() -> motor.runVoltage(0.0))); } @Override 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..1729806 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; @@ -46,17 +47,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"); } diff --git a/src/main/java/org/team2342/frc/util/FiringSolver.java b/src/main/java/org/team2342/frc/util/FiringSolver.java index ffd7eca..b437c2a 100644 --- a/src/main/java/org/team2342/frc/util/FiringSolver.java +++ b/src/main/java/org/team2342/frc/util/FiringSolver.java @@ -21,33 +21,23 @@ 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 FiringSolution BUMPER_SHOT = new FiringSolution(new Rotation2d(), 23.5); 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); + speedMap.put(1.859, 15.0); + speedMap.put(2.203, 15.0); + speedMap.put(2.510, 17.0); + speedMap.put(2.844, 18.5); + speedMap.put(3.046, 19.0); + speedMap.put(3.315, 20.5); + speedMap.put(3.973, 20.5); + speedMap.put(5.042, 20.0); tofMap.put(1.859, 1.0); } @@ -99,10 +89,9 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { Rotation2d turretAngle = hub.minus(predictedPose.getTranslation()).getAngle(); turretAngle = turretAngle.minus(position.getRotation()).minus(Rotation2d.k180deg); - double hoodAngle = angleMap.get(predictedDistance); double wheelSpeed = speedMap.get(predictedDistance); - lastSolution = new FiringSolution(turretAngle, wheelSpeed, hoodAngle); + lastSolution = new FiringSolution(turretAngle, wheelSpeed); return lastSolution; } @@ -111,5 +100,5 @@ public void clearCachedSolution() { lastSolution = null; } - public record FiringSolution(Rotation2d turretAngle, double wheelSpeed, double hoodAngle) {} + public record FiringSolution(Rotation2d turretAngle, double wheelSpeed) {} } 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 From 7698ff74d38f6be565e0d07f4187ca36d10c4b07 Mon Sep 17 00:00:00 2001 From: abi-appusamy9932 Date: Sat, 7 Mar 2026 21:42:54 -0500 Subject: [PATCH 02/16] adding logic for passing --- .../java/org/team2342/frc/RobotContainer.java | 2 +- .../org/team2342/frc/util/FiringSolver.java | 36 +++++++++++++++++++ 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 13390e8..4a3948d 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -325,7 +325,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/util/FiringSolver.java b/src/main/java/org/team2342/frc/util/FiringSolver.java index b437c2a..f2edfb7 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 { @@ -54,6 +55,41 @@ 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.nearRightCorner); + + Translation2d passTarget = + (turretTranslation.getDistance(leftBump) < turretTranslation.getDistance(rightBump) + ? leftBump + : rightBump); + + Rotation2d turretAngle = + passTarget + .minus(turretTranslation) + .getAngle() + .minus(position.getRotation()) + .minus(Rotation2d.k180deg); + + // TODO: tune real passing speed + lastSolution = new FiringSolution(turretAngle, 15.0); + + return lastSolution; + } + Translation2d hub = AllianceUtils.flipToAlliance(FieldConstants.Hub.topCenterPoint).toTranslation2d(); Pose2d turretPose = From ed3676dedcc5efbebaf612529e14d3e328f90d27 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Sun, 8 Mar 2026 14:16:42 -0400 Subject: [PATCH 03/16] tuning flywheel + starting interpolation table work --- src/main/java/org/team2342/frc/Constants.java | 16 ++++++------ .../java/org/team2342/frc/RobotContainer.java | 25 +++++++++++-------- .../team2342/frc/subsystems/Conductor.java | 8 +++--- .../frc/subsystems/shooter/Flywheel.java | 2 +- .../frc/subsystems/shooter/Turret.java | 5 ++-- .../org/team2342/frc/util/FiringSolver.java | 25 +++++++++---------- 6 files changed, 44 insertions(+), 37 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index b1dda5e..2a69cf3 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -200,11 +200,11 @@ public static final class ShooterConstants { public static final PIDFFConfigs FLYWHEEL_PID_CONFIGS = new PIDFFConfigs() - .withKP(0.5) - .withKI(0.0) - .withKS(Units.radiansToRotations(0.10346)) - .withKV(Units.radiansToRotations(0.057928)) - .withKA(Units.radiansToRotations(0.0080087)); + .withKP(0.625) + .withKI(0.4) + .withKS(0.15989) + .withKV(0.019968) + .withKA(0.005977); public static final SmartMotorConfig FLYWHEEL_CONFIG = new SmartMotorConfig() @@ -248,9 +248,9 @@ public static final class TurretConstants { 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 = (48.0 / 12.0) * (100.0 / 10.0); diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 4a3948d..6561ce6 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -51,6 +51,8 @@ import org.team2342.frc.subsystems.vision.VisionIOPhoton; import org.team2342.frc.subsystems.vision.VisionIOSim; import org.team2342.frc.util.FieldConstants; +import org.team2342.lib.leds.LedIO; +import org.team2342.lib.leds.LedStrip; import org.team2342.lib.motors.dumb.DumbMotorIO; import org.team2342.lib.motors.dumb.DumbMotorIOSim; import org.team2342.lib.motors.dumb.DumbMotorIOTalonFXFOC; @@ -70,6 +72,7 @@ public class RobotContainer { @Getter private final Wheels wheels; @Getter private final Flywheel flywheel; @Getter private final Turret turret; + @Getter private final LedStrip leds; @Getter private final Conductor conductor; @@ -126,6 +129,8 @@ public RobotContainer() { indexer = new Indexer(new DumbMotorIO() {}); wheels = new Wheels(new DumbMotorIO() {}); + leds = new LedStrip(new LedIO() {}, "CANdle"); + LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev); break; @@ -171,6 +176,8 @@ public RobotContainer() { turret = new Turret(new SmartMotorIO() {}); pivot = new Pivot(new SmartMotorIO() {}); + leds = new LedStrip(new LedIO() {}, "CANdle"); + break; default: @@ -194,10 +201,13 @@ public RobotContainer() { turret = new Turret(new SmartMotorIO() {}); kicker = new Kicker(new DumbMotorIO() {}); + leds = new LedStrip(new LedIO() {}, "CANdle"); + break; } - conductor = new Conductor(flywheel, turret, kicker, drive::getPose, drive::getChassisSpeeds); + conductor = + new Conductor(flywheel, turret, kicker, leds, drive::getPose, drive::getChassisSpeeds); configureNamedCommands(); @@ -262,12 +272,7 @@ private void configureBindings() { driverController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); // Shooting Controls - driverController - .rightTrigger() - .whileTrue( - conductor - .goToState(ConductorState.TUNING) - .andThen(conductor.runState(ConductorState.TUNING))); + driverController.rightTrigger().whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)); driverController.a().whileTrue(flywheel.runVoltage(12)).whileFalse(flywheel.stop()); @@ -279,9 +284,9 @@ private void configureBindings() { operatorController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); // Location Triggers - // allianceZoneTrigger - // .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) - // .whileTrue(conductor.runState(ConductorState.WARM_UP)); + allianceZoneTrigger + .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) + .whileTrue(conductor.runState(ConductorState.WARM_UP)); } public Command getAutonomousCommand() { diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index 1b5a3f6..59b338d 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -19,6 +19,7 @@ import org.team2342.frc.subsystems.shooter.Turret; import org.team2342.frc.util.FiringSolver; import org.team2342.lib.fsm.StateMachine; +import org.team2342.lib.leds.LedStrip; import org.team2342.lib.logging.ExecutionLogger; import org.team2342.lib.logging.tunable.TunableNumber; @@ -54,6 +55,7 @@ public Conductor( Flywheel flywheel, Turret turret, Kicker kicker, + LedStrip leds, Supplier poseSupplier, Supplier velocitySupplier) { this.flywheel = flywheel; @@ -78,7 +80,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) { @@ -107,7 +109,7 @@ private void setupStateCommands() { fsm.addStateCommand( ConductorState.TRACKED_FIRING, turret - .goToPositionCommand( + .runPositionCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) @@ -123,7 +125,7 @@ private void setupStateCommands() { fsm.addStateCommand( ConductorState.TUNING, turret - .goToPositionCommand( + .runPositionCommand( () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) 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 08b7fd4..835b79d 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java @@ -47,7 +47,7 @@ public Flywheel(SmartMotorIO motor) { this)); setName("Shooter/Flywheel"); - // setDefaultCommand(run(() -> motor.runVoltage(0.0))); + setDefaultCommand(run(() -> motor.runVoltage(0.0))); } @Override 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 1729806..35ef28f 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java @@ -30,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))); } @@ -100,8 +101,8 @@ public void zeroTurret() { 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/util/FiringSolver.java b/src/main/java/org/team2342/frc/util/FiringSolver.java index f2edfb7..dfba3f1 100644 --- a/src/main/java/org/team2342/frc/util/FiringSolver.java +++ b/src/main/java/org/team2342/frc/util/FiringSolver.java @@ -31,16 +31,11 @@ public class FiringSolver { // TODO: tune real maps static { - speedMap.put(1.859, 15.0); - speedMap.put(2.203, 15.0); - speedMap.put(2.510, 17.0); - speedMap.put(2.844, 18.5); - speedMap.put(3.046, 19.0); - speedMap.put(3.315, 20.5); - speedMap.put(3.973, 20.5); - speedMap.put(5.042, 20.0); - - tofMap.put(1.859, 1.0); + speedMap.put(1.141, 15.0); + speedMap.put(1.445, 17.0); + + tofMap.put(1.141, 12.66 - 11.46); + tofMap.put(1.445, 8.34 - 6.95); } public static FiringSolver getInstance() { @@ -82,7 +77,7 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { .minus(turretTranslation) .getAngle() .minus(position.getRotation()) - .minus(Rotation2d.k180deg); + .minus(Rotation2d.kCCW_Pi_2); // TODO: tune real passing speed lastSolution = new FiringSolution(turretAngle, 15.0); @@ -111,6 +106,7 @@ 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); predictedPose = @@ -122,8 +118,11 @@ 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 wheelSpeed = speedMap.get(predictedDistance); From 0d92adef5453294cd17178cda2c1e5a27883c6b2 Mon Sep 17 00:00:00 2001 From: abi-appusamy9932 Date: Mon, 9 Mar 2026 21:33:15 -0400 Subject: [PATCH 04/16] adding leds for states added leds for firing, warming up, and tuning (temporary) --- .../java/org/team2342/frc/subsystems/Conductor.java | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index 59b338d..ab04d9f 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -22,6 +22,8 @@ import org.team2342.lib.leds.LedStrip; import org.team2342.lib.logging.ExecutionLogger; import org.team2342.lib.logging.tunable.TunableNumber; +import org.team2342.lib.leds.LedIO.LEDAnimation; +import edu.wpi.first.wpilibj.util.Color; public class Conductor extends SubsystemBase { @@ -47,6 +49,7 @@ public enum ConductorState { private final Flywheel flywheel; private final Turret turret; private final Kicker kicker; + private final LedStrip leds; private final Supplier poseSupplier; private final Supplier velocitySupplier; @@ -61,6 +64,7 @@ public Conductor( this.flywheel = flywheel; this.turret = turret; this.kicker = kicker; + this.leds = leds; this.poseSupplier = poseSupplier; this.velocitySupplier = velocitySupplier; @@ -104,7 +108,8 @@ private void setupStateCommands() { .calculate(velocitySupplier.get(), poseSupplier.get()) .turretAngle()) .alongWith(flywheel.warmUp()) - .alongWith(kicker.stop())); + .alongWith(kicker.stop()) + .alongWith(leds.setAllCommand(LEDAnimation.SOLID, Color.kRed))); fsm.addStateCommand( ConductorState.TRACKED_FIRING, @@ -120,7 +125,8 @@ private void setupStateCommands() { FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) .wheelSpeed())) - .alongWith(kicker.in())); + .alongWith(kicker.in()) + .alongWith(leds.setAllCommand(LEDAnimation.SOLID, Color.kOrange))); fsm.addStateCommand( ConductorState.TUNING, @@ -131,7 +137,8 @@ private void setupStateCommands() { .calculate(velocitySupplier.get(), poseSupplier.get()) .turretAngle()) .alongWith(flywheel.shoot(flywheelSpeed)) - .alongWith(kicker.in())); + .alongWith(kicker.in()) + .alongWith(leds.setAllCommand(LEDAnimation.SOLID, Color.kPurple))); } public Command disable() { From 33658404395a42034d379948c2456c80d86e26b8 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Mon, 9 Mar 2026 20:09:32 -0400 Subject: [PATCH 05/16] more pid tuning --- src/main/java/org/team2342/frc/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 2a69cf3..ab56bde 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -200,8 +200,8 @@ public static final class ShooterConstants { public static final PIDFFConfigs FLYWHEEL_PID_CONFIGS = new PIDFFConfigs() - .withKP(0.625) - .withKI(0.4) + .withKP(0.575) + .withKI(0.6) .withKS(0.15989) .withKV(0.019968) .withKA(0.005977); From ef9fff8ebcc8fcff5afb24ccae2708d4e6435bba Mon Sep 17 00:00:00 2001 From: abi-appusamy9932 Date: Wed, 11 Mar 2026 18:41:18 -0400 Subject: [PATCH 06/16] making paths for auto --- .../deploy/pathplanner/autos/LeftTrench.auto | 31 +++++++ .../deploy/pathplanner/autos/RightTrench.auto | 31 +++++++ src/main/deploy/pathplanner/paths/L1.path | 54 ++++++++++++ src/main/deploy/pathplanner/paths/L2.path | 86 +++++++++++++++++++ src/main/deploy/pathplanner/paths/L3.path | 86 +++++++++++++++++++ src/main/deploy/pathplanner/paths/R1.path | 54 ++++++++++++ src/main/deploy/pathplanner/paths/R2.path | 86 +++++++++++++++++++ src/main/deploy/pathplanner/paths/R3.path | 86 +++++++++++++++++++ 8 files changed, 514 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/LeftTrench.auto create mode 100644 src/main/deploy/pathplanner/autos/RightTrench.auto create mode 100644 src/main/deploy/pathplanner/paths/L1.path create mode 100644 src/main/deploy/pathplanner/paths/L2.path create mode 100644 src/main/deploy/pathplanner/paths/L3.path create mode 100644 src/main/deploy/pathplanner/paths/R1.path create mode 100644 src/main/deploy/pathplanner/paths/R2.path create mode 100644 src/main/deploy/pathplanner/paths/R3.path diff --git a/src/main/deploy/pathplanner/autos/LeftTrench.auto b/src/main/deploy/pathplanner/autos/LeftTrench.auto new file mode 100644 index 0000000..80cc7ab --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftTrench.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "L1" + } + }, + { + "type": "path", + "data": { + "pathName": "L2" + } + }, + { + "type": "path", + "data": { + "pathName": "L3" + } + } + ] + } + }, + "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..25eab6c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RightTrench.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R1" + } + }, + { + "type": "path", + "data": { + "pathName": "R2" + } + }, + { + "type": "path", + "data": { + "pathName": "R3" + } + } + ] + } + }, + "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..b1a1afb --- /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.412672784962453, + "y": 4.729036049442378 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.282, + "y": 5.969204819277109 + }, + "prevControl": { + "x": 2.0890855281730722, + "y": 6.128214273586397 + }, + "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": -42.27368900609373 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -29.16761337957779 + }, + "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..11ffc44 --- /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.784139256412005, + "y": 6.467901003200028 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8224216867469876, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 3.2389540480080994, + "y": 7.311855883645149 + }, + "nextControl": { + "x": 4.068842444662846, + "y": 7.453814875551677 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.898686746987952, + "y": 7.411662650602409 + }, + "prevControl": { + "x": 4.756795135625847, + "y": 7.423892570078729 + }, + "nextControl": { + "x": 6.568075333666703, + "y": 7.404493346237625 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.221, + "y": 6.078 + }, + "prevControl": { + "x": 6.938337410220809, + "y": 6.448604687790133 + }, + "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": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -39.497 + }, + "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..d767362 --- /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.409770391442066, + "y": 5.805185542220385 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8440481927710834, + "y": 7.378879518072289 + }, + "prevControl": { + "x": 6.453365128252008, + "y": 7.130662897315942 + }, + "nextControl": { + "x": 5.611419730119451, + "y": 7.473645062124593 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2978915662650605, + "y": 7.171253012048193 + }, + "prevControl": { + "x": 3.7104254130968934, + "y": 7.448500201429967 + }, + "nextControl": { + "x": 2.5876965838813497, + "y": 6.693959901940825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.587590361445783, + "y": 5.269831325301205 + }, + "prevControl": { + "x": 2.626970566775512, + "y": 5.022952408768852 + }, + "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": -40.601294645004494 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -177.66687659440373 + }, + "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..76a8bf1 --- /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": 30.4753154919261 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 33.24675324837286 + }, + "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..8fd3064 --- /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.1484544028211756, + "y": 1.8239774710468408 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9919156626506025, + "y": 0.84410843373494 + }, + "prevControl": { + "x": 2.5302831391445184, + "y": 1.1192385013240713 + }, + "nextControl": { + "x": 3.2917909132718615, + "y": 0.6653846788636105 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.281156626506024, + "y": 0.84410843373494 + }, + "prevControl": { + "x": 5.743691570285816, + "y": 0.5279226592442804 + }, + "nextControl": { + "x": 6.710977200889518, + "y": 1.096967944563833 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.871, + "y": 2.062 + }, + "prevControl": { + "x": 6.365470403556441, + "y": 1.7973692474107146 + }, + "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": -179.37140021738475 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 36.869897645844176 + }, + "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..d009707 --- /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.629993227281043, + "y": 1.4883331821104697 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.2265180722891555, + "y": 0.7020481927710842 + }, + "prevControl": { + "x": 6.788697247207432, + "y": 0.918004206755939 + }, + "nextControl": { + "x": 5.993144589675134, + "y": 0.6123998932188741 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.751506024096386, + "y": 0.7020481927710842 + }, + "prevControl": { + "x": 3.8113649917761085, + "y": 0.6238826126913387 + }, + "nextControl": { + "x": 2.2395770031209197, + "y": 0.7398034320099484 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7352289156626508, + "y": 1.6199759036144585 + }, + "prevControl": { + "x": 1.9795607124284185, + "y": 1.67290957049303 + }, + "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": 45.41820994149708 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.46138778354057 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 669ab2f300cd317ccd11f86c02f179e461156771 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Wed, 11 Mar 2026 11:31:32 -0400 Subject: [PATCH 07/16] indexer and intake wheel bringup --- src/main/java/org/team2342/frc/Constants.java | 16 +++++------ .../java/org/team2342/frc/RobotContainer.java | 27 +++++++++++++------ .../team2342/frc/subsystems/Conductor.java | 11 +++----- 3 files changed, 28 insertions(+), 26 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index ab56bde..49a8e08 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -138,12 +138,12 @@ public static final class ConductorConstants { } public static final class IndexerConstants { - public static final double RUN_VOLTAGE = 8.0; + public static final double RUN_VOLTAGE = 7.0; public static final double RUN_CURRENT = 30.0; public static final MotorConfig INDEXER_MOTOR_CONFIG = new MotorConfig() - .withMotorInverted(false) + .withMotorInverted(true) .withSupplyCurrentLimit(30.0) .withStatorCurrentLimit(70.0) .withIdleMode(MotorConfig.IdleMode.BRAKE); @@ -279,19 +279,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; // Kicker motor ID - public static final int HOOD_ENCODER_ID = 16; // Turret ID + 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_MOTOR_ID = 20; - - 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/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 6561ce6..0a36797 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -55,6 +55,7 @@ import org.team2342.lib.leds.LedStrip; 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; @@ -126,8 +127,15 @@ public RobotContainer() { new Kicker( new DumbMotorIOTalonFXFOC(CANConstants.KICKER_ID, KickerConstants.KICKER_CONFIG)); - indexer = new Indexer(new DumbMotorIO() {}); - wheels = new Wheels(new DumbMotorIO() {}); + indexer = + new Indexer( + new DumbMotorIOTalonFXFOC( + CANConstants.INDEXER_MOTOR_ID, IndexerConstants.INDEXER_MOTOR_CONFIG)); + wheels = + new Wheels( + new DumbMotorIOTalonFX( + CANConstants.INTAKE_WHEEL_MOTOR_ID, + IntakeConstants.INTAKE_WHEELS_MOTOR_CONFIG)); leds = new LedStrip(new LedIO() {}, "CANdle"); @@ -270,11 +278,13 @@ private void configureBindings() { .ignoringDisable(true)); driverController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); + driverController.a().whileTrue(indexer.in()).onFalse(indexer.stop()); // Shooting Controls - driverController.rightTrigger().whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)); - - driverController.a().whileTrue(flywheel.runVoltage(12)).whileFalse(flywheel.stop()); + driverController + .rightTrigger() + .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING).alongWith(indexer.in())) + .onFalse(indexer.stop()); // Operator Overrides operatorController.povRight().whileTrue(indexer.in()).onFalse(indexer.stop()); @@ -284,9 +294,10 @@ private void configureBindings() { operatorController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); // Location Triggers - allianceZoneTrigger - .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) - .whileTrue(conductor.runState(ConductorState.WARM_UP)); + // allianceZoneTrigger + // + // .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) + // .whileTrue(conductor.runState(ConductorState.WARM_UP)); } public Command getAutonomousCommand() { diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index ab04d9f..6ce50f5 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -22,8 +22,6 @@ import org.team2342.lib.leds.LedStrip; import org.team2342.lib.logging.ExecutionLogger; import org.team2342.lib.logging.tunable.TunableNumber; -import org.team2342.lib.leds.LedIO.LEDAnimation; -import edu.wpi.first.wpilibj.util.Color; public class Conductor extends SubsystemBase { @@ -108,8 +106,7 @@ private void setupStateCommands() { .calculate(velocitySupplier.get(), poseSupplier.get()) .turretAngle()) .alongWith(flywheel.warmUp()) - .alongWith(kicker.stop()) - .alongWith(leds.setAllCommand(LEDAnimation.SOLID, Color.kRed))); + .alongWith(kicker.stop())); fsm.addStateCommand( ConductorState.TRACKED_FIRING, @@ -125,8 +122,7 @@ private void setupStateCommands() { FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) .wheelSpeed())) - .alongWith(kicker.in()) - .alongWith(leds.setAllCommand(LEDAnimation.SOLID, Color.kOrange))); + .alongWith(kicker.in())); fsm.addStateCommand( ConductorState.TUNING, @@ -137,8 +133,7 @@ private void setupStateCommands() { .calculate(velocitySupplier.get(), poseSupplier.get()) .turretAngle()) .alongWith(flywheel.shoot(flywheelSpeed)) - .alongWith(kicker.in()) - .alongWith(leds.setAllCommand(LEDAnimation.SOLID, Color.kPurple))); + .alongWith(kicker.in())); } public Command disable() { From f555fec4da45c1029ba59f13b45f236a8b39611e Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Wed, 11 Mar 2026 10:33:34 -0400 Subject: [PATCH 08/16] fixing passing --- src/main/java/org/team2342/frc/Constants.java | 2 +- .../java/org/team2342/frc/util/FieldConstants.java | 12 ++++++++---- .../java/org/team2342/frc/util/FiringSolver.java | 8 +++++++- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index 49a8e08..f2d167f 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -23,7 +23,7 @@ import org.team2342.lib.util.CameraParameters; public final class Constants { - public static final Mode CURRENT_MODE = Mode.REAL; + public static final Mode CURRENT_MODE = Mode.SIM; public static final boolean TUNING = true; public static enum Mode { 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 dfba3f1..d4da128 100644 --- a/src/main/java/org/team2342/frc/util/FiringSolver.java +++ b/src/main/java/org/team2342/frc/util/FiringSolver.java @@ -65,13 +65,18 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { Translation2d leftBump = AllianceUtils.flipToAlliance(FieldConstants.LeftBump.nearLeftCorner); Translation2d rightBump = - AllianceUtils.flipToAlliance(FieldConstants.RightBump.nearRightCorner); + 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) @@ -109,6 +114,7 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { 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)), From a143d7ec31e4dce69c6f42e7c33c2b54a307e168 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Wed, 11 Mar 2026 11:04:08 -0400 Subject: [PATCH 09/16] reactivating rotation locked drive --- .../frc/commands/RotationLockedDrive.java | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java index f239eb2..9706e7f 100644 --- a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java +++ b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java @@ -26,8 +26,13 @@ public class RotationLockedDrive extends Command { private final Drive drive; private final Timer inputTimer = new Timer(); - private final ProfiledPIDController rotationPID = - new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(8.0, 20.0)); + private final ProfiledPIDController angleController = + new ProfiledPIDController( + 5.0, + 0.0, + 0.4, + new TrapezoidProfile.Constraints(8.0, 20.0)); + private Rotation2d rotationLock; public RotationLockedDrive( @@ -40,7 +45,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 +55,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 +74,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 From a4ab89cdffc88ba325fca5a1e10b6038f2fdb160 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Wed, 11 Mar 2026 13:07:45 -0400 Subject: [PATCH 10/16] adding shooter controls + hub timing --- build.gradle | 1 + src/main/java/org/team2342/frc/Robot.java | 5 +- .../java/org/team2342/frc/RobotContainer.java | 93 ++++++--- .../frc/commands/RotationLockedDrive.java | 6 +- .../team2342/frc/subsystems/Conductor.java | 23 +-- .../frc/subsystems/shooter/Flywheel.java | 8 + .../frc/subsystems/shooter/Turret.java | 4 + .../org/team2342/frc/util/FiringSolver.java | 11 +- .../org/team2342/frc/util/HubShiftUtil.java | 190 ++++++++++++++++++ 9 files changed, 289 insertions(+), 52 deletions(-) create mode 100644 src/main/java/org/team2342/frc/util/HubShiftUtil.java 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/java/org/team2342/frc/Robot.java b/src/main/java/org/team2342/frc/Robot.java index 923f566..66b90d5 100644 --- a/src/main/java/org/team2342/frc/Robot.java +++ b/src/main/java/org/team2342/frc/Robot.java @@ -32,6 +32,8 @@ import org.team2342.frc.util.FiringSolver; import org.team2342.frc.util.PhoenixUtils; import org.team2342.lib.logging.ExecutionLogger; +import org.team2342.frc.util.HubShiftUtil; + public class Robot extends LoggedRobot { private Command autonomousCommand; @@ -158,7 +160,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); ExecutionLogger.log("Commands"); - robotContainer.getFlywheel().runVoltage(12); + Logger.recordOutput("ShiftUtil/Official", HubShiftUtil.getOfficialShiftInfo()); + Logger.recordOutput("ShiftUtil/Shifted", HubShiftUtil.getShiftedShiftInfo()); robotContainer.updateAlerts(); FiringSolver.getInstance().clearCachedSolution(); diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 0a36797..091c264 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -8,14 +8,18 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; + +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; 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.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import lombok.Getter; @@ -51,6 +55,8 @@ import org.team2342.frc.subsystems.vision.VisionIOPhoton; 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.LedStrip; import org.team2342.lib.motors.dumb.DumbMotorIO; @@ -93,6 +99,9 @@ public class RobotContainer { new Alert("Operator controller is disconnected!", AlertType.kError); private final Trigger allianceZoneTrigger; + private final Trigger shiftAboutToEnd; + private final Trigger activeOrPassing; + private final Trigger readyToFire; public RobotContainer() { switch (Constants.CURRENT_MODE) { @@ -214,8 +223,7 @@ public RobotContainer() { break; } - conductor = - new Conductor(flywheel, turret, kicker, leds, drive::getPose, drive::getChassisSpeeds); + conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); configureNamedCommands(); @@ -224,17 +232,6 @@ public RobotContainer() { if (Constants.TUNING) setupDevelopmentRoutines(); - autoChooser.addOption( - "wait n fire", - conductor - .runState(ConductorState.WARM_UP) - .withTimeout(1) - .andThen( - conductor - .goToState(ConductorState.TRACKED_FIRING) - .andThen(conductor.runState(ConductorState.TRACKED_FIRING)) - .alongWith(indexer.in()))); - SmartDashboard.putData( "Calculate Vision Heading Offset", Commands.runOnce(() -> drive.calculateVisionHeadingOffset()) @@ -248,6 +245,16 @@ public RobotContainer() { drive.getPose().getX(), AllianceUtils.flipToAlliance(Pose2d.kZero).getX(), 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()); + readyToFire = + new Trigger(() -> flywheel.atGoal() && turret.atGoal()); configureBindings(); } @@ -265,10 +272,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( () -> @@ -277,27 +286,57 @@ private void configureBindings() { drive) .ignoringDisable(true)); + // Trench Drive + driverController + .b() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + drive, + () -> -driverController.getLeftY(), + () -> -driverController.getLeftX(), + () -> Rotation2d.kZero)); + + // Run Intake driverController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); - driverController.a().whileTrue(indexer.in()).onFalse(indexer.stop()); - // Shooting Controls + // Retract Intake + driverController.leftBumper(); + + // Auto Shoot driverController .rightTrigger() - .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING).alongWith(indexer.in())) - .onFalse(indexer.stop()); + .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) + .and(activeOrPassing) + .and(readyToFire.debounce(0.2, DebounceType.kFalling)) + .whileTrue(indexer.in().alongWith(kicker.in())) + .onFalse(indexer.stop().alongWith(kicker.stop())); - // Operator Overrides - operatorController.povRight().whileTrue(indexer.in()).onFalse(indexer.stop()); - operatorController.povLeft().whileTrue(indexer.out()).onFalse(indexer.stop()); + // Firing during inactive period + driverController + .rightTrigger() + .and(() -> !HubShiftUtil.getShiftedShiftInfo().active()).onTrue(driverController.rumble(RumbleType.kBothRumble, 1.0).withTimeout(0.5)); - operatorController.leftBumper().whileTrue(wheels.out()).onFalse(wheels.stop()); - operatorController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); + // Shift Timer Override + driverController + .rightBumper() + .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING).alongWith(indexer.in())) + .and(readyToFire.debounce(0.2, DebounceType.kFalling)) + .whileTrue(indexer.in().alongWith(kicker.in())) + .onFalse(indexer.stop().alongWith(kicker.stop())); // Location Triggers - // allianceZoneTrigger - // - // .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) - // .whileTrue(conductor.runState(ConductorState.WARM_UP)); + 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() { diff --git a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java index 9706e7f..900c798 100644 --- a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java +++ b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java @@ -27,11 +27,7 @@ public class RotationLockedDrive extends Command { private final Timer inputTimer = new Timer(); private final ProfiledPIDController angleController = - new ProfiledPIDController( - 5.0, - 0.0, - 0.4, - new TrapezoidProfile.Constraints(8.0, 20.0)); + new ProfiledPIDController(5.0, 0.0, 0.4, new TrapezoidProfile.Constraints(8.0, 20.0)); private Rotation2d rotationLock; diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index 6ce50f5..243fd53 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -15,7 +15,6 @@ import lombok.experimental.Delegate; import org.team2342.frc.Constants; import org.team2342.frc.subsystems.shooter.Flywheel; -import org.team2342.frc.subsystems.shooter.Kicker; import org.team2342.frc.subsystems.shooter.Turret; import org.team2342.frc.util.FiringSolver; import org.team2342.lib.fsm.StateMachine; @@ -46,8 +45,6 @@ public enum ConductorState { private final Flywheel flywheel; private final Turret turret; - private final Kicker kicker; - private final LedStrip leds; private final Supplier poseSupplier; private final Supplier velocitySupplier; @@ -55,14 +52,10 @@ public enum ConductorState { public Conductor( Flywheel flywheel, Turret turret, - Kicker kicker, - LedStrip leds, Supplier poseSupplier, Supplier velocitySupplier) { this.flywheel = flywheel; this.turret = turret; - this.kicker = kicker; - this.leds = leds; this.poseSupplier = poseSupplier; this.velocitySupplier = velocitySupplier; @@ -94,8 +87,7 @@ public Command waitForState(ConductorState state) { } private void setupStateCommands() { - fsm.addStateCommand( - ConductorState.DISABLED, turret.stop().alongWith(flywheel.stop()).alongWith(kicker.stop())); + fsm.addStateCommand(ConductorState.DISABLED, Commands.parallel(turret.stop(), flywheel.stop())); fsm.addStateCommand( ConductorState.WARM_UP, @@ -105,8 +97,7 @@ private void setupStateCommands() { FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) .turretAngle()) - .alongWith(flywheel.warmUp()) - .alongWith(kicker.stop())); + .alongWith(flywheel.warmUp())); fsm.addStateCommand( ConductorState.TRACKED_FIRING, @@ -121,8 +112,7 @@ private void setupStateCommands() { () -> FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) - .wheelSpeed())) - .alongWith(kicker.in())); + .wheelSpeed()))); fsm.addStateCommand( ConductorState.TUNING, @@ -132,8 +122,7 @@ private void setupStateCommands() { FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) .turretAngle()) - .alongWith(flywheel.shoot(flywheelSpeed)) - .alongWith(kicker.in())); + .alongWith(flywheel.shoot(flywheelSpeed))); } public Command disable() { @@ -144,6 +133,10 @@ public Command enable() { return Commands.runOnce(() -> fsm.enable()); } + public ConductorState getCurrentState() { + return fsm.getCurrentState(); + } + private void setupTransitions() { fsm.addOmniTransition(ConductorState.DISABLED); fsm.addOmniTransition(ConductorState.TRACKED_FIRING); 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..61d3928 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,12 +64,14 @@ public void periodic() { public void runVelocity(double metersPerSec) { double radPerSec = metersPerSec / ShooterConstants.FLYWHEEL_RADIUS_METERS; + atGoal = Math.abs(radPerSec - motorInputs.velocityRadPerSec) <= 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); } @@ -100,6 +104,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/shooter/Turret.java b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java index 35ef28f..90cbd21 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Turret.java @@ -95,6 +95,10 @@ 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); } diff --git a/src/main/java/org/team2342/frc/util/FiringSolver.java b/src/main/java/org/team2342/frc/util/FiringSolver.java index d4da128..7e92851 100644 --- a/src/main/java/org/team2342/frc/util/FiringSolver.java +++ b/src/main/java/org/team2342/frc/util/FiringSolver.java @@ -22,7 +22,7 @@ public class FiringSolver { private static final int ITERATIONS = 5; - public static final FiringSolution BUMPER_SHOT = new FiringSolution(new Rotation2d(), 23.5); + public static final double MIN_TOF, MAX_TOF; private FiringSolution lastSolution = null; @@ -34,6 +34,9 @@ public class FiringSolver { speedMap.put(1.141, 15.0); speedMap.put(1.445, 17.0); + MIN_TOF = 1.0; + MAX_TOF = 1.0; + tofMap.put(1.141, 12.66 - 11.46); tofMap.put(1.445, 8.34 - 6.95); } @@ -85,7 +88,7 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { .minus(Rotation2d.kCCW_Pi_2); // TODO: tune real passing speed - lastSolution = new FiringSolution(turretAngle, 15.0); + lastSolution = new FiringSolution(turretAngle, 15.0, true); return lastSolution; } @@ -132,7 +135,7 @@ public FiringSolution calculate(ChassisSpeeds velocity, Pose2d position) { double wheelSpeed = speedMap.get(predictedDistance); - lastSolution = new FiringSolution(turretAngle, wheelSpeed); + lastSolution = new FiringSolution(turretAngle, wheelSpeed, false); return lastSolution; } @@ -141,5 +144,5 @@ public void clearCachedSolution() { lastSolution = null; } - public record FiringSolution(Rotation2d turretAngle, double wheelSpeed) {} + 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); + } +} From 93dad99b8446b9a85735e47435ce116f1a1809b4 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Wed, 11 Mar 2026 13:34:04 -0400 Subject: [PATCH 11/16] led subsystem --- src/main/java/org/team2342/frc/Constants.java | 1 + src/main/java/org/team2342/frc/Robot.java | 3 +- .../java/org/team2342/frc/RobotContainer.java | 27 +++--- .../team2342/frc/subsystems/Conductor.java | 1 - .../frc/subsystems/leds/LEDSubsystem.java | 90 +++++++++++++++++++ .../frc/subsystems/shooter/Flywheel.java | 8 +- .../frc/subsystems/vision/Vision.java | 9 ++ 7 files changed, 122 insertions(+), 17 deletions(-) create mode 100644 src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index f2d167f..eaa7646 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -197,6 +197,7 @@ public static final class ShooterConstants { 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 = 2.0; public static final PIDFFConfigs FLYWHEEL_PID_CONFIGS = new PIDFFConfigs() diff --git a/src/main/java/org/team2342/frc/Robot.java b/src/main/java/org/team2342/frc/Robot.java index 66b90d5..9d0d963 100644 --- a/src/main/java/org/team2342/frc/Robot.java +++ b/src/main/java/org/team2342/frc/Robot.java @@ -30,10 +30,9 @@ 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; -import org.team2342.frc.util.HubShiftUtil; - public class Robot extends LoggedRobot { private Command autonomousCommand; diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 091c264..8414762 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -8,7 +8,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; - import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -47,6 +46,7 @@ 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.Kicker; import org.team2342.frc.subsystems.shooter.Turret; @@ -58,7 +58,6 @@ import org.team2342.frc.util.FiringSolver; import org.team2342.frc.util.HubShiftUtil; import org.team2342.lib.leds.LedIO; -import org.team2342.lib.leds.LedStrip; import org.team2342.lib.motors.dumb.DumbMotorIO; import org.team2342.lib.motors.dumb.DumbMotorIOSim; import org.team2342.lib.motors.dumb.DumbMotorIOTalonFX; @@ -79,7 +78,7 @@ public class RobotContainer { @Getter private final Wheels wheels; @Getter private final Flywheel flywheel; @Getter private final Turret turret; - @Getter private final LedStrip leds; + @Getter private final LEDSubsystem leds; @Getter private final Conductor conductor; @@ -146,7 +145,9 @@ public RobotContainer() { CANConstants.INTAKE_WHEEL_MOTOR_ID, IntakeConstants.INTAKE_WHEELS_MOTOR_CONFIG)); - leds = new LedStrip(new LedIO() {}, "CANdle"); + conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + leds = + new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState); LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev); break; @@ -193,7 +194,9 @@ public RobotContainer() { turret = new Turret(new SmartMotorIO() {}); pivot = new Pivot(new SmartMotorIO() {}); - leds = new LedStrip(new LedIO() {}, "CANdle"); + conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + leds = + new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState); break; @@ -218,13 +221,13 @@ public RobotContainer() { turret = new Turret(new SmartMotorIO() {}); kicker = new Kicker(new DumbMotorIO() {}); - leds = new LedStrip(new LedIO() {}, "CANdle"); + conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); + leds = + new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState); break; } - conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); - configureNamedCommands(); autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -253,8 +256,7 @@ public RobotContainer() { || FiringSolver.getInstance() .calculate(drive.getChassisSpeeds(), drive.getPose()) .passing()); - readyToFire = - new Trigger(() -> flywheel.atGoal() && turret.atGoal()); + readyToFire = new Trigger(() -> flywheel.atGoal() && turret.atGoal()); configureBindings(); } @@ -314,7 +316,8 @@ private void configureBindings() { // Firing during inactive period driverController .rightTrigger() - .and(() -> !HubShiftUtil.getShiftedShiftInfo().active()).onTrue(driverController.rumble(RumbleType.kBothRumble, 1.0).withTimeout(0.5)); + .and(() -> !HubShiftUtil.getShiftedShiftInfo().active()) + .onTrue(driverController.rumble(RumbleType.kBothRumble, 1.0).withTimeout(0.5)); // Shift Timer Override driverController @@ -326,7 +329,7 @@ private void configureBindings() { // Location Triggers allianceZoneTrigger - .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) + .and(driverController.rightTrigger().negate().and(driverController.rightBumper().negate())) .whileTrue(conductor.runState(ConductorState.WARM_UP)); // Shift Util Resets diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index 243fd53..acfd718 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -18,7 +18,6 @@ import org.team2342.frc.subsystems.shooter.Turret; import org.team2342.frc.util.FiringSolver; import org.team2342.lib.fsm.StateMachine; -import org.team2342.lib.leds.LedStrip; import org.team2342.lib.logging.ExecutionLogger; import org.team2342.lib.logging.tunable.TunableNumber; 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..f18ce6d --- /dev/null +++ b/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java @@ -0,0 +1,90 @@ +// 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.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.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; + + 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; + + //teleop code here + + + } 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) { + setFirst(effect); + setSecond(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 61d3928..46fa895 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java @@ -64,14 +64,18 @@ public void periodic() { public void runVelocity(double metersPerSec) { double radPerSec = metersPerSec / ShooterConstants.FLYWHEEL_RADIUS_METERS; - atGoal = Math.abs(radPerSec - motorInputs.velocityRadPerSec) <= ShooterConstants.FLYWHEEL_AT_GOAL_TOLERANCE; + atGoal = + Math.abs(radPerSec - motorInputs.velocityRadPerSec) + <= 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; + atGoal = + Math.abs(radPerSec - motorInputs.velocityRadPerSec) + <= ShooterConstants.FLYWHEEL_AT_GOAL_TOLERANCE; Logger.recordOutput("Shooter/Flywheel/SetpointMetersPerSec", metersPerSec); motor.runVelocity(radPerSec); } 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<>(); From 3538206628f6566712316d17298aed2f0705e9f2 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Wed, 11 Mar 2026 20:14:41 -0400 Subject: [PATCH 12/16] Merging intake pivot code Co-authored-by: aniruddhsridhar Co-authored-by: Phoenix2342-Bot Co-authored-by: abi-appusamy9932 Co-authored-by: novA2026 Co-authored-by: CuevasN1 Co-authored-by: abhi-team2342 Co-authored-by: dlaflotte --- src/main/java/org/team2342/frc/RobotContainer.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 8414762..a5894a3 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -139,6 +139,12 @@ public RobotContainer() { 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))); wheels = new Wheels( new DumbMotorIOTalonFX( From 7ca8ccad929a49591fe498b36de90a234859a6e1 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Wed, 11 Mar 2026 20:39:20 -0400 Subject: [PATCH 13/16] wip --- src/main/java/org/team2342/frc/Constants.java | 9 ++++---- .../java/org/team2342/frc/RobotContainer.java | 22 +++++++++++++------ .../frc/commands/RotationLockedDrive.java | 2 +- .../team2342/frc/subsystems/intake/Pivot.java | 6 +++++ .../frc/subsystems/leds/LEDSubsystem.java | 5 ++--- .../frc/subsystems/shooter/Flywheel.java | 2 +- 6 files changed, 30 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index eaa7646..b627b0b 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -19,11 +19,12 @@ import org.team2342.lib.motors.MotorConfig.IdleMode; import org.team2342.lib.motors.smart.SmartMotorConfig; import org.team2342.lib.motors.smart.SmartMotorConfig.ControlType; +import org.team2342.lib.motors.smart.SmartMotorConfig.FeedbackConfig; import org.team2342.lib.pidff.PIDFFConfigs; import org.team2342.lib.util.CameraParameters; public final class Constants { - public static final Mode CURRENT_MODE = Mode.SIM; + public static final Mode CURRENT_MODE = Mode.REAL; public static final boolean TUNING = true; public static enum Mode { @@ -138,13 +139,13 @@ public static final class ConductorConstants { } public static final class IndexerConstants { - public static final double RUN_VOLTAGE = 7.0; + public static final double RUN_VOLTAGE = 5.0; public static final double RUN_CURRENT = 30.0; public static final MotorConfig INDEXER_MOTOR_CONFIG = new MotorConfig() .withMotorInverted(true) - .withSupplyCurrentLimit(30.0) + .withSupplyCurrentLimit(40.0) .withStatorCurrentLimit(70.0) .withIdleMode(MotorConfig.IdleMode.BRAKE); @@ -197,7 +198,7 @@ public static final class ShooterConstants { 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 = 2.0; + public static final double FLYWHEEL_AT_GOAL_TOLERANCE = 1.0; public static final PIDFFConfigs FLYWHEEL_PID_CONFIGS = new PIDFFConfigs() diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index a5894a3..401907c 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -305,10 +305,16 @@ private void configureBindings() { () -> Rotation2d.kZero)); // Run Intake - driverController.leftTrigger().whileTrue(wheels.in()).onFalse(wheels.stop()); + driverController + .leftTrigger() + .whileTrue(wheels.in().alongWith(pivot.holdAngle(IntakeConstants.MIN_ANGLE))) + .onFalse(wheels.stop().alongWith(pivot.stop())); // Retract Intake - driverController.leftBumper(); + driverController + .leftBumper() + .whileTrue(pivot.holdAngle(IntakeConstants.MAX_ANGLE)) + .onFalse(pivot.stop()); // Auto Shoot driverController @@ -316,8 +322,9 @@ private void configureBindings() { .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) .and(activeOrPassing) .and(readyToFire.debounce(0.2, DebounceType.kFalling)) - .whileTrue(indexer.in().alongWith(kicker.in())) - .onFalse(indexer.stop().alongWith(kicker.stop())); + .whileTrue( + Commands.parallel(indexer.in(), kicker.in(), pivot.agitate(), wheels.runIntake(3))) + .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), pivot.stop(), wheels.stop())); // Firing during inactive period driverController @@ -328,10 +335,11 @@ private void configureBindings() { // Shift Timer Override driverController .rightBumper() - .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING).alongWith(indexer.in())) + .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) .and(readyToFire.debounce(0.2, DebounceType.kFalling)) - .whileTrue(indexer.in().alongWith(kicker.in())) - .onFalse(indexer.stop().alongWith(kicker.stop())); + .whileTrue( + Commands.parallel(indexer.in(), kicker.in(), pivot.agitate(), wheels.runIntake(3))) + .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), pivot.stop(), wheels.stop())); // Location Triggers allianceZoneTrigger diff --git a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java index 900c798..bd18dcf 100644 --- a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java +++ b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java @@ -27,7 +27,7 @@ public class RotationLockedDrive extends Command { private final Timer inputTimer = new Timer(); private final ProfiledPIDController angleController = - new ProfiledPIDController(5.0, 0.0, 0.4, new TrapezoidProfile.Constraints(8.0, 20.0)); + new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(8.0, 20.0)); private Rotation2d rotationLock; 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..10bdccc 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; @@ -64,4 +65,9 @@ public Command holdAngle(double angle) { public Command stop() { return runOnce(() -> pivotMotor.runVoltage(0.0)).withName("Pivot Stop"); } + + public Command agitate() { + return Commands.repeatingSequence( + holdAngle(1.0).withTimeout(0.5), holdAngle(IntakeConstants.MIN_ANGLE).withTimeout(0.5)); + } } diff --git a/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java b/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java index f18ce6d..12cc75c 100644 --- a/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java +++ b/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java @@ -30,7 +30,7 @@ public class LEDSubsystem extends SubsystemBase { private final Supplier stateSupplier; private boolean hasEnabled = false; - private Debouncer visionDebouncer; + private Debouncer visionDebouncer = new Debouncer(0.2); public LEDSubsystem( LedIO io, String name, BooleanSupplier hasTags, Supplier stateSupplier) { @@ -53,9 +53,8 @@ public void periodic() { if (DriverStation.isTeleopEnabled()) { hasEnabled = true; - //teleop code here + // teleop code here - } else if (DriverStation.isAutonomousEnabled()) { hasEnabled = true; setAll(new LEDEffect(LEDAnimation.RAINBOW, Color.kWhite)); 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 46fa895..3102714 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java @@ -65,7 +65,7 @@ public void periodic() { public void runVelocity(double metersPerSec) { double radPerSec = metersPerSec / ShooterConstants.FLYWHEEL_RADIUS_METERS; atGoal = - Math.abs(radPerSec - motorInputs.velocityRadPerSec) + Math.abs(metersPerSec - getVelocityMetersPerSec()) <= ShooterConstants.FLYWHEEL_AT_GOAL_TOLERANCE; Logger.recordOutput("Shooter/Flywheel/SetpointMetersPerSec", metersPerSec); motor.runVelocity(radPerSec); From 37a507b434b06f02a769f874edfc13263deb2be6 Mon Sep 17 00:00:00 2001 From: abi-appusamy9932 Date: Wed, 11 Mar 2026 20:53:39 -0400 Subject: [PATCH 14/16] update leds --- src/main/deploy/pathplanner/paths/R1.path | 4 +-- src/main/deploy/pathplanner/paths/R2.path | 28 +++++++++---------- src/main/deploy/pathplanner/paths/R3.path | 28 +++++++++---------- .../frc/subsystems/leds/LEDSubsystem.java | 9 ++++-- 4 files changed, 37 insertions(+), 32 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/R1.path b/src/main/deploy/pathplanner/paths/R1.path index 76a8bf1..6103521 100644 --- a/src/main/deploy/pathplanner/paths/R1.path +++ b/src/main/deploy/pathplanner/paths/R1.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 30.4753154919261 + "rotation": -149.189 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 33.24675324837286 + "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 index 8fd3064..56d968f 100644 --- a/src/main/deploy/pathplanner/paths/R2.path +++ b/src/main/deploy/pathplanner/paths/R2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.1484544028211756, - "y": 1.8239774710468408 + "x": 2.183090085894316, + "y": 1.8380379656816952 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 0.84410843373494 }, "prevControl": { - "x": 2.5302831391445184, - "y": 1.1192385013240713 + "x": 2.5473352340379964, + "y": 1.1596555660285341 }, "nextControl": { - "x": 3.2917909132718615, - "y": 0.6653846788636105 + "x": 3.2312215506910658, + "y": 0.6742577568893806 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 0.84410843373494 }, "prevControl": { - "x": 5.743691570285816, - "y": 0.5279226592442804 + "x": 5.764583687019719, + "y": 0.5070260819383028 }, "nextControl": { - "x": 6.710977200889518, - "y": 1.096967944563833 + "x": 6.661872155520445, + "y": 1.0925389574346511 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ "y": 2.062 }, "prevControl": { - "x": 6.365470403556441, - "y": 1.7973692474107146 + "x": 6.725101421617429, + "y": 1.8283617076130094 }, "nextControl": null, "isLocked": false, @@ -74,13 +74,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -179.37140021738475 + "rotation": 2.606 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 36.869897645844176 + "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 index d009707..79f1204 100644 --- a/src/main/deploy/pathplanner/paths/R3.path +++ b/src/main/deploy/pathplanner/paths/R3.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.629993227281043, - "y": 1.4883331821104697 + "x": 6.593518983931554, + "y": 1.4505471402803671 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 0.7020481927710842 }, "prevControl": { - "x": 6.788697247207432, - "y": 0.918004206755939 + "x": 6.788520440156554, + "y": 0.9251369332284056 }, "nextControl": { - "x": 5.993144589675134, - "y": 0.6123998932188741 + "x": 5.9941555915520075, + "y": 0.6098111202585716 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 0.7020481927710842 }, "prevControl": { - "x": 3.8113649917761085, - "y": 0.6238826126913387 + "x": 3.7550118117890667, + "y": 0.6357757561431316 }, "nextControl": { - "x": 2.2395770031209197, - "y": 0.7398034320099484 + "x": 2.272922602581813, + "y": 0.7336542780199183 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ "y": 1.6199759036144585 }, "prevControl": { - "x": 1.9795607124284185, - "y": 1.67290957049303 + "x": 1.9769027739250977, + "y": 1.5559935135954637 }, "nextControl": null, "isLocked": false, @@ -74,13 +74,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 45.41820994149708 + "rotation": -149.189 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -178.46138778354057 + "rotation": 2.606 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java b/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java index 12cc75c..6cdbca8 100644 --- a/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java +++ b/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java @@ -52,8 +52,13 @@ public void periodic() { if (DriverStation.isTeleopEnabled()) { hasEnabled = true; - - // teleop code here + 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; From 1b2e916172117e4fc30263f7bfa34265c5b25f65 Mon Sep 17 00:00:00 2001 From: abi-appusamy9932 Date: Wed, 11 Mar 2026 22:41:56 -0400 Subject: [PATCH 15/16] adding autos --- .../deploy/pathplanner/autos/LeftTrench.auto | 18 ++++++++++++ .../deploy/pathplanner/autos/RightTrench.auto | 18 ++++++++++++ src/main/deploy/pathplanner/paths/L1.path | 12 ++++---- src/main/deploy/pathplanner/paths/L2.path | 28 +++++++++---------- src/main/deploy/pathplanner/paths/L3.path | 28 +++++++++---------- src/main/deploy/pathplanner/paths/R3.path | 28 +++++++++---------- .../java/org/team2342/frc/RobotContainer.java | 24 ++++++++++++++++ 7 files changed, 108 insertions(+), 48 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/LeftTrench.auto b/src/main/deploy/pathplanner/autos/LeftTrench.auto index 80cc7ab..f6f570e 100644 --- a/src/main/deploy/pathplanner/autos/LeftTrench.auto +++ b/src/main/deploy/pathplanner/autos/LeftTrench.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "autoShoot" + } + }, { "type": "path", "data": { @@ -16,11 +22,23 @@ "pathName": "L2" } }, + { + "type": "named", + "data": { + "name": "autoIntake" + } + }, { "type": "path", "data": { "pathName": "L3" } + }, + { + "type": "named", + "data": { + "name": "autoShoot" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/RightTrench.auto b/src/main/deploy/pathplanner/autos/RightTrench.auto index 25eab6c..5cdc8e0 100644 --- a/src/main/deploy/pathplanner/autos/RightTrench.auto +++ b/src/main/deploy/pathplanner/autos/RightTrench.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "autoShoot" + } + }, { "type": "path", "data": { @@ -16,11 +22,23 @@ "pathName": "R2" } }, + { + "type": "named", + "data": { + "name": "autoIntake" + } + }, { "type": "path", "data": { "pathName": "R3" } + }, + { + "type": "named", + "data": { + "name": "autoShoot" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/L1.path b/src/main/deploy/pathplanner/paths/L1.path index b1a1afb..6677534 100644 --- a/src/main/deploy/pathplanner/paths/L1.path +++ b/src/main/deploy/pathplanner/paths/L1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 3.412672784962453, - "y": 4.729036049442378 + "x": 3.4642092444429213, + "y": 4.547552372815045 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.969204819277109 }, "prevControl": { - "x": 2.0890855281730722, - "y": 6.128214273586397 + "x": 2.1223114027842978, + "y": 6.161557495159928 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -42.27368900609373 + "rotation": 142.56500000000003 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -29.16761337957779 + "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 index 11ffc44..cf9a7ab 100644 --- a/src/main/deploy/pathplanner/paths/L2.path +++ b/src/main/deploy/pathplanner/paths/L2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.784139256412005, - "y": 6.467901003200028 + "x": 2.502228276547937, + "y": 6.08731950899461 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 7.411662650602409 }, "prevControl": { - "x": 3.2389540480080994, - "y": 7.311855883645149 + "x": 2.9992886550228137, + "y": 7.24829220211798 }, "nextControl": { - "x": 4.068842444662846, - "y": 7.453814875551677 + "x": 4.067638556570527, + "y": 7.460331806686578 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 7.411662650602409 }, "prevControl": { - "x": 4.756795135625847, - "y": 7.423892570078729 + "x": 4.792781990522218, + "y": 7.409964962466848 }, "nextControl": { - "x": 6.568075333666703, - "y": 7.404493346237625 + "x": 6.561335127600884, + "y": 7.412679890366718 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ "y": 6.078 }, "prevControl": { - "x": 6.938337410220809, - "y": 6.448604687790133 + "x": 6.9961203530394185, + "y": 6.319179213587149 }, "nextControl": null, "isLocked": false, @@ -74,13 +74,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": -2.2493064773545726 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -39.497 + "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 index d767362..98c8388 100644 --- a/src/main/deploy/pathplanner/paths/L3.path +++ b/src/main/deploy/pathplanner/paths/L3.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.409770391442066, - "y": 5.805185542220385 + "x": 7.338850958716274, + "y": 5.857520632417328 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 7.378879518072289 }, "prevControl": { - "x": 6.453365128252008, - "y": 7.130662897315942 + "x": 6.450791877215048, + "y": 7.126510358553121 }, "nextControl": { - "x": 5.611419730119451, - "y": 7.473645062124593 + "x": 5.613219480999081, + "y": 7.474890485267073 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 7.171253012048193 }, "prevControl": { - "x": 3.7104254130968934, - "y": 7.448500201429967 + "x": 3.6947172730944753, + "y": 7.449264616345176 }, "nextControl": { - "x": 2.5876965838813497, - "y": 6.693959901940825 + "x": 2.740325263749712, + "y": 6.780628363502267 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ "y": 5.269831325301205 }, "prevControl": { - "x": 2.626970566775512, - "y": 5.022952408768852 + "x": 2.6197177782925976, + "y": 5.60770395147218 }, "nextControl": null, "isLocked": false, @@ -74,13 +74,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -40.601294645004494 + "rotation": 145.263577656602 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -177.66687659440373 + "rotation": -5.020341700004992 }, "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 index 79f1204..4f3d9a9 100644 --- a/src/main/deploy/pathplanner/paths/R3.path +++ b/src/main/deploy/pathplanner/paths/R3.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.593518983931554, - "y": 1.4505471402803671 + "x": 6.628907859032533, + "y": 1.418560826553372 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 0.7020481927710842 }, "prevControl": { - "x": 6.788520440156554, - "y": 0.9251369332284056 + "x": 6.776606455949892, + "y": 0.907849549399763 }, "nextControl": { - "x": 5.9941555915520075, - "y": 0.6098111202585716 + "x": 5.982340608807217, + "y": 0.61069550339358 }, "isLocked": false, "linkedName": null @@ -36,24 +36,24 @@ "y": 0.7020481927710842 }, "prevControl": { - "x": 3.7550118117890667, - "y": 0.6357757561431316 + "x": 3.044562187542487, + "y": 0.619758952159487 }, "nextControl": { - "x": 2.272922602581813, - "y": 0.7336542780199183 + "x": 2.131443827357652, + "y": 0.8761596896991247 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.7352289156626508, - "y": 1.6199759036144585 + "x": 1.735, + "y": 2.062 }, "prevControl": { - "x": 1.9769027739250977, - "y": 1.5559935135954637 + "x": 1.9184873757368757, + "y": 1.8921989901526062 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index 401907c..eb83f6b 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -18,6 +18,7 @@ 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; @@ -269,6 +270,29 @@ public RobotContainer() { 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() { From 73029aeab12ec6e6ed69fce3d8be7112a0bde395 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Thu, 12 Mar 2026 21:30:40 -0400 Subject: [PATCH 16/16] final tuning --- src/main/java/org/team2342/frc/Constants.java | 8 +- src/main/java/org/team2342/frc/Robot.java | 4 +- .../java/org/team2342/frc/RobotContainer.java | 81 +++++++++++-------- .../team2342/frc/subsystems/Conductor.java | 7 +- .../frc/subsystems/indexer/Indexer.java | 6 ++ .../team2342/frc/subsystems/intake/Pivot.java | 11 ++- .../frc/subsystems/leds/LEDSubsystem.java | 21 ++--- .../frc/subsystems/shooter/Flywheel.java | 12 ++- .../org/team2342/frc/util/FiringSolver.java | 24 ++++-- 9 files changed, 112 insertions(+), 62 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index b627b0b..30ae940 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -139,7 +139,7 @@ public static final class ConductorConstants { } public static final class IndexerConstants { - public static final double RUN_VOLTAGE = 5.0; + public static final double RUN_VOLTAGE = 6; public static final double RUN_CURRENT = 30.0; public static final MotorConfig INDEXER_MOTOR_CONFIG = @@ -198,7 +198,7 @@ public static final class ShooterConstants { 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 = 1.0; + public static final double FLYWHEEL_AT_GOAL_TOLERANCE = 3.0; public static final PIDFFConfigs FLYWHEEL_PID_CONFIGS = new PIDFFConfigs() @@ -229,7 +229,7 @@ public static final class ShooterConstants { } public static final class KickerConstants { - public static final double RUN_VOLTAGE = 8.0; + public static final double RUN_VOLTAGE = 7.5; public static final MotorConfig KICKER_CONFIG = new MotorConfig() @@ -261,7 +261,7 @@ public static final class TurretConstants { .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); diff --git a/src/main/java/org/team2342/frc/Robot.java b/src/main/java/org/team2342/frc/Robot.java index 9d0d963..6d9845e 100644 --- a/src/main/java/org/team2342/frc/Robot.java +++ b/src/main/java/org/team2342/frc/Robot.java @@ -175,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 eb83f6b..838c377 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -8,7 +8,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Alert; @@ -59,6 +58,7 @@ 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; @@ -101,7 +101,6 @@ public class RobotContainer { private final Trigger allianceZoneTrigger; private final Trigger shiftAboutToEnd; private final Trigger activeOrPassing; - private final Trigger readyToFire; public RobotContainer() { switch (Constants.CURRENT_MODE) { @@ -154,7 +153,11 @@ public RobotContainer() { conductor = new Conductor(flywheel, turret, drive::getPose, drive::getChassisSpeeds); leds = - new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState); + new LEDSubsystem( + new LedIOCANdle(CANConstants.CANDLE_ID, 32), + "CANdle", + vision::hasTags, + conductor::getCurrentState); LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev); break; @@ -239,6 +242,15 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); autoChooser.get(); + autoChooser.addOption( + "Point and fire", + conductor + .runState(ConductorState.WARM_UP) + .withTimeout(2.0) + .alongWith(pivot.holdAngle(0)) + .alongWith(wheels.in()) + .alongWith(indexer.in()) + .alongWith(kicker.in())); if (Constants.TUNING) setupDevelopmentRoutines(); @@ -263,36 +275,35 @@ public RobotContainer() { || FiringSolver.getInstance() .calculate(drive.getChassisSpeeds(), drive.getPose()) .passing()); - readyToFire = new Trigger(() -> flywheel.atGoal() && turret.atGoal()); configureBindings(); } private void configureNamedCommands() { NamedCommands.registerCommand("Named Command Test", Commands.print("Named Command Test")); - NamedCommands.registerCommand("autoShoot", - conductor.runState(ConductorState.TRACKED_FIRING) + 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(); - }) - ); + .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() { @@ -340,15 +351,23 @@ private void configureBindings() { .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.runState(ConductorState.TRACKED_FIRING)) .and(activeOrPassing) - .and(readyToFire.debounce(0.2, DebounceType.kFalling)) - .whileTrue( - Commands.parallel(indexer.in(), kicker.in(), pivot.agitate(), wheels.runIntake(3))) - .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), pivot.stop(), wheels.stop())); + .whileTrue(Commands.parallel(indexer.in(), kicker.in())) + .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); // Firing during inactive period driverController @@ -360,10 +379,8 @@ private void configureBindings() { driverController .rightBumper() .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING)) - .and(readyToFire.debounce(0.2, DebounceType.kFalling)) - .whileTrue( - Commands.parallel(indexer.in(), kicker.in(), pivot.agitate(), wheels.runIntake(3))) - .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), pivot.stop(), wheels.stop())); + .whileTrue(Commands.parallel(indexer.in(), kicker.in())) + .onFalse(Commands.parallel(indexer.stop(), kicker.stop())); // Location Triggers allianceZoneTrigger diff --git a/src/main/java/org/team2342/frc/subsystems/Conductor.java b/src/main/java/org/team2342/frc/subsystems/Conductor.java index acfd718..cffb130 100644 --- a/src/main/java/org/team2342/frc/subsystems/Conductor.java +++ b/src/main/java/org/team2342/frc/subsystems/Conductor.java @@ -96,7 +96,12 @@ private void setupStateCommands() { FiringSolver.getInstance() .calculate(velocitySupplier.get(), poseSupplier.get()) .turretAngle()) - .alongWith(flywheel.warmUp())); + .alongWith( + flywheel.warmUpCommand( + () -> + FiringSolver.getInstance() + .calculate(velocitySupplier.get(), poseSupplier.get()) + .wheelSpeed()))); fsm.addStateCommand( ConductorState.TRACKED_FIRING, 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 dca8844..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; @@ -48,6 +49,11 @@ public Command out() { return run(() -> motor.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(() -> 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 10bdccc..a7f09f6 100644 --- a/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java +++ b/src/main/java/org/team2342/frc/subsystems/intake/Pivot.java @@ -25,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 @@ -54,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"); } @@ -67,7 +71,6 @@ public Command stop() { } public Command agitate() { - return Commands.repeatingSequence( - holdAngle(1.0).withTimeout(0.5), holdAngle(IntakeConstants.MIN_ANGLE).withTimeout(0.5)); + 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 index 6cdbca8..b5a7ecb 100644 --- a/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java +++ b/src/main/java/org/team2342/frc/subsystems/leds/LEDSubsystem.java @@ -7,6 +7,7 @@ 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; @@ -16,6 +17,7 @@ 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; @@ -30,7 +32,7 @@ public class LEDSubsystem extends SubsystemBase { private final Supplier stateSupplier; private boolean hasEnabled = false; - private Debouncer visionDebouncer = new Debouncer(0.2); + private Debouncer visionDebouncer = new Debouncer(0.5, DebounceType.kBoth); public LEDSubsystem( LedIO io, String name, BooleanSupplier hasTags, Supplier stateSupplier) { @@ -52,14 +54,14 @@ public void periodic() { 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); - }; + 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)); @@ -88,7 +90,6 @@ public void setSecond(LEDEffect effect) { } public void setAll(LEDEffect effect) { - setFirst(effect); - setSecond(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 3102714..0826551 100644 --- a/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java +++ b/src/main/java/org/team2342/frc/subsystems/shooter/Flywheel.java @@ -80,8 +80,8 @@ public void runVelocity(DoubleSupplier 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); @@ -96,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) { diff --git a/src/main/java/org/team2342/frc/util/FiringSolver.java b/src/main/java/org/team2342/frc/util/FiringSolver.java index 7e92851..50f8c7f 100644 --- a/src/main/java/org/team2342/frc/util/FiringSolver.java +++ b/src/main/java/org/team2342/frc/util/FiringSolver.java @@ -31,14 +31,26 @@ public class FiringSolver { // TODO: tune real maps static { - speedMap.put(1.141, 15.0); - speedMap.put(1.445, 17.0); - - MIN_TOF = 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.141, 12.66 - 11.46); - tofMap.put(1.445, 8.34 - 6.95); + 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() {