From 613e220ed29daa6f34423e2a560a63e0963ed708 Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Sun, 25 Jan 2026 13:31:02 -0800 Subject: [PATCH 1/5] syncing current changes. --- .vscode/settings.json | 3 +- src/main/java/frc/robot/Autos.java | 4 +- src/main/java/frc/robot/Robot.java | 316 +++++++++++------- src/main/java/frc/robot/Superstructure.java | 171 +++++++--- .../frc/robot/components/camera/Camera.java | 4 +- .../components/canrange/CANrangeIOReal.java | 5 +- .../robot/components/rollers/RollerIO.java | 11 +- .../subsystems/climber/ClimberSubsystem.java | 17 + .../frc/robot/subsystems/indexer/Indexer.java | 29 ++ ...rSubsystem.java => LindexerSubsystem.java} | 45 ++- .../indexer/SpindexerSubsystem.java | 61 ++++ ...keSubsystem.java => FintakeSubsystem.java} | 33 +- .../frc/robot/subsystems/intake/Intake.java | 19 ++ .../subsystems/intake/LintakeSubsystem.java | 37 ++ .../robot/subsystems/shooter/FlywheelIO.java | 31 +- .../frc/robot/subsystems/shooter/HoodIO.java | 35 +- .../frc/robot/subsystems/shooter/Shooter.java | 43 +++ .../subsystems/shooter/ShooterSubsystem.java | 54 ++- .../subsystems/shooter/TurretSubsystem.java | 69 ++++ .../subsystems/swerve/SwerveSubsystem.java | 30 +- .../constants/AlphaSwerveConstants.java | 7 - .../java/frc/robot/utils/autoaim/AutoAim.java | 32 +- .../utils/autoaim/InterpolatingShotTree.java | 6 +- 23 files changed, 806 insertions(+), 256 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/indexer/Indexer.java rename src/main/java/frc/robot/subsystems/indexer/{IndexerSubsystem.java => LindexerSubsystem.java} (84%) create mode 100644 src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java rename src/main/java/frc/robot/subsystems/intake/{IntakeSubsystem.java => FintakeSubsystem.java} (73%) create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/Shooter.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 7399d64..50e3d8c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -59,5 +59,6 @@ ], "java.dependency.enableDependencyCheckup": false, "wpilib.selectDefaultSimulateExtension": false, - "wpilib.skipSelectSimulateExtension": true + "wpilib.skipSelectSimulateExtension": true, + "wpilib.autoStartRioLog": false } diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index ebad639..97a4364 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Robot.RobotType; +import frc.robot.Robot.RobotMode; import frc.robot.subsystems.swerve.SwerveSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -78,7 +78,7 @@ public Autos(SwerveSubsystem swerve) { true, swerve, (traj, edge) -> { - if (Robot.ROBOT_TYPE != RobotType.REAL) + if (Robot.ROBOT_MODE != RobotMode.REAL) Logger.recordOutput( "Choreo/Active Traj", DriverStation.getAlliance().isPresent() diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 036f351..d6d290e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,15 +30,22 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; -import frc.robot.subsystems.indexer.IndexerSubsystem; -import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.climber.ClimberSubsystem; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.indexer.LindexerSubsystem; +import frc.robot.subsystems.indexer.SpindexerSubsystem; +import frc.robot.subsystems.intake.FintakeSubsystem; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.LintakeSubsystem; import frc.robot.subsystems.led.LEDIOReal; import frc.robot.subsystems.led.LEDSubsystem; import frc.robot.subsystems.shooter.FlywheelIO; import frc.robot.subsystems.shooter.FlywheelIOSim; import frc.robot.subsystems.shooter.HoodIO; import frc.robot.subsystems.shooter.HoodIOSim; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; @@ -55,16 +62,47 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { - public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; - public static final boolean TUNING_MODE = true; - public boolean hasZeroedSinceStartup = false; - - public enum RobotType { + /** This is whether or not the robot is real, sim, or replay */ + public enum RobotMode { REAL, SIM, REPLAY } + /** This is whether or not the robot is Alpha or Comp */ + public enum RobotEdition { + ALPHA, + COMP + } + + public static final RobotMode ROBOT_MODE = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM; + // public static final RobotEdition ROBOT_EDITION = RobotEdition.COMP; + public static final RobotEdition ROBOT_EDITION; + + // TODO get rio serial numbers + static { + switch (RobotController.getSerialNumber()) { + case "1": + ROBOT_EDITION = RobotEdition.ALPHA; + break; + case "2": + ROBOT_EDITION = RobotEdition.COMP; + break; + default: + // defaulting to comp is probably safer? + ROBOT_EDITION = RobotEdition.COMP; + } + } + + /** + * This is for when we're testing shot and extension numbers and should be FALSE once bring up is + * complete + */ + public static final boolean TUNING_MODE = true; + + public boolean hasZeroedSinceStartup = false; + + // Add stuff related to dashboard alerts private final Alert driverJoystickDisconnectedAlert = new Alert("Driver controller disconnected!", AlertType.kError); private final Alert operatorJoystickDisconnectedAlert = @@ -77,8 +115,12 @@ public enum RobotType { new Alert( "Battery voltage is very low, consider turning off the robot or replacing the battery.", AlertType.kWarning); - - private static CANBus canivore = new CANBus("*"); + private final Alert wrongRobotAlert = + new Alert( + "!! ALPHA CODE HAS BEEN DEPLOYED TO COMP BOT THIS IS VERY BAD !!", AlertType.kError); + private final Alert tuningModeAlert = + new Alert( + "! Tuning mode is enabled while FMS attached, this is Quite bad !", AlertType.kWarning); private final Timer canInitialErrorTimer = new Timer(); private final Timer canErrorTimer = new Timer(); @@ -92,85 +134,40 @@ public enum RobotType { private static final double lowBatteryDisabledTime = 1.5; private static final double lowBatteryMinCycleCount = 10; - // Instantiate subsystems + /** + * As per the 2026+ ctre api we should be passing in the actual canbus object to any ctre device + * constructors, NOT the name as a string + */ + public static CANBus canivore = new CANBus("*"); + + // Declare subsystems that aren't part of the superstructure + // This means they can do stuff regardless of what state the robot's in + // since none of them are dependent on the state - f.e. climber can do what it wants + // Subsystems that *are* part of the superstructure are initialized later - // Subsystem initialization + // swervesubsystem decides on its own whether or not to use alpha or comp swerve constants private final SwerveSubsystem swerve = new SwerveSubsystem(canivore); - private final IndexerSubsystem indexer = - new IndexerSubsystem( - canivore, - (ROBOT_TYPE == RobotType.REAL) - ? new RollerIO(9, IndexerSubsystem.getIndexerConfigs(), canivore) - : new RollerIOSim( - 9, - IndexerSubsystem.getIndexerConfigs(), - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.003, IndexerSubsystem.GEAR_RATIO), - DCMotor.getKrakenX44Foc(1)), - MotorType.KrakenX44, - canivore), - (ROBOT_TYPE == RobotType.REAL) - ? new RollerIO(10, IndexerSubsystem.getIndexerConfigs(), canivore) - : new RollerIOSim( - 10, - IndexerSubsystem.getKickerConfigs(), - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.00001, IndexerSubsystem.KICKER_GEAR_RATIO), - DCMotor.getKrakenX44Foc(1)), - MotorType.KrakenX44, - canivore)); - - // canivore, new RollerIOReal(0, IndexerSubsystem.getIndexerConfigs())); private final LEDSubsystem leds; - private final ShooterSubsystem shooter = - new ShooterSubsystem( - ROBOT_TYPE == RobotType.REAL - ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) - : new HoodIOSim(canivore), - ROBOT_TYPE == RobotType.REAL - ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); - private final IntakeSubsystem intake = - new IntakeSubsystem( - (ROBOT_TYPE == RobotType.REAL) - ? new RollerIO(8, IntakeSubsystem.getIntakeConfig(), canivore) - : new RollerIOSim( - 8, - IntakeSubsystem.getIntakeConfig(), - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.001, IntakeSubsystem.GEAR_RATIO), - DCMotor.getKrakenX44Foc(1)), - MotorType.KrakenX44, - canivore)); + + // climber only exists for the comp bot - this is accounted for later + private ClimberSubsystem climber; + + private final Superstructure superstructure; private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); + // Assign non-superstructure triggers @AutoLogOutput(key = "Superstructure/Autoaim Request") private Trigger autoAimReq = driver.rightBumper().or(driver.leftBumper()); - @AutoLogOutput(key = "Robot/Pre Zeroing Request") - private Trigger preZeroingReq = driver.a(); - - @AutoLogOutput(key = "Robot/Zeroing Request") - private Trigger zeroingReq = driver.b(); - - private final Superstructure superstructure = - new Superstructure(swerve, indexer, intake, shooter, driver, operator); - + // Auto stuff private final Autos autos; private Optional lastAlliance = Optional.empty(); - @AutoLogOutput boolean haveAutosGenerated = false; - private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Autos"); - // Logged mechanisms - - // temporarily override map with empty map to avoid collisions swith reefscape elements + // temporarily override map with empty map to avoid collisions with reefscape elements // unfortunately this also turns off collisions with walls but that's fine // TODO update once rebuilt is added to maplesim private static class EvergreenArena extends SimulatedArena { @@ -186,18 +183,109 @@ public void placeGamePiecesOnField() {} SimulatedArena.overrideInstance(new EvergreenArena()); } + // this is here because it doesn't like that the power distribution logger is never closed @SuppressWarnings("resource") public Robot() { + + // these have to be instantiated as null to ensure that there's always *something* in it + // although it always gets assigned to the robot-specific version in the switch-case statement + // below, the compiler doesn't technically know that for sure + // that would be bad because if for some reason we had some case of ROBOT_EDITION that wasn't + // accounted for, it wouldn't be able to pass anything to the superstructure below and it would + // break + // granted this would never actually happen but + Indexer indexer = null; + Intake intake = null; + Shooter shooter = null; + + // this looks at the ROBOT_EDITION variable and decides which version of each subsystem to + // create based on that + // alpha: lindexer, fintake, shooter + // comp: spindexer, lintake, turret + switch (ROBOT_EDITION) { + case ALPHA: + indexer = + new LindexerSubsystem( + canivore, + (ROBOT_MODE == RobotMode.REAL) + ? new RollerIO(9, LindexerSubsystem.getIndexerConfigs(), canivore) + : new RollerIOSim( + 9, + LindexerSubsystem.getIndexerConfigs(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.003, LindexerSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore), + (ROBOT_MODE == RobotMode.REAL) + ? new RollerIO(10, LindexerSubsystem.getIndexerConfigs(), canivore) + : new RollerIOSim( + 10, + LindexerSubsystem.getKickerConfigs(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), + 0.00001, + LindexerSubsystem.KICKER_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore)); + + shooter = + new ShooterSubsystem( + ROBOT_MODE == RobotMode.REAL + ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) + : new HoodIOSim(canivore), + ROBOT_MODE == RobotMode.REAL + ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); + + intake = + new FintakeSubsystem( + (ROBOT_MODE == RobotMode.REAL) + ? new RollerIO(8, FintakeSubsystem.getIntakeConfig(), canivore) + : new RollerIOSim( + 8, + FintakeSubsystem.getIntakeConfig(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.001, FintakeSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore)); + // note that the climber is not instantiated here + break; + case COMP: + indexer = new SpindexerSubsystem(); + intake = new LintakeSubsystem(); + shooter = new TurretSubsystem(); + climber = new ClimberSubsystem(); // TODO climber + break; + } + // now that we've assigned the correct subsystems based on robot edition, we can pass them into + // the superstructure + superstructure = new Superstructure(swerve, indexer, intake, shooter, driver, operator); + // if this is alpha, we won't have assigned a climber yet + // this creates a placeholder "no-operation" climber that will just not do anything, but is not + // null (and we need it to be not null) + if (climber == null) + climber = new ClimberSubsystem(); // TODO new ClimberSubsystem(new ClimberIO() {}) and such + DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); RobotController.setBrownoutVoltage(6.0); + // Metadata about the current code running on the robot - Logger.recordMetadata("Codebase", "2026 Template"); + Logger.recordMetadata("Codebase", "Rebuilt"); Logger.recordMetadata("RuntimeType", getRuntimeType().toString()); - Logger.recordMetadata("Robot Mode", ROBOT_TYPE.toString()); + Logger.recordMetadata("Robot Mode", ROBOT_MODE.toString()); + Logger.recordMetadata("Robot Edition", ROBOT_EDITION.toString()); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + + // log if we have uncommitted changes switch (BuildConstants.DIRTY) { case 0: Logger.recordMetadata("GitDirty", "All changes committed"); @@ -210,11 +298,16 @@ public Robot() { break; } - switch (ROBOT_TYPE) { + // set up logging stuff depending on robot mode + switch (ROBOT_MODE) { case REAL: Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - new PowerDistribution(1, ModuleType.kCTRE); // Enables power distribution logging + // TODO confirm pdp vs pdh + // apparently LoggedPowerDistribution doesn't work with the pdp 2.0 + // LoggedPowerDistribution.getInstance(1, ModuleType.kCTRE); // Enables power distribution + // logging + new PowerDistribution(1, ModuleType.kCTRE); break; case REPLAY: setUseTiming(false); // Run as fast as possible @@ -234,16 +327,15 @@ public Robot() { // be added. Logger.recordOutput("Canivore Status", canivore.getStatus().Status); + Logger.recordOutput("Robot Edition", ROBOT_EDITION); PhoenixOdometryThread.getInstance().start(); - leds = new LEDSubsystem(new LEDIOReal()); + leds = new LEDSubsystem(new LEDIOReal()); // TODO sim // Set default commands - driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); - swerve.setDefaultCommand( swerve.driveOpenLoopFieldRelative( () -> @@ -258,39 +350,14 @@ public Robot() { addControllerBindings(); + // Auto things autos = new Autos(swerve); autoChooser.addDefaultOption("None", Commands.none()); - // Generates autos on connected - new Trigger( - () -> - DriverStation.isDSAttached() - && DriverStation.getAlliance().isPresent() - && !haveAutosGenerated) - .onTrue(Commands.print("Connected")) - .onTrue(Commands.runOnce(this::addAutos).ignoringDisable(true)); - - new Trigger( - () -> { - boolean allianceChanged = !DriverStation.getAlliance().equals(lastAlliance); - lastAlliance = DriverStation.getAlliance(); - return allianceChanged && DriverStation.getAlliance().isPresent(); - }) - .onTrue(Commands.runOnce(this::addAutos).ignoringDisable(true)); - // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); - CommandScheduler.getInstance() - .onCommandInterrupt( - (interrupted, interrupting) -> { - System.out.println("Interrupted: " + interrupted); - System.out.println( - "Interrputing: " - + (interrupting.isPresent() ? interrupting.get().getName() : "none")); - }); - // Add autos on alliance change new Trigger( () -> { @@ -315,13 +382,26 @@ public Robot() { Commands.runOnce(() -> addAutos()) .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); + SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); + // SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true)); + // SmartDashboard.putData("Test Shot", shooter.testShoot()); // Reset alert timers canInitialErrorTimer.restart(); canErrorTimer.restart(); canivoreErrorTimer.restart(); disabledTimer.restart(); + + // log when commands get interrupted + CommandScheduler.getInstance() + .onCommandInterrupt( + (interrupted, interrupting) -> { + System.out.println("Interrupted: " + interrupted); + System.out.println( + "Interrputing: " + + (interrupting.isPresent() ? interrupting.get().getName() : "none")); + }); } /** Scales a joystick value for teleop driving */ @@ -329,7 +409,6 @@ private static double modifyJoystick(double val) { return MathUtil.applyDeadband(Math.abs(Math.pow(val, 2)) * Math.signum(val), 0.02); } - @SuppressWarnings("unlikely-arg-type") private void addControllerBindings() { // heading reset driver @@ -356,9 +435,11 @@ private void addControllerBindings() { () -> modifyJoystick(driver.getLeftX()) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); + // TODO add binding for climb // ---zeroing stuff--- + // create triggers for joystick disconnect alerts new Trigger(() -> DriverStation.isJoystickConnected(0)) .negate() .onTrue(Commands.runOnce(() -> driverJoystickDisconnectedAlert.set(true))) @@ -376,10 +457,10 @@ private void addAutos() { "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString())); // Sysid Autos - autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); - autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); - autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); - autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); + // autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); + // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); + // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); + // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); haveAutosGenerated = true; System.out.println("Done generating autos"); } @@ -387,10 +468,21 @@ private void addAutos() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + superstructure.periodic(); - // Log mechanism poses + // TODO Log mechanism poses + updateAlerts(); + } + + public void updateAlerts() { + + if (ROBOT_EDITION == RobotEdition.ALPHA && DriverStation.isFMSAttached()) { + wrongRobotAlert.set(true); + // TODO make leds strobe red or something + } + if (TUNING_MODE && DriverStation.isFMSAttached()) tuningModeAlert.set(true); // Check CAN status var canStatus = RobotController.getCANStatus(); if (canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0) { diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 9be6815..0ce8617 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -5,13 +5,14 @@ package frc.robot; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.indexer.IndexerSubsystem; -import frc.robot.subsystems.intake.IntakeSubsystem; -import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils.FeedTargets; @@ -46,14 +47,20 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/State") private static SuperState state = SuperState.IDLE; + @AutoLogOutput(key = "Scoring/Scoring Active") + public boolean isScoringActive = + isOurShift(); // assuming we want the dashboard to show if the time allows us to score not if + + // its litterly possible + private SuperState prevState = SuperState.IDLE; private Timer stateTimer = new Timer(); private final SwerveSubsystem swerve; - private final IndexerSubsystem indexer; - private final IntakeSubsystem intake; - private final ShooterSubsystem shooter; + private final Indexer indexer; + private final Intake intake; + private final Shooter shooter; private final CommandXboxControllerSubsystem driver; private final CommandXboxControllerSubsystem operator; @@ -67,8 +74,8 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/Feed Request") private Trigger feedReq; - @AutoLogOutput(key = "Superstructure/Flowstate Request") - private Trigger flowReq; + // @AutoLogOutput(key = "Superstructure/Flowstate Request") + // private Trigger flowReq; @AutoLogOutput(key = "Superstructure/Anti Jam Req") private Trigger antiJamReq; @@ -87,9 +94,9 @@ public Trigger getTrigger() { /** Creates a new Superstructure. */ public Superstructure( SwerveSubsystem swerve, - IndexerSubsystem indexer, - IntakeSubsystem intake, - ShooterSubsystem shooter, + Indexer indexer, + Intake intake, + Shooter shooter, CommandXboxControllerSubsystem driver, CommandXboxControllerSubsystem operator) { this.swerve = swerve; @@ -115,77 +122,88 @@ private void addTriggers() { driver .rightTrigger() .and(DriverStation::isTeleop) - .and(() -> shouldFeed == false) + .and(() -> canScore()) .or(Autos.autoScoreReq); // Maybe should include if its our turn? intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); - feedReq = - driver - .rightBumper() - .and(DriverStation::isTeleop) - .and(() -> shouldFeed == true) - .or(Autos.autoFeedReq); + feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); - flowReq = driver.leftTrigger().and(driver.rightTrigger()); + // flowReq = driver.leftTrigger().and(driver.rightTrigger()); antiJamReq = driver.a().or(operator.a()); - isFull = new Trigger(indexer::isFull); + isFull = new Trigger(indexer::isFull).debounce(0.5); // TODO tune isEmpty = new Trigger(indexer::isEmpty); } private void addTransitions() { - bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq); + bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq.and(scoreReq.negate())); bindTransition(SuperState.INTAKE, SuperState.IDLE, intakeReq.negate().and(isEmpty)); bindTransition( - SuperState.INTAKE, SuperState.READY, (intakeReq.negate().and(isEmpty.negate())).or(isFull)); + SuperState.INTAKE, + SuperState.READY, + (intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate()))); + // .or(isFull)); - bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq); + // bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq); - bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); + bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq); + // .and(isFull.negate())); bindTransition(SuperState.READY, SuperState.SPIN_UP_SCORE, scoreReq); bindTransition( SuperState.SPIN_UP_SCORE, SuperState.SCORE, - new Trigger(shooter::atFlywheelVelocitySetpoint)); + new Trigger(shooter::atFlywheelVelocitySetpoint) + .debounce(0.5) + .and(new Trigger(shooter::atHoodSetpoint).debounce(0.5)) + .and(() -> stateTimer.hasElapsed(0.5))); - bindTransition( - SuperState.SPIN_UP_FEED, SuperState.FEED, new Trigger(shooter::atFlywheelVelocitySetpoint)); + // bindTransition( + // SuperState.SPIN_UP_FEED, + // SuperState.FEED, + // new Trigger(shooter::atFlywheelVelocitySetpoint) + // .and(() -> stateTimer.hasElapsed(0.2)) + // .and(shooter::atHoodSetpoint)); - bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); + // bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); - bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty); + bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty.debounce(0.5).and(scoreReq.negate())); // FEED_FLOW transitions - { - bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); + // { + // bindTransition(SuperState.FEED, SuperState.FEED_FLOW, intakeReq.and(feedReq)); - bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq)); + // bindTransition(SuperState.FEED_FLOW, SuperState.FEED, intakeReq.negate().and(feedReq)); - bindTransition( - SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); + // bindTransition( + // SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); - // No so sure about the end condition here. - bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); - } + // // No so sure about the end condition here. + // bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); + // } // SCORE_FLOW transitions { - bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); + bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, scoreReq.and(intakeReq)); - bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq)); + bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, intakeReq.negate().and(scoreReq)); bindTransition( - SuperState.SCORE_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); + SuperState.SCORE_FLOW, + SuperState.READY, + intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate())); // No so sure about the end condition here. - bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); + bindTransition( + SuperState.SCORE_FLOW, + SuperState.IDLE, + intakeReq.negate().and(scoreReq.negate()).and(isEmpty)); } // Transition from any state to SPIT for anti jamming @@ -206,11 +224,14 @@ private void addCommands() { bindCommands( SuperState.READY, intake.rest(), - indexer.index(), + indexer.rest(), shooter.rest()); // Maybe index at slower speed? bindCommands( - SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), shooter.shoot(swerve::getPose)); + SuperState.SPIN_UP_SCORE, + intake.rest(), + indexer.rest(), /*shooter.shoot(swerve::getPose)*/ + shooter.testShoot()); bindCommands( SuperState.SPIN_UP_FEED, @@ -220,10 +241,12 @@ private void addCommands() { swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC bindCommands( - SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose)); + SuperState.SCORE, + intake.rest(), + indexer.kick(), /*shooter.shoot(swerve::getPose)*/ + shooter.testShoot()); - bindCommands( - SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); + bindCommands(SuperState.SCORE_FLOW, intake.intake(), indexer.kick(), shooter.testShoot()); bindCommands( SuperState.FEED, @@ -239,7 +262,7 @@ private void addCommands() { indexer.index(), shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); - bindCommands(SuperState.SPIT, intake.outake(), indexer.outtake(), shooter.spit()); + bindCommands(SuperState.SPIT, intake.outtake(), indexer.spit(), shooter.spit()); } public void periodic() { @@ -327,4 +350,58 @@ public static SuperState getState() { public boolean stateIsIdle() { return getState() == SuperState.IDLE; } + + private Alliance getStartingAlliance() { + String gameData = DriverStation.getGameSpecificMessage(); + // gives first inactive alliance + if (gameData.length() > 0) { + switch (gameData.charAt(0)) { + case 'B': + return Alliance.Red; + case 'R': + return Alliance.Blue; + default: + return Alliance.Blue; + } + } else { + // not sure + return Alliance.Blue; + } + } + + private int getCurrentShift() { + double timeLeftinMatch = Timer.getMatchTime(); + // may be a nicer way to do this + if (105.00 <= timeLeftinMatch && timeLeftinMatch <= 130.00) { + return 1; + } else if (80.00 <= timeLeftinMatch && timeLeftinMatch <= 105.00) { + return 2; + } else if ((55.00 <= timeLeftinMatch && timeLeftinMatch <= 80.00)) { + return 3; + } else if ((30.00 <= timeLeftinMatch && timeLeftinMatch <= 55.00)) { + return 4; + } else { + return 0; + } + } + + public boolean isOurShift() { + // only cant score when its the others turn, otherwise everyone can + if (getStartingAlliance() == DriverStation.getAlliance().orElse(Alliance.Blue)) { + return !(getCurrentShift() == 2 || getCurrentShift() == 4); + } else { + return !(getCurrentShift() == 1 || getCurrentShift() == 3); + } + } + + public boolean inScoringArea() { + return (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + && (swerve.getPose().getX() <= 4.6914191246032715) + || DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red + && (swerve.getPose().getX() >= 11.889562606811523)); + } + + public boolean canScore() { + return isOurShift() && inScoringArea(); + } } diff --git a/src/main/java/frc/robot/components/camera/Camera.java b/src/main/java/frc/robot/components/camera/Camera.java index c26cc50..2ff9563 100644 --- a/src/main/java/frc/robot/components/camera/Camera.java +++ b/src/main/java/frc/robot/components/camera/Camera.java @@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Robot; -import frc.robot.Robot.RobotType; +import frc.robot.Robot.RobotMode; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.Tracer; import java.util.NoSuchElementException; @@ -106,7 +106,7 @@ public Optional update(PhotonPipelineResult result) { if (result.getTargets().size() < 1) { return Optional.empty(); } - if (Robot.ROBOT_TYPE != RobotType.REAL) + if (Robot.ROBOT_MODE != RobotMode.REAL) Logger.recordOutput( "Vision/" + io.getName() + "/Best Distance", result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); diff --git a/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java b/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java index 0548179..343ed4f 100644 --- a/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java +++ b/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java @@ -17,14 +17,15 @@ public class CANrangeIOReal implements CANrangeIO { private final StatusSignal distance; private final StatusSignal isDetected; - public CANrangeIOReal(int CANrangeID, CANBus canbus) { + public CANrangeIOReal(int CANrangeID, CANBus canbus, double xFovDegrees) { canrange = new CANrange(CANrangeID, canbus); distance = canrange.getDistance(); isDetected = canrange.getIsDetected(); final CANrangeConfiguration config = new CANrangeConfiguration(); config.ToFParams.UpdateFrequency = 50; // update frequency in Hz - config.ProximityParams.ProximityThreshold = 0.05; + config.ProximityParams.ProximityThreshold = 0.254; + config.FovParams.FOVRangeX = xFovDegrees; BaseStatusSignal.setUpdateFrequencyForAll(50.0, distance, isDetected); diff --git a/src/main/java/frc/robot/components/rollers/RollerIO.java b/src/main/java/frc/robot/components/rollers/RollerIO.java index 1575259..46cea56 100644 --- a/src/main/java/frc/robot/components/rollers/RollerIO.java +++ b/src/main/java/frc/robot/components/rollers/RollerIO.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; @@ -23,6 +24,7 @@ public static class RollerIOInputs { public double appliedVoltage = 0.0; public double statorCurrentAmps = 0.0; public double motorTemperatureCelsius = 0.0; + public double motorPositionRotations = 0.0; } protected final TalonFX motor; @@ -32,6 +34,7 @@ public static class RollerIOInputs { private final StatusSignal appliedVoltage; private final StatusSignal statorCurrentAmps; private final StatusSignal motorTemperatureCelsius; + private final StatusSignal motorPosition; private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private final VelocityVoltage velocityVoltage = @@ -47,6 +50,7 @@ public RollerIO(int motorID, TalonFXConfiguration config, CANBus canbus) { appliedVoltage = motor.getMotorVoltage(); statorCurrentAmps = motor.getStatorCurrent(); motorTemperatureCelsius = motor.getDeviceTemp(); + motorPosition = motor.getPosition(); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, @@ -54,7 +58,8 @@ public RollerIO(int motorID, TalonFXConfiguration config, CANBus canbus) { supplyCurrentAmps, statorCurrentAmps, appliedVoltage, - motorTemperatureCelsius); + motorTemperatureCelsius, + motorPosition); motor.getConfigurator().apply(config); motor.optimizeBusUtilization(); @@ -66,13 +71,15 @@ public void updateInputs(RollerIOInputs inputs) { supplyCurrentAmps, appliedVoltage, statorCurrentAmps, - motorTemperatureCelsius); + motorTemperatureCelsius, + motorPosition); inputs.velocityRotsPerSec = angularVelocityRotsPerSec.getValueAsDouble(); inputs.supplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); inputs.motorTemperatureCelsius = motorTemperatureCelsius.getValueAsDouble(); + inputs.motorPositionRotations = motorPosition.getValueAsDouble(); } public void setRollerVoltage(double volts) { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java new file mode 100644 index 0000000..2b5d698 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ClimberSubsystem extends SubsystemBase { + /** Creates a new ClimberSubsystem. */ + public ClimberSubsystem() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 0000000..6bd3118 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.indexer; + +import edu.wpi.first.wpilibj2.command.Command; + +/** Add your docs here. */ +public interface Indexer { + + public boolean isFull(); + + public boolean isEmpty(); + + public boolean isPartiallyFull(); + + /** Run indexer towards shooter and kicker away from shooter */ + public Command index(); + + /** Run everything backwards. This is for antijamming the robot */ + public Command spit(); + + /** Run both indexer and kicker towards the shooter */ + public Command kick(); + + /** Not running (set to 0) */ + public Command rest(); +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java similarity index 84% rename from src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java rename to src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index af761a4..44cceff 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java @@ -17,7 +17,8 @@ import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; -public class IndexerSubsystem extends SubsystemBase { +/** Lindexer = Linear Indexer. !! ALPHA !! */ +public class LindexerSubsystem extends SubsystemBase implements Indexer { public static final double GEAR_RATIO = 2.0; private CANrangeIOReal firstCANRangeIO; @@ -39,58 +40,55 @@ public class IndexerSubsystem extends SubsystemBase { null, null, null, - (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state)), + (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state.toString())), new Mechanism((volts) -> indexRollerIO.setRollerVoltage(volts.in(Volts)), null, this)); public static final double MAX_ACCELERATION = 10.0; public static final double MAX_VELOCITY = 10.0; public static final double KICKER_GEAR_RATIO = 2.0; - public IndexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { + public LindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { this.kickerIO = kickerIO; - firstCANRangeIO = new CANrangeIOReal(0, canbus); - secondCANRangeIO = new CANrangeIOReal(1, canbus); + firstCANRangeIO = new CANrangeIOReal(0, canbus, 10); + secondCANRangeIO = new CANrangeIOReal(1, canbus, 10); this.indexRollerIO = indexRollerIO; } + @Override public boolean isFull() { return firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; } - public Command stopKicker() { - return this.run(() -> kickerIO.setRollerVoltage(0)); - } - ; - - public Command shoot() { - return this.run(() -> kickerIO.setRollerVoltage(0)); - } - + @Override public boolean isEmpty() { return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected; } + @Override public boolean isPartiallyFull() { return !firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; } + @Override public Command index() { return this.run( () -> { - indexRollerIO.setRollerVoltage(5); - kickerIO.setRollerVoltage(-5); + indexRollerIO.setRollerVoltage(7); + kickerIO.setRollerVoltage(7); }); } - public Command indexToShoot() { + @Override + public Command kick() { return this.run( () -> { - indexRollerIO.setRollerVoltage(10); - kickerIO.setRollerVoltage(5); + indexRollerIO.setRollerVoltage(12); + kickerIO.setRollerVoltage(-7); }); } - public Command outtake() { + @Override + public Command spit() { return this.run( () -> { indexRollerIO.setRollerVoltage(-5); @@ -98,6 +96,7 @@ public Command outtake() { }); } + @Override public Command rest() { return this.run( () -> { @@ -111,7 +110,7 @@ public static TalonFXConfiguration getIndexerConfigs() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -136,7 +135,7 @@ public static TalonFXConfiguration getKickerConfigs() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; // Converts angular motion to linear motion config.Feedback.SensorToMechanismRatio = KICKER_GEAR_RATIO; @@ -166,7 +165,7 @@ public void periodic() { indexRollerIO.updateInputs(rollerInputs); Logger.processInputs("Indexer/Roller", rollerInputs); kickerIO.updateInputs(kickerInputs); - Logger.processInputs("Intake/Kicker", kickerInputs); + Logger.processInputs("Indexer/Kicker", kickerInputs); } public Command runRollerSysId() { diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java new file mode 100644 index 0000000..4346925 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Spindexer = SPINning Indexer. !! COMP !! */ +public class SpindexerSubsystem extends SubsystemBase implements Indexer { + /** Creates a new SpindexerSubsystem. */ + public SpindexerSubsystem() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public boolean isFull() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isFull'"); + } + + @Override + public boolean isEmpty() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isEmpty'"); + } + + @Override + public boolean isPartiallyFull() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isPartiallyFull'"); + } + + @Override + public Command index() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'index'"); + } + + @Override + public Command spit() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'spit'"); + } + + @Override + public Command kick() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'kick'"); + } + + @Override + public Command rest() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'rest'"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java similarity index 73% rename from src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java rename to src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index 89ab34d..b4dcfd1 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java @@ -14,7 +14,8 @@ import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; -public class IntakeSubsystem extends SubsystemBase { +/** Fintake = Fixed Intake. !! ALPHA !! */ +public class FintakeSubsystem extends SubsystemBase implements Intake { public static final double GEAR_RATIO = 2.0; private RollerIO io; @@ -22,27 +23,34 @@ public class IntakeSubsystem extends SubsystemBase { private SysIdRoutine intakeRollerSysid = new SysIdRoutine( - new Config(null, null, null, (state) -> Logger.recordOutput("Intake/SysID State", state)), + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Intake/SysID State", state.toString())), new Mechanism((volts) -> io.setRollerVoltage(volts.in(Volts)), null, this)); - public IntakeSubsystem(RollerIO io) { + public FintakeSubsystem(RollerIO io) { this.io = io; } + @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); } - // TODO get actual values + @Override public Command intake() { - return this.run(() -> io.setRollerVoltage(5)); + return this.run(() -> io.setRollerVoltage(10)); } - public Command outake() { - return this.run(() -> io.setRollerVoltage(-2)); + @Override + public Command outtake() { + return this.run(() -> io.setRollerVoltage(-5)); } + @Override public Command rest() { return this.run(() -> io.setRollerVoltage(0)); } @@ -62,14 +70,13 @@ public static TalonFXConfiguration getIntakeConfig() { config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kS = 0.24; - config.Slot0.kV = 0.6; - config.Slot0.kP = 110.0; - config.Slot0.kD = 0.0; + config.Slot0.kS = 0.42; + config.Slot0.kV = 0.21; + config.Slot0.kA = 0.00347; - config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimit = 40.0; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = 60.0; + config.CurrentLimits.SupplyCurrentLimit = 40.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; return config; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..9a6196f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,19 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.Command; + +/** Add your docs here. */ +public interface Intake { + /** Run balls towards the shooter */ + public Command intake(); + + /** Run balls away from the shooter. This is for antijamming the robot */ + public Command outtake(); + + /** Not running (set to 0) */ + public Command rest(); +} diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java new file mode 100644 index 0000000..a400db1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Lintake = Linear Intake. !! COMP !! */ +public class LintakeSubsystem extends SubsystemBase implements Intake { + /** Creates a new LintakeSubsystem. */ + public LintakeSubsystem() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public Command intake() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'intake'"); + } + + @Override + public Command outtake() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'outtake'"); + } + + @Override + public Command rest() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'rest'"); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index ac355ed..304e65b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -16,10 +16,12 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; /** Add your docs here. */ public class FlywheelIO { @@ -31,6 +33,7 @@ public static class FlywheelIOInputs { public double flywheelLeaderSupplyCurrentAmp = 0.0; public double flywheelLeaderVoltage = 0.0; public double flywheelLeaderTempC = 0.0; + public double flywheelLeaderPosition = 0.0; public double flywheelFollowerVelocityRotationsPerSecond = 0.0; public double flywheelFollowerStatorCurrentAmps = 0.0; @@ -47,6 +50,7 @@ public static class FlywheelIOInputs { private final StatusSignal flywheelLeaderStatorCurrent; private final StatusSignal flywheelLeaderSupplyCurrent; private final StatusSignal flywheelLeaderTemp; + private final StatusSignal flywheelLeaderPosition; private final BaseStatusSignal flywheelFollowerVelocity; private final StatusSignal flywheelFollowerVoltage; @@ -56,8 +60,7 @@ public static class FlywheelIOInputs { private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); - private MotionMagicVelocityVoltage motionMagicVelocityVoltage = - new MotionMagicVelocityVoltage(0.0).withEnableFOC(true).withAcceleration(100); + private MotionMagicVelocityVoltage motionMagicVelocityVoltage; private double velocitySetpointRotPerSec = 0.0; @@ -79,6 +82,7 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelLeaderStatorCurrent = flywheelLeader.getStatorCurrent(); flywheelLeaderSupplyCurrent = flywheelLeader.getSupplyCurrent(); flywheelLeaderTemp = flywheelLeader.getDeviceTemp(); + flywheelLeaderPosition = flywheelLeader.getPosition(); flywheelFollowerVelocity = flywheelFollower.getVelocity(); flywheelFollowerVoltage = flywheelFollower.getMotorVoltage(); @@ -86,6 +90,11 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollowerSupplyCurrent = flywheelFollower.getSupplyCurrent(); flywheelFollowerTemp = flywheelFollower.getDeviceTemp(); + motionMagicVelocityVoltage = + new MotionMagicVelocityVoltage(0.0) + .withAcceleration(config.MotionMagic.MotionMagicAcceleration) + .withEnableFOC(true); + BaseStatusSignal.setUpdateFrequencyForAll( 50.0, flywheelLeader.getVelocity(), @@ -97,7 +106,8 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollower.getMotorVoltage(), flywheelFollower.getStatorCurrent(), flywheelFollower.getSupplyCurrent(), - flywheelFollower.getDeviceTemp()); + flywheelFollower.getDeviceTemp(), + flywheelLeaderPosition); flywheelLeader.optimizeBusUtilization(); flywheelFollower.optimizeBusUtilization(); @@ -111,15 +121,18 @@ public static TalonFXConfiguration getFlywheelConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO; - config.Slot0.kS = 0.0; - config.Slot0.kV = 0.15; // From sim - config.Slot0.kP = 10.0; + config.Slot0.kS = 0.43477; + config.Slot0.kV = 0.144; + config.Slot0.kA = 0.016433; + config.Slot0.kP = 0.1; config.Slot0.kD = 0.0; config.CurrentLimits.StatorCurrentLimit = 120.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 80.0; + config.MotionMagic.MotionMagicAcceleration = 100.0; + return config; } @@ -148,7 +161,8 @@ public void updateInputs(FlywheelIOInputs inputs) { flywheelFollowerVoltage, flywheelFollowerStatorCurrent, flywheelFollowerSupplyCurrent, - flywheelFollowerTemp); + flywheelFollowerTemp, + flywheelLeaderPosition); inputs.flywheelLeaderVelocityRotationsPerSecond = flywheelLeaderVelocity.getValueAsDouble(); inputs.flywheelLeaderVoltage = flywheelLeaderVoltage.getValueAsDouble(); @@ -156,6 +170,8 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelLeaderSupplyCurrentAmp = flywheelLeaderSupplyCurrent.getValueAsDouble(); inputs.flywheelLeaderTempC = flywheelLeaderTemp.getValueAsDouble(); + inputs.flywheelLeaderPosition = flywheelLeaderPosition.getValueAsDouble(); + inputs.flywheelFollowerVelocityRotationsPerSecond = flywheelFollowerVelocity.getValueAsDouble(); inputs.flywheelFollowerVoltage = flywheelFollowerVoltage.getValueAsDouble(); inputs.flywheelFollowerStatorCurrentAmps = flywheelFollowerStatorCurrent.getValueAsDouble(); @@ -163,6 +179,7 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelFollowerTempC = flywheelFollowerTemp.getValueAsDouble(); } + @AutoLogOutput(key = "Shooter/Setpoint") public double getSetpointRotPerSec() { return velocitySetpointRotPerSec; } diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index e852e44..3c38920 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -14,13 +14,16 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; public class HoodIO { /** Creates a new HoodIOReal. */ @@ -32,11 +35,13 @@ public static class HoodIOInputs { public double hoodSupplyCurrentAmp = 0.0; public double hoodVoltage = 0.0; public double hoodTempC = 0.0; + // For sysid + public double hoodRotations = 0.0; } protected TalonFX hoodMotor; - private final BaseStatusSignal hoodPositionRotations; + private final StatusSignal hoodPositionRotations; private final BaseStatusSignal hoodAngularVelocity; private final StatusSignal hoodVoltage; private final StatusSignal hoodStatorCurrent; @@ -46,6 +51,8 @@ public static class HoodIOInputs { private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + private Rotation2d hoodSetpoint = Rotation2d.kZero; + public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor = new TalonFX(11, canbus); hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration()); @@ -81,13 +88,13 @@ public static TalonFXConfiguration getHoodConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; - // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; Potentially need, maybe not tho. + config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.kS = 0.0; - config.Slot0.kG = 0.0; - config.Slot0.kV = 1.1; - config.Slot0.kP = 5.0; - config.Slot0.kD = 0.0; + config.Slot0.kS = 0.055; + config.Slot0.kG = 0.445; + config.Slot0.kV = 1.45; + config.Slot0.kP = 35; + config.Slot0.kD = 0.25; config.CurrentLimits.StatorCurrentLimit = 80.0; config.CurrentLimits.StatorCurrentLimitEnable = true; @@ -101,6 +108,7 @@ public void setHoodVoltage(double hoodVoltage) { } public void setHoodPosition(Rotation2d hoodPosition) { + hoodSetpoint = hoodPosition; hoodMotor.setControl(positionVoltage.withPosition(hoodPosition.getRotations())); } @@ -117,11 +125,22 @@ public void updateInputs(HoodIOInputs inputs) { hoodSupplyCurrent, hoodTemp); - inputs.hoodPositionRotations = Rotation2d.fromRadians(hoodPositionRotations.getValueAsDouble()); + inputs.hoodPositionRotations = + Rotation2d.fromRotations(hoodPositionRotations.getValueAsDouble()); inputs.hoodAngularVelocity = hoodAngularVelocity.getValueAsDouble(); inputs.hoodVoltage = hoodVoltage.getValueAsDouble(); inputs.hoodStatorCurrentAmps = hoodStatorCurrent.getValueAsDouble(); inputs.hoodSupplyCurrentAmp = hoodSupplyCurrent.getValueAsDouble(); inputs.hoodTempC = hoodTemp.getValueAsDouble(); + inputs.hoodRotations = hoodPositionRotations.getValueAsDouble(); + } + + @AutoLogOutput(key = "Shooter/Hood/Setpoint") + public Rotation2d getHoodSetpoint() { + return hoodSetpoint; + } + + public void resetEncoder(Rotation2d rotations) { + hoodMotor.setPosition(rotations.getRotations()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..cacef67 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; + +/** Add your docs here. */ +public interface Shooter { + + /** + * Sets hood angle and flywheel velocity based on distance from hub from the shot map + current + * pose + */ + public Command shoot(Supplier robotPoseSupplier); + + /** + * Sets hood angle and flywheel velocity based on distance from hub from the feed map + current + * pose + feed target + */ + public Command feed(Supplier robotPoseSupplier, Supplier feedTarget); + + /** Not running (set to 0) */ + public Command rest(); + + /** Run balls out from the shooter. This is for antijamming the robot */ + public Command spit(); + + /** Check if flywheel has spun up to desired speed */ + public boolean atFlywheelVelocitySetpoint(); + + /** Check if hood is at its desired position */ + public boolean atHoodSetpoint(); + + /** Reset hood encoder to its minimum position */ + public Command zeroHood(); + + /** Shoots based on dashboard numbers. For testing only */ + public Command testShoot(); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 9cfde0a..cd4ab04 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -16,19 +16,22 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class ShooterSubsystem extends SubsystemBase { - public static double HOOD_GEAR_RATIO = 147.0 / 13.0; +/** Fixed shooter. !! ALPHA !! */ +public class ShooterSubsystem extends SubsystemBase implements Shooter { + public static double HOOD_GEAR_RATIO = 24.230769; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); - public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(0); + public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; - public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; // TODO: TUNE + public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); @@ -39,7 +42,10 @@ public class ShooterSubsystem extends SubsystemBase { private SysIdRoutine hoodSysid = new SysIdRoutine( new Config( - null, null, null, (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state)), + null, + null, + null, + (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state.toString())), new Mechanism((voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)), null, this)); private SysIdRoutine flywheelSysid = @@ -48,15 +54,28 @@ public class ShooterSubsystem extends SubsystemBase { null, null, null, - (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state)), + (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())), new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); + private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); + private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 30.0); + /** Creates a new HoodSubsystem. */ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.hoodIO = hoodIO; this.flywheelIO = flywheelIO; } + @Override + public Command testShoot() { + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + }); + } + + @Override public Command shoot(Supplier robotPoseSupplier) { return this.run( () -> { @@ -67,6 +86,7 @@ public Command shoot(Supplier robotPoseSupplier) { }); } + @Override public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { return this.run( () -> { @@ -81,14 +101,16 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar }); } + @Override public Command rest() { return this.run( () -> { - hoodIO.setHoodPosition(Rotation2d.kZero); // TODO: TUNE TUCKED POSITION IF NEEDED + hoodIO.setHoodPosition(HOOD_MIN_ROTATION); // TODO: TUNE TUCKED POSITION IF NEEDED flywheelIO.setFlywheelVoltage(0.0); }); } + @Override public Command spit() { return this.run( () -> { @@ -97,10 +119,6 @@ public Command spit() { }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } - public Command setHoodPositionCommand(Supplier hoodPosition) { - return this.run(() -> hoodIO.setHoodPosition(hoodPosition.get())); - } - @Override public void periodic() { hoodIO.updateInputs(hoodInputs); @@ -146,10 +164,24 @@ public Command runFlywheelSysid() { flywheelSysid.dynamic(Direction.kReverse)); } + @Override + @AutoLogOutput(key = "Shooter/At Vel Setpoint") public boolean atFlywheelVelocitySetpoint() { return MathUtil.isNear( flywheelInputs.flywheelLeaderVelocityRotationsPerSecond, flywheelIO.getSetpointRotPerSec(), FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); } + + @Override + @AutoLogOutput(key = "Shooter/Hood/At Setpoint") + public boolean atHoodSetpoint() { + return MathUtil.isNear( + hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); + } + + @Override + public Command zeroHood() { + return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION)); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java new file mode 100644 index 0000000..685b95a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -0,0 +1,69 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.Supplier; + +/** Pivoting hooded shooter (turret). !! COMP !! */ +public class TurretSubsystem extends SubsystemBase implements Shooter { + /** Creates a new TurretSubsystem. */ + public TurretSubsystem() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public Command shoot(Supplier robotPoseSupplier) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'shoot'"); + } + + @Override + public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'feed'"); + } + + @Override + public Command rest() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'rest'"); + } + + @Override + public Command spit() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'spit'"); + } + + @Override + public boolean atFlywheelVelocitySetpoint() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'atFlywheelVelocitySetpoint'"); + } + + @Override + public boolean atHoodSetpoint() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'atHoodSetpoint'"); + } + + @Override + public Command zeroHood() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'zeroHood'"); + } + + @Override + public Command testShoot() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'testShoot'"); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index af916be..aa7230b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -27,11 +27,13 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; -import frc.robot.Robot.RobotType; +import frc.robot.Robot.RobotEdition; +import frc.robot.Robot.RobotMode; import frc.robot.components.camera.Camera; import frc.robot.components.camera.CameraIOReal; import frc.robot.components.camera.CameraIOSim; import frc.robot.subsystems.swerve.constants.AlphaSwerveConstants; +import frc.robot.subsystems.swerve.constants.CompBotSwerveConstants; import frc.robot.subsystems.swerve.constants.SwerveConstants; import frc.robot.subsystems.swerve.gyro.GyroIO; import frc.robot.subsystems.swerve.gyro.GyroIOInputsAutoLogged; @@ -63,7 +65,12 @@ import org.littletonrobotics.junction.Logger; public class SwerveSubsystem extends SubsystemBase { - public static final SwerveConstants SWERVE_CONSTANTS = new AlphaSwerveConstants(); + // decide which set of swerve constants to use based on robot edition + // defaulting to comp is probably safer? + public static final SwerveConstants SWERVE_CONSTANTS = + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? new AlphaSwerveConstants() + : new CompBotSwerveConstants(); private final Module[] modules; // Front Left, Front Right, Back Left, Back Right private final GyroIO gyroIO; @@ -129,7 +136,7 @@ public class SwerveSubsystem extends SubsystemBase { new SwerveDriveSimulation(driveTrainSimConfig, new Pose2d(3, 3, Rotation2d.kZero)); public SwerveSubsystem(CANBus canbus) { - if (Robot.ROBOT_TYPE == RobotType.SIM) { + if (Robot.ROBOT_MODE == RobotMode.SIM) { // Add simulated modules modules = new Module[] { @@ -211,7 +218,7 @@ public SwerveSubsystem(CANBus canbus) { } this.gyroIO = - Robot.ROBOT_TYPE != RobotType.SIM + Robot.ROBOT_MODE != RobotMode.SIM ? new GyroIOReal(SWERVE_CONSTANTS.getGyroID(), SWERVE_CONSTANTS.getGyroConfig(), canbus) : new GyroIOSim(swerveSimulation.getGyroSimulation()); @@ -228,7 +235,7 @@ public SwerveSubsystem(CANBus canbus) { this.odometryThread = PhoenixOdometryThread.getInstance(); - if (Robot.ROBOT_TYPE == RobotType.SIM) { + if (Robot.ROBOT_MODE == RobotMode.SIM) { SimulatedArena.getInstance().addDriveTrainSimulation(swerveSimulation); } } @@ -263,6 +270,8 @@ public void periodic() { Tracer.trace("Update odometry", this::updateOdometry); Tracer.trace("Update vision", this::updateVision); + + Logger.recordOutput("Current Hub Pose", FieldUtils.getCurrentHubPose()); }); } @@ -348,12 +357,12 @@ private void updateVision() { cameraPoses[i] = cameras[i].getPose(); } // only do all this logging stuff if we're not irl for performance - if (Robot.ROBOT_TYPE != RobotType.REAL) { + if (Robot.ROBOT_MODE != RobotMode.REAL) { Logger.recordOutput("Vision/Camera Poses", cameraPoses); Pose3d[] arr = new Pose3d[cameras.length]; for (int k = 0; k < cameras.length; k++) { // honetsly not sure if this distinction is the way to go but - if (Robot.ROBOT_TYPE == RobotType.SIM) + if (Robot.ROBOT_MODE == RobotMode.SIM) // If we're in sim, use the maplesim pose to calculate vision arr[k] = new Pose3d(swerveSimulation.getSimulatedDriveTrainPose()) @@ -586,8 +595,11 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { () -> { Translation2d robotHubVec = FieldUtils.getCurrentHubTranslation().minus(getPose().getTranslation()); + // return FieldUtils.getCurrentHubPose().minus(getPose()).getRotation(); + // Logger.recordOutput("robot hub vec", robotHubVec); // atan2 takes y as the first arg (i think bc θ = atan(y/x) but idk) - return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())); + return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())) + .plus(Rotation2d.kCW_90deg); }, xVel, yVel); @@ -653,7 +665,7 @@ public Rotation2d getRotation() { public void resetPose(Pose2d newPose) { estimator.resetPose(newPose); - if (Robot.ROBOT_TYPE == RobotType.SIM) { + if (Robot.ROBOT_MODE == RobotMode.SIM) { swerveSimulation.setSimulationWorldPose(newPose); swerveSimulation.setRobotSpeeds(new ChassisSpeeds()); } diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java index ce122de..78470e3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java @@ -181,27 +181,23 @@ public Mass getMass() { @Override public ModuleConstants getFrontLeftModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants( 0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(-0.29).plus(Rotation2d.k180deg)); } @Override public ModuleConstants getFrontRightModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants(1, "Front Right", 2, 3, 1, Rotation2d.fromRotations(0.012)); } @Override public ModuleConstants getBackLeftModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants( 2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(0.229).plus(Rotation2d.k180deg)); } @Override public ModuleConstants getBackRightModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(-0.205)); } @@ -212,7 +208,6 @@ public int getGyroID() { @Override public Pigeon2Configuration getGyroConfig() { - // TODO getGyroConfig Pigeon2Configuration config = new Pigeon2Configuration(); config.MountPose.MountPosePitch = 0.18661323189735413; config.MountPose.MountPoseRoll = -0.706454336643219; @@ -222,7 +217,6 @@ public Pigeon2Configuration getGyroConfig() { @Override public TalonFXConfiguration getDriveConfig() { - // TODO getDriveConfig var driveConfig = new TalonFXConfiguration(); // Current limits driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; @@ -255,7 +249,6 @@ public TalonFXConfiguration getDriveConfig() { @Override public TalonFXConfiguration getTurnConfig(int cancoderID) { - // TODO getTurnConfig var turnConfig = new TalonFXConfiguration(); // Current limits turnConfig.CurrentLimits.SupplyCurrentLimit = 20.0; diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 1426b50..d1f27a5 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -2,19 +2,37 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import org.littletonrobotics.junction.Logger; public class AutoAim { public static final InterpolatingShotTree HUB_SHOT_TREE = new InterpolatingShotTree(); - // If we need other shot trees (i.e. for feeding) we can put them here - static { // For hub shot tree - // TODO: ADD SHOTS TO HUB SHOT HERE HUB_SHOT_TREE.put( - 1.0, new ShotData(Rotation2d.kCW_90deg, 10, 0.5)); // Placeholder to prevent crashes + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), + new ShotData(Rotation2d.fromDegrees(6), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), + new ShotData(Rotation2d.fromDegrees(10.5), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), + new ShotData(Rotation2d.fromDegrees(14.5), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), + new ShotData(Rotation2d.fromDegrees(18), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), + new ShotData(Rotation2d.fromDegrees(21.5), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), + new ShotData(Rotation2d.fromDegrees(24.5), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), + new ShotData(Rotation2d.fromDegrees(28), 30)); } // Ig we'll see if we need more than 1 feed shot tree @@ -23,12 +41,14 @@ public class AutoAim { static { // For feed shot tree // TODO: POPULATE FEED_SHOT_TREE.put( - 1.0, new ShotData(Rotation2d.kCW_90deg, 10, 0.5)); // Placeholder to prevent crashes + 1.0, new ShotData(Rotation2d.kCW_90deg, 10)); // Placeholder to prevent crashes } // TODO: SOTM public static double distanceToHub(Pose2d pose) { - return pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); + double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); + Logger.recordOutput("Autoaim/Distance To Hub", distance); + return distance; } } diff --git a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java index deaebd9..a9e90c0 100644 --- a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java +++ b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java @@ -5,8 +5,7 @@ import java.util.TreeMap; public class InterpolatingShotTree { - public record ShotData( - Rotation2d hoodAngle, double flywheelVelocityRotPerSec, double flightTimeSec) {} + public record ShotData(Rotation2d hoodAngle, double flywheelVelocityRotPerSec) {} private final TreeMap map = new TreeMap<>(); @@ -90,8 +89,7 @@ private ShotData interpolate(ShotData startValue, ShotData endValue, double t) { MathUtil.interpolate( startValue.hoodAngle().getRadians(), endValue.hoodAngle().getRadians(), t)), MathUtil.interpolate( - startValue.flywheelVelocityRotPerSec(), endValue.flywheelVelocityRotPerSec(), t), - MathUtil.interpolate(startValue.flightTimeSec(), endValue.flightTimeSec(), t)); + startValue.flywheelVelocityRotPerSec(), endValue.flywheelVelocityRotPerSec(), t)); } /** From f96d55dbe63deee3562c89d66529efaa4ce72ab2 Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Sun, 25 Jan 2026 14:58:46 -0800 Subject: [PATCH 2/5] Create a bump align method --- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 0abe90d..dc6b5c4 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -605,13 +605,13 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { yVel); } - public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { + public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { return driveWithHeadingSnap( () -> { - Translation2d robotHubVec = + Translation2d robotBumpVec = FieldUtils.getCurrentHubTranslation().minus(getPose().getTranslation()); // atan2 takes y as the first arg (i think bc θ = atan(y/x) but idk) - return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())) + return Rotation2d.fromRadians(Math.atan2(robotBumpVec.getY(), robotBumpVec.getX())) .plus(Rotation2d.kCW_90deg); }, xVel, From 0c88d20cb729fcb23675ed077a11ee0faacdb653 Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Sun, 1 Feb 2026 14:44:39 -0800 Subject: [PATCH 3/5] Creating bumpAlign command --- .../subsystems/swerve/SwerveSubsystem.java | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index dc6b5c4..2aa2b9d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -606,16 +606,23 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { } public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { - return driveWithHeadingSnap( - () -> { - Translation2d robotBumpVec = - FieldUtils.getCurrentHubTranslation().minus(getPose().getTranslation()); - // atan2 takes y as the first arg (i think bc θ = atan(y/x) but idk) - return Rotation2d.fromRadians(Math.atan2(robotBumpVec.getY(), robotBumpVec.getX())) - .plus(Rotation2d.kCW_90deg); - }, - xVel, - yVel); + Pose2d RobotPose = getPose(); + if( + ((Math.abs(RobotPose.getX() - 4.6) < 2) || Math.abs(RobotPose.getX() - 11.9) < 2) + && + (( RobotPose.getY() > 5 && RobotPose.getY() < 6) || (RobotPose.getY() > 2 && RobotPose.getY() < 3))) + { + Rotation2d target; + if(RobotPose.getRotation().getDegrees() <= 90 || RobotPose.getRotation().getDegrees() >= 270){ + target = Rotation2d.kZero; + } else { + target = Rotation2d.k180deg; + } + return driveWithHeadingSnap(() -> target, xVel, yVel); + } else { + return null; + } + } public boolean isInAutoAimTolerance(Pose2d target) { From 3ee6f7af0e543fbea750ef7ee11cf3488dd8edbf Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Sun, 1 Feb 2026 16:05:48 -0800 Subject: [PATCH 4/5] Creating is close to bump method --- .../subsystems/swerve/SwerveSubsystem.java | 31 +++++++++++-------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 2aa2b9d..b856f64 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -606,23 +606,28 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { } public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { - Pose2d RobotPose = getPose(); - if( - ((Math.abs(RobotPose.getX() - 4.6) < 2) || Math.abs(RobotPose.getX() - 11.9) < 2) - && - (( RobotPose.getY() > 5 && RobotPose.getY() < 6) || (RobotPose.getY() > 2 && RobotPose.getY() < 3))) - { - Rotation2d target; - if(RobotPose.getRotation().getDegrees() <= 90 || RobotPose.getRotation().getDegrees() >= 270){ - target = Rotation2d.kZero; + return driveWithHeadingSnap( + () -> { + if(getPose().getRotation().getDegrees() <= 90 || getPose().getRotation().getDegrees() >= 270){ + return Rotation2d.kZero; } else { - target = Rotation2d.k180deg; + return Rotation2d.k180deg; } - return driveWithHeadingSnap(() -> target, xVel, yVel); + }, + xVel, + yVel); + } + + public boolean isCloseToBump(){ + if( + ((Math.abs(getPose().getX() - 4.62) < 2) || (Math.abs(getPose().getX() - 11.91) < 2)) + && + ((getPose().getY() > 5.04 && getPose().getY() < 6.07) || (getPose().getY() > 2 && getPose().getY() < 3.01))) + { + return true; } else { - return null; + return false; } - } public boolean isInAutoAimTolerance(Pose2d target) { From 43f8aa36bdff400cb1d58b99694bd56fea210f48 Mon Sep 17 00:00:00 2001 From: Dhilan Belur Date: Sun, 1 Feb 2026 16:28:50 -0800 Subject: [PATCH 5/5] Added trigger in robot.java --- src/main/java/frc/robot/Robot.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6d290e..1635d05 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -435,6 +435,17 @@ private void addControllerBindings() { () -> modifyJoystick(driver.getLeftX()) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); + + while(swerve.isCloseToBump()){ + swerve.bumpAlign( + () -> + modifyJoystick(driver.getLeftY()) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + () -> + modifyJoystick(driver.getLeftX()) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()); + } + // TODO add binding for climb // ---zeroing stuff---