From 50b8312c48be14a559876064cdddbcd27bcb8ef7 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Tue, 10 Mar 2026 08:39:05 -0600 Subject: [PATCH 01/33] Work in progress --- .../java/frc4388/robot/RobotContainer.java | 287 +++++++++--------- .../robot/commands/MoveForTimeCommand.java | 49 --- .../robot/commands/MoveUntilSuply.java | 45 --- .../robot/commands/Swerve/RotateToAngle.java | 35 --- .../robot/commands/Swerve/StayInPosition.java | 56 ++++ .../commands/Swerve/neoJoystickPlayback.java | 197 ------------ .../commands/Swerve/neoJoystickRecorder.java | 129 -------- .../robot/commands/WhileTrueCommand.java | 104 ------- .../robot/commands/alignment/AutoAlign.java | 2 +- .../robot/commands/alignment/RotTo45.java | 37 --- .../robot/commands/wait/waitSupplier.java | 36 --- .../robot/subsystems/{ => led}/LED.java | 2 +- .../robot/subsystems/shooter/Shooter.java | 2 +- .../robot/subsystems/swerve/SwerveDrive.java | 5 + .../robot/subsystems/{ => vision}/Lidar.java | 6 +- .../subsystems/{ => vision}/RPLidarA1.java | 2 +- .../frc4388/utility/status/FaultA1M8.java | 4 +- 17 files changed, 219 insertions(+), 779 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/MoveForTimeCommand.java delete mode 100644 src/main/java/frc4388/robot/commands/MoveUntilSuply.java delete mode 100644 src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java create mode 100644 src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java delete mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java delete mode 100644 src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java delete mode 100644 src/main/java/frc4388/robot/commands/WhileTrueCommand.java delete mode 100644 src/main/java/frc4388/robot/commands/alignment/RotTo45.java delete mode 100644 src/main/java/frc4388/robot/commands/wait/waitSupplier.java rename src/main/java/frc4388/robot/subsystems/{ => led}/LED.java (98%) rename src/main/java/frc4388/robot/subsystems/{ => vision}/Lidar.java (98%) rename src/main/java/frc4388/robot/subsystems/{ => vision}/RPLidarA1.java (99%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd47223..dd76e20 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,18 +30,18 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc4388.robot.commands.Swerve.StayInPosition; import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; -// Subsystems -import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; +import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; import frc4388.utility.compute.FieldPositions; @@ -84,148 +84,159 @@ public class RobotContainer { // private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID); // public List subsystems = new ArrayList<>(); - - // ! Teleop Commands - public void stop() { - new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); - m_robotSwerveDrive.stopModules(); - Constants.AutoConstants.Y_OFFSET_TRIM.set(0); - } - - // ! /* Autos */ - private SendableChooser autoChooser; - private Command autoCommand; - - - private Command IntakeExtended = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) - ); - - // private Command LidarIntake = new SequentialCommandGroup( - // // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball - // // RobotIntakeDown, - // // new WaitCommand(1), - // // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), - // new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) - - // // new RunCommand( - // // () -> m_robotSwerveDrive.driveWithInput( - // // m_lidar.getClosestBall(), - // // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), - // // false - // // ), - // // m_robotSwerveDrive - // // ) - // // .withTimeout(5.0) - // // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) - // ); - - private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), - IntakeExtended, - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) - ); - - private Command IntakeRetracted = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) - ); - - private Command RobotShoot = new SequentialCommandGroup( - // TEST NEW AUTO ALIGN - //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), - new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), - new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), - new WaitCommand(2), - IntakeRetracted, - new WaitCommand(5), - new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), - new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) - ); - + private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive); - public RobotContainer() { - - configureButtonBindings(); - - // Called on first robot enable - DeferredBlock.addBlock(() -> { - m_robotSwerveDrive.resetGyro(); - }, false); - - // Called on every robot enable - DeferredBlock.addBlock(() -> { - TimesNegativeOne.update(); - FieldPositions.update(); - - m_robotIntake.io.updateGains(); - m_robotShooter.io.updateGains(); - }, true); - - NamedCommands.registerCommand("Robot Rev Up", RobotRev); - NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); - NamedCommands.registerCommand("Robot Shoot", RobotShoot); - // NamedCommands.registerCommand("Lidar Intake", LidarIntake); - NamedCommands.registerCommand("Intake Extended", IntakeExtended); - - - DriverStation.silenceJoystickConnectionWarning(true); - - // Drive normally - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(),true); - - }, m_robotSwerveDrive) - .withName("SwerveDrive DefaultCommand")); - - m_robotSwerveDrive.setToSlow(); - - makeAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - - } + private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d()); + // ! Teleop Commands + public void stop() { + new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); + m_robotSwerveDrive.stopModules(); + Constants.AutoConstants.Y_OFFSET_TRIM.set(0); + } - - - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - - //Driver controls - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - + // ! /* Autos */ + private SendableChooser autoChooser; + private Command autoCommand; + + + private Command IntakeExtended = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake) + ); + + // private Command LidarIntake = new SequentialCommandGroup( + // // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball + // // RobotIntakeDown, + // // new WaitCommand(1), + // // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), + // new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) + + // // new RunCommand( + // // () -> m_robotSwerveDrive.driveWithInput( + // // m_lidar.getClosestBall(), + // // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), + // // false + // // ), + // // m_robotSwerveDrive + // // ) + // // .withTimeout(5.0) + // // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) + // ); + + private Command RobotRev = new SequentialCommandGroup( + new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), + IntakeExtended, + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) + ); + + private Command IntakeRetracted = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) + ); + + private Command RobotShoot = new SequentialCommandGroup( + // TEST NEW AUTO ALIGN + //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), + new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), + new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), + new WaitCommand(2), + IntakeRetracted, + new WaitCommand(5), + new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), + new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) + ); - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> { + public RobotContainer() { + + configureButtonBindings(); + + // Called on first robot enable + DeferredBlock.addBlock(() -> { + m_robotSwerveDrive.resetGyro(); + }, false); + + // Called on every robot enable + DeferredBlock.addBlock(() -> { + TimesNegativeOne.update(); + FieldPositions.update(); + m_robotIntake.io.updateGains(); m_robotShooter.io.updateGains(); - })); - + }, true); + + NamedCommands.registerCommand("Robot Rev Up", RobotRev); + NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); + NamedCommands.registerCommand("Robot Shoot", RobotShoot); + // NamedCommands.registerCommand("Lidar Intake", LidarIntake); + NamedCommands.registerCommand("Intake Extended", IntakeExtended); + + + DriverStation.silenceJoystickConnectionWarning(true); + + // Drive normally + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(),true); + + }, m_robotSwerveDrive) + .withName("SwerveDrive DefaultCommand")); - // IF the driver is holding the left trigger, intake driving - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - () -> { - m_robotSwerveDrive.driveIntakeOrientation( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight() - - ); - }, m_robotSwerveDrive)) + m_robotSwerveDrive.setToSlow(); + + makeAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + + } + + + + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + + //Driver controls + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + + + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.io.updateGains(); + m_robotShooter.io.updateGains(); + })); + + + // TEST-> the driver is holding the left trigger, drive slow and rotation up + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + m_robotSwerveDrive.shiftUpRot(); + })) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.setToFast(); + m_robotSwerveDrive.shiftDownRot(); + })); + + //TEST - > Swerve drive pid on position + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> { m_robotSwerveDrive.softStop(); })); diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java deleted file mode 100644 index 3a4e043..0000000 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc4388.robot.commands; - -import java.time.Instant; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -// Command to repeat a joystick movement for a specific time. -public class MoveForTimeCommand extends Command { - private final SwerveDrive swerveDrive; - private final Translation2d leftStick; - private final Translation2d rightStick; - private final long duration; - private final boolean robotRelative; - - private Instant startTime; - - public MoveForTimeCommand( - SwerveDrive swerveDrive, - Translation2d leftStick, - Translation2d rightStick, - long millis, - boolean robotRelative) { - - addRequirements(swerveDrive); - - this.swerveDrive = swerveDrive; - this.leftStick = leftStick; - this.rightStick = rightStick; - this.duration = millis; - this.robotRelative = robotRelative; - } - - @Override - public void initialize() { - startTime = Instant.now(); - } - - @Override - public void execute() { - swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); - } - - @Override - public boolean isFinished() { - return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java b/src/main/java/frc4388/robot/commands/MoveUntilSuply.java deleted file mode 100644 index c5b5e53..0000000 --- a/src/main/java/frc4388/robot/commands/MoveUntilSuply.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc4388.robot.commands; - -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -// Command to repeat a joystick movement for a specific time. -public class MoveUntilSuply extends Command { - private final SwerveDrive swerveDrive; - private final Translation2d leftStick; - private final Translation2d rightStick; - private final Supplier truth; - private final boolean robotRelative; - - public MoveUntilSuply( - SwerveDrive swerveDrive, - Translation2d leftStick, - Translation2d rightStick, - Supplier truth, - boolean robotRelative) { - addRequirements(swerveDrive); - - this.swerveDrive = swerveDrive; - this.leftStick = leftStick; - this.rightStick = rightStick; - this.truth = truth; - this.robotRelative = robotRelative; - } - - @Override - public void initialize() { - } - - @Override - public void execute() { - swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative); - } - - @Override - public boolean isFinished() { - return truth.get(); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java deleted file mode 100644 index a3610d1..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ /dev/null @@ -1,35 +0,0 @@ -// 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 frc4388.robot.commands.Swerve; - -import edu.wpi.first.math.geometry.Translation2d; -import frc4388.robot.commands.PID; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -public class RotateToAngle extends PID { - - SwerveDrive drive; - double targetAngle; - - /** Creates a new RotateToAngle. */ - public RotateToAngle(SwerveDrive drive, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.drive = drive; - this.targetAngle = targetAngle; - - addRequirements(drive); - } - - @Override - public double getError() { - return targetAngle - drive.getGyroAngle(); - } - - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java new file mode 100644 index 0000000..8d9be18 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -0,0 +1,56 @@ +// 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 frc4388.robot.commands.Swerve; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.commands.PID; +import frc4388.robot.subsystems.swerve.SwerveDrive; + +public class StayInPosition extends PID { + + SwerveDrive drive; + + /** Creates a new StayInPosition. */ + public StayInPosition(SwerveDrive drive) { + super(0.3, 0.0, 0.0, 0.0, 1); + + this.drive = drive; + + addRequirements(drive); + } + + public void goToTargetPose(Pose2d targetPose){ + Pose2d currentPose = drive.getCurrentPose(); + double translationX = targetPose.getX() - currentPose.getX(); + double translationY = targetPose.getY() - currentPose.getY(); + Rotation2d deltaRotation = targetPose.getRotation().minus(currentPose.getRotation()); + Translation2d driveTranslation = new Translation2d(translationX, translationY); + + drive.driveFieldAngle(driveTranslation, deltaRotation); + } + + @Override + public double getError() { + return 0; + // return targetAngle - drive.getGyroAngle(); + } + + @Override + public void runWithOutput(double output) { + // drive.driveWithInput(new Pose2d(new Translation2d(0.0, 0.0), new Rotation2d(output / Math.abs(getError())))); + } + + // @Override + // public boolean isFinished() { + // Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation(); + // double ballAngleDeg = m_lidar.getLatestBallAngleDegrees(); + + // // TODO: Tune + // return Math.abs(curRot.getDegrees() +ballAngleDeg) < 5; + // } + +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java deleted file mode 100644 index ba3c6ce..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ /dev/null @@ -1,197 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileInputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.utility.compute.DataUtils; -import frc4388.utility.controller.VirtualController; -import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; - - -/** - * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickPlayback extends Command { - private final SwerveDrive swerve; - private final VirtualController[] controllers; - private final ArrayList frames = new ArrayList<>(); - private final Supplier filenameGetter; - private String filename; - private int frame_index = 0; - // private long startTime = 0; - // private long playbackTime = 0; - private boolean m_finished = false; // ! There is no better way. - private boolean m_shouldfree = false; // should free memory on ending - - private byte m_numAxes = 0; - private byte m_numPOVs = 0; - private byte m_numControllers = 0; - private short m_numFrames = -1; - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree Unloads the auto on compleation or intruption. - * @param instantload Load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this.swerve = swerve; - this.filenameGetter = filenameGetter; - this.controllers = controllers; - this.m_shouldfree = shouldfree; - - if (instantload) loadAuto(); - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param shouldfree unloads the auto on compleation or intruption. - * @param instantload load the auto on object instantiation - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) { - this(swerve, () -> filename, controllers, shouldfree, instantload); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, Supplier filenameGetter, VirtualController[] controllers) { - this(swerve, filenameGetter, controllers, true, false); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param filename a String containing the name of the auto file you wish to playback. - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - */ - public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) { - this(swerve, () -> filename, controllers, true, false); - } - - /** - * Load the auto file from disk into memory - * @return Returns true if loading was successful, else wise; return false - * @implNote if the auto is already loaded, it will return true. - */ - public boolean loadAuto() { - filename = filenameGetter.get(); - try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) { - if (m_numFrames != -1 && m_numFrames == frames.size()) { - System.out.println("AUTOPLAYBACK: Auto Already loaded."); - return true; - } - - m_numAxes = stream.readNBytes(1)[0]; - m_numPOVs = stream.readNBytes(1)[0]; - m_numControllers = stream.readNBytes(1)[0]; - m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2)); - - if (m_numControllers > controllers.length) { - System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers - + " virtual controllers but only " + controllers.length + " were given"); - return false; - } - - for (int i = 0; i < m_numFrames; i++) { - AutoRecordingFrame frame = new AutoRecordingFrame(); - for (int j = 0; j < m_numControllers; j++) { - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = new double[m_numAxes]; - for (int k = 0; k < m_numAxes; k++) { // we love third level for loops. - axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8)); - } - short button = DataUtils.byteArrayToShort(stream.readNBytes(2)); - short[] POV = new short[m_numPOVs]; - for (int k = 0; k < m_numPOVs; k++) { - POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2)); - } - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[j] = controllerFrame; - } - frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4)); - frames.add(frame); - } - - System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long"); - return true; - - } catch (Exception e) { - e.printStackTrace(); - System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`'); - return false; - } - } - - /** - * Unloads the auto. - */ - public void unloadAuto() { - System.out.println("AUTOPLAYBACK: Auto unloaded"); - frames.clear(); - } - - @Override - public void initialize() { - // startTime = System.currentTimeMillis(); - // playbackTime = 0; - frame_index = 0; - - m_finished = !loadAuto(); - } - - @Override - public void execute() { - if (frame_index >= m_numFrames) m_finished = true; - if (m_finished) return; - - // if (frame_index == 0) { - // startTime = System.currentTimeMillis(); - // playbackTime = 0; - // } else { - // playbackTime = System.currentTimeMillis() - startTime; - // } - - AutoRecordingFrame frame = frames.get(frame_index); - for (int i = 0; i < controllers.length; i++) { - AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i]; - controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); - if (i == 0) { - this.swerve.driveWithInput( - new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), - new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), - true); - } - } - frame_index++; - } - - @Override - public void end(boolean interrupted) { - for (VirtualController controller : controllers) controller.zeroControls(); - swerve.stopModules(); - if (m_shouldfree) unloadAuto(); - } - - @Override - public boolean isFinished() { - return m_finished; - } -} diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java deleted file mode 100644 index 9aa9283..0000000 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ /dev/null @@ -1,129 +0,0 @@ -package frc4388.robot.commands.Swerve; - -import java.io.FileOutputStream; -import java.util.ArrayList; -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.utility.compute.DataUtils; -import frc4388.utility.controller.DeadbandedXboxController; -import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame; -import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame; - -/** - * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s - * @author Zachary Wilke - */ -public class neoJoystickRecorder extends Command { - private final SwerveDrive swerve; - private final XboxController[] controllers; - private String filename; - private final Supplier filenameGetter; - private long startTime = -1; - private final ArrayList frames = new ArrayList<>(); - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier filenameGetter) { - this.swerve = swerve; - this.controllers = controllers; - this.filenameGetter = filenameGetter; - this.filename = ""; - - addRequirements(this.swerve); - } - - /** - * Creates an new NEO Joystick Playback with specifyed pramiters. - * @param swerve m_robotSwerveDrive - * @param controllers an Order-Specific Array of Virtual controllers, index 0 means driver, index 1 means operator, etc. - * @param filename a String containing the name of the auto file you wish to playback. - */ - public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) { - this(swerve, controllers, () -> filename); - } - - @Override - public void initialize() { - frames.clear(); - - this.startTime = System.currentTimeMillis(); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()}; - frames.add(frame); - this.filename = this.filenameGetter.get(); - } - - - @Override - public void execute() { - System.out.println("AUTORECORD: RECORDING"); - AutoRecordingFrame frame = new AutoRecordingFrame(); - frame.timeStamp = (int) (System.currentTimeMillis() - startTime); - for (int i = 0; i < controllers.length; i++) { - XboxController controller = controllers[i]; - AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); - double[] axes = {controller.getLeftX(), controller.getLeftY(), - controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), - controller.getRightX(), controller.getRightY()}; - short button = 0; - for (int j = 0; j < 10; j++) - if (controller.getRawButton(j+1)) - button |= 1 << j; - short[] POV = {(short)(controller.getPOV())}; - controllerFrame.axes = axes; - controllerFrame.button = button; - controllerFrame.POV = POV; - frame.controllerFrames[i] = controllerFrame; - } - - frames.add(frame); - - swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), - new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), - true); // Really jank way of doing this. - - } - @Override - public void end(boolean interrupted) { - try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) { - // header: size of 0x5 - // byte Number of axes per controller - // byte Number of POVs per controller - // byte Number of controllers - // short Number of frames - stream.write(new byte[]{6, 1, (byte) controllers.length}); - stream.write(DataUtils.shortToByteArray((short) frames.size())); - - // frame - // controller frame * number of controllers - // int unix time stamp. - for (AutoRecordingFrame frame : frames) { - // controller frame - // double axis * Number of axes per controller - // short button states - // short POV * Number of POVs per controller - for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) { - for (double axis: controllerFrame.axes) { - stream.write(DataUtils.doubleToByteArray(axis)); - } - stream.write(DataUtils.shortToByteArray(controllerFrame.button)); - for (short POV: controllerFrame.POV) { - stream.write(DataUtils.shortToByteArray(POV)); - } - } - stream.write(DataUtils.intToByteArray(frame.timeStamp)); - } - System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); - } catch (Exception e) { - e.printStackTrace(); - } - } -} diff --git a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java b/src/main/java/frc4388/robot/commands/WhileTrueCommand.java deleted file mode 100644 index 2a0d045..0000000 --- a/src/main/java/frc4388/robot/commands/WhileTrueCommand.java +++ /dev/null @@ -1,104 +0,0 @@ -package frc4388.robot.commands; - -// 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. - -import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj2.command.Command; - -import java.util.function.BooleanSupplier; - -/** - * A command composition that runs one of two commands, depending on the value of the given - * condition when this command is initialized. - * - *

The rules for command compositions apply: command instances that are passed to it cannot be - * added to any other composition or scheduled individually, and the composition requires all - * subsystems its components require. - * - *

This class is provided by the NewCommands VendorDep - */ -public class WhileTrueCommand extends Command { - private final Command m_whileTrue; - private final BooleanSupplier m_condition; - - /** - * Creates a new WhileTrueCommand. - * - * @param whileTrue the command to run while the condition is true - * @param condition the condition to determine which command to run - */ - @SuppressWarnings("this-escape") - public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) { - m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand"); - m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand"); - - //CommandScheduler.getInstance().registerComposedCommands(whileTrue); - - // addRequirements(whileTrue.getRequirements()); - } - - @Override - public void initialize() { - if(m_condition.getAsBoolean()) - m_whileTrue.initialize(); - } - - @Override - public void execute() { - m_whileTrue.execute(); - - System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " + m_condition.getAsBoolean()); - - if(!m_whileTrue.isFinished()) - return; - - if(m_condition.getAsBoolean()){ - m_whileTrue.end(false); - m_whileTrue.initialize(); - } - } - - @Override - public void end(boolean interrupted) { - m_whileTrue.end(interrupted); - } - - @Override - public boolean isFinished() { - return !m_condition.getAsBoolean() && m_whileTrue.isFinished(); - } - - @Override - public boolean runsWhenDisabled() { - return m_whileTrue.runsWhenDisabled(); - } - - @Override - public InterruptionBehavior getInterruptionBehavior() { - if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) { - return InterruptionBehavior.kCancelSelf; - } else { - return InterruptionBehavior.kCancelIncoming; - } - } - - @Override - public void initSendable(SendableBuilder builder) { - super.initSendable(builder); - builder.addStringProperty("whileTrue", m_whileTrue::getName, null); - builder.addStringProperty( - "selected", - () -> { - if (m_whileTrue == null) { - return "null"; - } else { - return m_whileTrue.getName(); - } - }, - null); - } -} diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 770280d..5a1d2d3 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -6,8 +6,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; -import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.FieldPositions; import frc4388.utility.structs.Gains; diff --git a/src/main/java/frc4388/robot/commands/alignment/RotTo45.java b/src/main/java/frc4388/robot/commands/alignment/RotTo45.java deleted file mode 100644 index 197cf8b..0000000 --- a/src/main/java/frc4388/robot/commands/alignment/RotTo45.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc4388.robot.commands.alignment; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Command; -import frc4388.robot.subsystems.swerve.SwerveDrive; - -public class RotTo45 extends Command { - - SwerveDrive m_SwerveDrive; - Rotation2d targetAngle; - - - public RotTo45(SwerveDrive swerveDrive) { - m_SwerveDrive = swerveDrive; - - addRequirements(swerveDrive); - } - - @Override - public void initialize() { - targetAngle = new Rotation2d(); - } - - @Override - public void execute() { - m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle); - } - - @Override - public boolean isFinished() { - Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation(); - // TODO: Tune - return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5; - } - -} diff --git a/src/main/java/frc4388/robot/commands/wait/waitSupplier.java b/src/main/java/frc4388/robot/commands/wait/waitSupplier.java deleted file mode 100644 index a23db69..0000000 --- a/src/main/java/frc4388/robot/commands/wait/waitSupplier.java +++ /dev/null @@ -1,36 +0,0 @@ -// 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 frc4388.robot.commands.wait; - -import java.util.function.Supplier; - -import edu.wpi.first.wpilibj2.command.Command; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class waitSupplier extends Command { - /** Creates a new waitSupplier. */ - private final Supplier truth; - public waitSupplier(Supplier truth) { - this.truth = truth; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return truth.get(); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/led/LED.java similarity index 98% rename from src/main/java/frc4388/robot/subsystems/LED.java rename to src/main/java/frc4388/robot/subsystems/led/LED.java index 58a4591..07f2876 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/led/LED.java @@ -5,7 +5,7 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.led; import java.util.concurrent.TimeUnit; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 15f0edf..cb46a97 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -15,8 +15,8 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; -import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.FieldPositions; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 9cb77d1..6065683 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -377,6 +377,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } + public Pose2d getCurrentPose(){ + return state.currentPose; + } + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { leftStick = leftStick.rotateBy(heading); @@ -515,6 +519,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { gear_index = i; } + public void setPercentOutput(double speed) { speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed; gear_index = -1; diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/vision/Lidar.java similarity index 98% rename from src/main/java/frc4388/robot/subsystems/Lidar.java rename to src/main/java/frc4388/robot/subsystems/vision/Lidar.java index fdbf2a6..d859102 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Lidar.java @@ -1,4 +1,4 @@ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.vision; import java.util.ArrayList; import java.util.LinkedList; @@ -20,8 +20,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.subsystems.RPLidarA1.PolarPoint; -import frc4388.robot.subsystems.RPLidarA1.ScanListener; +import frc4388.robot.subsystems.vision.RPLidarA1.PolarPoint; +import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.FaultA1M8; diff --git a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java b/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java similarity index 99% rename from src/main/java/frc4388/robot/subsystems/RPLidarA1.java rename to src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java index fa5aa32..41ad20a 100644 --- a/src/main/java/frc4388/robot/subsystems/RPLidarA1.java +++ b/src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java @@ -1,4 +1,4 @@ -package frc4388.robot.subsystems; +package frc4388.robot.subsystems.vision; import com.fazecast.jSerialComm.SerialPort; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc4388/utility/status/FaultA1M8.java b/src/main/java/frc4388/utility/status/FaultA1M8.java index aedb866..e99eb3e 100644 --- a/src/main/java/frc4388/utility/status/FaultA1M8.java +++ b/src/main/java/frc4388/utility/status/FaultA1M8.java @@ -1,7 +1,7 @@ package frc4388.utility.status; -import frc4388.robot.subsystems.RPLidarA1; -import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus; +import frc4388.robot.subsystems.vision.RPLidarA1; +import frc4388.robot.subsystems.vision.RPLidarA1.ConnectionStatus; import frc4388.utility.status.Status.ReportLevel; // Fault reporter for the RPLidar A1M8 Revolving lidar sensor From 960b9f5dd4750a4aca5d905ae9133138906e4c74 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Tue, 10 Mar 2026 21:06:43 -0600 Subject: [PATCH 02/33] Test offset --- .../java/frc4388/robot/RobotContainer.java | 9 +++++++ .../robot/constants/BuildConstants.java | 12 +++++----- .../robot/subsystems/shooter/Shooter.java | 10 ++++++-- .../robot/subsystems/shooter/ShooterIO.java | 1 + .../robot/subsystems/shooter/ShooterReal.java | 1 + .../robot/subsystems/swerve/SwerveDrive.java | 24 +++++++++++++++++++ 6 files changed, 49 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd76e20..697bfb7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -251,6 +251,15 @@ public class RobotContainer { ShooterConstants.AIM_LEAD_TIME.get() ); }, m_robotSwerveDrive) + // () -> { + // m_robotSwerveDrive.driveFacingVelocity( + // getDeadbandedDriverController().getLeft(), + // FieldPositions.HUB_POSITION, + // ShooterConstants.AIM_LEAD_TIME.get(), + // m_robotShooter.getBallVelocity(), + // m_robotShooter.getDistanceToHub() + // ); + // }, m_robotSwerveDrive) ); // D-PAD fine alignment diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9ff0c32..c801a29 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 147; - public static final String GIT_SHA = "b073fad27703951ba251eaffc56d86f791e531e6"; - public static final String GIT_DATE = "2026-03-07 20:08:15 MST"; - public static final String GIT_BRANCH = "PikesPeak"; - public static final String BUILD_DATE = "2026-03-08 14:27:19 MDT"; - public static final long BUILD_UNIX_TIME = 1773001639109L; + public static final int GIT_REVISION = 149; + public static final String GIT_SHA = "50b8312c48be14a559876064cdddbcd27bcb8ef7"; + public static final String GIT_DATE = "2026-03-10 08:39:05 MDT"; + public static final String GIT_BRANCH = "MiraOrg"; + public static final String BUILD_DATE = "2026-03-10 21:05:03 MDT"; + public static final long BUILD_UNIX_TIME = 1773198303540L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index cb46a97..ed625f9 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -31,6 +31,7 @@ public class Shooter extends SubsystemBase { // Supplier m_swervePoseSupplier; public boolean badShooterVelocity; + public double distanceToHub = 5; public Shooter( @@ -76,7 +77,9 @@ public class Shooter extends SubsystemBase { this.mode = ShooterMode.Idle; } - + public double getDistanceToHub(){ + return distanceToHub; + } public void allowShooting() { shooterButtonReady = true; @@ -86,6 +89,9 @@ public class Shooter extends SubsystemBase { shooterButtonReady = false; } + public AngularVelocity getBallVelocity() { + return RotationsPerSecond.of((state.motor1Velocity.in(RotationsPerSecond) + state.indexerVelocity.in(RotationsPerSecond))); + } @AutoLogOutput public ShooterMode getMode() { @@ -126,7 +132,7 @@ public class Shooter extends SubsystemBase { Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation()); // Get the robot's aim distance to hub - double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); + distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); //Center of hub to cameras in inches Logger.recordOutput("Hub Dist", distanceToHub); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index c7d3369..9b32782 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -26,6 +26,7 @@ public interface ShooterIO { AngularVelocity motor1Velocity = RotationsPerSecond.of(0); AngularVelocity motor2Velocity = RotationsPerSecond.of(0); + AngularVelocity indexerVelocity = RotationsPerSecond.of(0); double indexerOutput = 0; Current motor1Current = Amps.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 6ca3ce4..55a75d9 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -95,6 +95,7 @@ public class ShooterReal implements ShooterIO { state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); state.indexerOutput = m_indexerMotor.get(); // state.indexerOutput = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 6065683..fa4a750 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems.swerve; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.function.Supplier; @@ -23,6 +24,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -377,6 +379,28 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } + + public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, AngularVelocity ballVelocity, double distanceToHub) { + + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + + if (ballVelocity.in(RotationsPerSecond) > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){ + double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond)); + fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); + Logger.recordOutput("Aim Offset", aimOffset); + } + + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); + + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + + + driveFieldAngle(leftStick, ang); + } + public Pose2d getCurrentPose(){ return state.currentPose; } From 3f26c2eaf2658cffb8f95cfa7c2eadc7ecb75a0d Mon Sep 17 00:00:00 2001 From: mimigamin Date: Wed, 11 Mar 2026 22:00:57 -0600 Subject: [PATCH 03/33] Recommit --- .../java/frc4388/robot/subsystems/swerve/SwerveDrive.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index fa4a750..54e77cd 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -391,8 +391,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); Logger.recordOutput("Aim Offset", aimOffset); + + // double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond))*Math.sqrt(1+Math.pow(7.161/distanceToHub, 2)); + // fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); + // Logger.recordOutput("Aim Offset", aimOffset); } + + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); From 4f361a805deb8cb48e62bc0a6c3a7406d6aa5f72 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Wed, 11 Mar 2026 22:49:27 -0600 Subject: [PATCH 04/33] more calculated calculation --- .../java/frc4388/robot/subsystems/swerve/SwerveDrive.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 54e77cd..a79bfa0 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -388,12 +388,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { ); if (ballVelocity.in(RotationsPerSecond) > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){ - double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond)); + double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity.in(RotationsPerSecond))); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); Logger.recordOutput("Aim Offset", aimOffset); - - // double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond))*Math.sqrt(1+Math.pow(7.161/distanceToHub, 2)); - // fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); + + // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity.in(RotationsPerSecond)) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); // Logger.recordOutput("Aim Offset", aimOffset); } From c4b8e2972e7be654bd544019760a174afc4871ae Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 12 Mar 2026 00:08:36 -0600 Subject: [PATCH 05/33] simulation added --- .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 12 +- .../robot/constants/BuildConstants.java | 10 +- .../subsystems/swerve/SimpleSwerveSim.java | 209 ++++++++++++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 10 + 5 files changed, 235 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 697bfb7..00ab2c5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -61,7 +61,7 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* RobotMap */ - public final RobotMap m_robotMap = new RobotMap(Mode.REAL); + public final RobotMap m_robotMap = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM); /*Limit Switch */ // public final DigitalInput m_armLimitSwitch = new DigitalInput(9); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9791edb..b3dc909 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -25,6 +25,7 @@ import frc4388.robot.subsystems.intake.IntakeReal; import frc4388.robot.subsystems.shooter.ShooterConstants; import frc4388.robot.subsystems.shooter.ShooterIO; import frc4388.robot.subsystems.shooter.ShooterReal; +import frc4388.robot.subsystems.swerve.SimpleSwerveSim; // import frc4388.robot.subsystems.elevator.ElevatorIO; // import frc4388.robot.subsystems.elevator.ElevatorReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; @@ -133,8 +134,15 @@ public class RobotMap { FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); break; - // case SIM: - // break; + case SIM: + leftCamera = new VisionIO() {}; + rightCamera = new VisionIO() {}; + + swerveDrivetrain = new SimpleSwerveSim() {}; + + shooterIO = new ShooterIO() {}; + intakeIO = new IntakeIO() {}; + break; default: leftCamera = new VisionIO() {}; rightCamera = new VisionIO() {}; diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index c801a29..1a1f2b3 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 149; - public static final String GIT_SHA = "50b8312c48be14a559876064cdddbcd27bcb8ef7"; - public static final String GIT_DATE = "2026-03-10 08:39:05 MDT"; + public static final int GIT_REVISION = 152; + public static final String GIT_SHA = "4f361a805deb8cb48e62bc0a6c3a7406d6aa5f72"; + public static final String GIT_DATE = "2026-03-11 22:49:27 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-10 21:05:03 MDT"; - public static final long BUILD_UNIX_TIME = 1773198303540L; + public static final String BUILD_DATE = "2026-03-12 00:02:41 MDT"; + public static final long BUILD_UNIX_TIME = 1773295361724L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java new file mode 100644 index 0000000..78fcd01 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -0,0 +1,209 @@ +package frc4388.robot.subsystems.swerve; + +import java.util.List; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; + +/** + * Minimal swerve simulator implementation for SIM mode. + * - Accepts SwerveRequest controls (tries to extract ChassisSpeeds via reflection) + * - Integrates chassis speeds into a Pose2d each updateInputs call + * - Provides reset/tare/vision correction hooks + * + * Lightweight: does not simulate voltages or module dynamics. Suitable to make + * the robot move in the simulator and provide pose/velocity feedback. + */ +public class SimpleSwerveSim implements SwerveIO { + private Pose2d pose = new Pose2d(); + private Pose2d lastPose = pose; + private double vx = 0.0; // m/s (robot-relative) + private double vy = 0.0; // m/s (robot-relative) + private double omega = 0.0; // rad/s (robot-relative) + + private long lastTimeNs = System.nanoTime(); + + public SimpleSwerveSim() { + } + + @Override + public synchronized void setControl(SwerveRequest ctrl) { + if (ctrl == null) return; + + // Try to extract a ChassisSpeeds field first (common approach) + ChassisSpeeds cs = tryGetSpeedsField(ctrl); + if (cs != null) { + vx = cs.vxMetersPerSecond; + vy = cs.vyMetersPerSecond; + omega = cs.omegaRadiansPerSecond; + return; + } + + // Fallbacks: try to pull numeric fields by common names + try { + Class cls = ctrl.getClass(); + double vxF = tryGetDoubleField(ctrl, cls, "VelocityX", "velocityX", "velocityx", "VelX"); + double vyF = tryGetDoubleField(ctrl, cls, "VelocityY", "velocityY", "velocityy", "VelY"); + double rotF = tryGetDoubleField(ctrl, cls, "RotationalRate", "rotationalRate", "rotationalrate", "omega", "Omega"); + vx = vxF; + vy = vyF; + omega = rotF; + } catch (Exception e) { + // Silently ignore reflection failures and keep previous speeds. + } + } + + private ChassisSpeeds tryGetSpeedsField(SwerveRequest ctrl) { + try { + java.lang.reflect.Field f = ctrl.getClass().getDeclaredField("Speeds"); + f.setAccessible(true); + Object o = f.get(ctrl); + if (o instanceof ChassisSpeeds) { + return (ChassisSpeeds) o; + } + } catch (NoSuchFieldException nsf) { + // ignore + } catch (IllegalAccessException iae) { + // ignore + } catch (SecurityException se) { + // ignore + } + return null; + } + + private double tryGetDoubleField(Object obj, Class cls, String... names) { + for (String n : names) { + try { + java.lang.reflect.Field f = cls.getDeclaredField(n); + f.setAccessible(true); + Object val = f.get(obj); + if (val instanceof Number) { + return ((Number) val).doubleValue(); + } + } catch (NoSuchFieldException nsf) { + // try next name + } catch (IllegalAccessException iae) { + // try next name + } catch (Throwable t) { + // ignore other reflection issues + } + } + return 0.0; + } + + @Override + public synchronized void updateInputs(SwerveState state) { + if (state == null) return; + + long now = System.nanoTime(); + double dt = Math.max(1e-6, (now - lastTimeNs) / 1.0e9); + lastTimeNs = now; + + // Save previous pose + lastPose = pose; + + // Compute robot-relative motion over dt + double dxRobot = vx * dt; + double dyRobot = vy * dt; + double dTheta = omega * dt; + + // Transform robot-relative delta into field frame using current rotation + Rotation2d r = pose.getRotation(); + double cos = r.getCos(); + double sin = r.getSin(); + + double dxField = dxRobot * cos - dyRobot * sin; + double dyField = dxRobot * sin + dyRobot * cos; + + Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField)); + Rotation2d newRot = r.plus(Rotation2d.fromRadians(dTheta)); + pose = new Pose2d(newTrans, newRot); + + // Populate provided SwerveState for callers + state.lastPose = lastPose; + state.currentPose = pose; + state.speeds = new ChassisSpeeds(vx, vy, omega); + state.odometryRate = dt; + } + + + public synchronized void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + System.out.println("It has worked!"); + if (leftStick == null || fieldPos == null) return; + + // current robot speed (robot-relative) + Translation2d robotSpeed = new Translation2d(vx, vy); + + // lead the target by robot motion over aimLeadTime + Translation2d fieldPosLead = fieldPos.plus(robotSpeed.times(aimLeadTime)); + + // compute angle from robot to lead point (field frame) + Rotation2d toLead = fieldPosLead.minus(pose.getTranslation()).getAngle(); + // Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle(); + + // compute shortest angle error (ang_error = desired - current) normalized to [-pi,pi] + double currentYaw = pose.getRotation().getRadians(); + double desiredYaw = toLead.getRadians(); + double error = desiredYaw - currentYaw; + // normalize + while (error > Math.PI) error -= 2 * Math.PI; + while (error < -Math.PI) error += 2 * Math.PI; + + // simple P controller for rotation + final double KP_ROT = 2.0; // tune as needed + final double MAX_OMEGA = 6.0; // rad/s cap + double commandedOmega = KP_ROT * error; + if (commandedOmega > MAX_OMEGA) commandedOmega = MAX_OMEGA; + if (commandedOmega < -MAX_OMEGA) commandedOmega = -MAX_OMEGA; + this.omega = commandedOmega; + + // apply translational command from leftStick (assume stick in [-1,1]) + final double STICK_SPEED_MPS = 3.0; // 3 m/s at full stick; tune as needed + this.vx = leftStick.getX() * STICK_SPEED_MPS; + this.vy = leftStick.getY() * STICK_SPEED_MPS; + } + + @Override + public synchronized void resetPose(Pose2d p) { + if (p == null) return; + pose = p; + lastPose = p; + lastTimeNs = System.nanoTime(); + } + + @Override + public synchronized void tareEverything() { + pose = new Pose2d(); + lastPose = pose; + vx = 0.0; + vy = 0.0; + omega = 0.0; + lastTimeNs = System.nanoTime(); + } + + @Override + public synchronized void addVisionMeasurement(List poses) { + if (poses == null || poses.isEmpty()) return; + Pose2d visPose = poses.get(poses.size() - 1).pose(); + if (visPose != null) { + pose = visPose; + lastPose = visPose; + } + } + + @Override + public synchronized void setLimits(double limitInAmps) { + // No-op for this simple sim + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index a79bfa0..3ee51e6 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -518,6 +518,16 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } io.addVisionMeasurement(vision.getPosesToAdd()); + io.updateInputs(state); + Logger.processInputs("SwerveDrive", state); + + vision.setLastOdomPose(state.currentPose); + setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); + if (state.speeds != null) { + this.chassisSpeeds = state.speeds; + } else { + this.chassisSpeeds = new ChassisSpeeds(); + } } // if(e.isPresent()) From 3ef0a876f602cd296356b4ae63e82c63a1ebb960 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 12 Mar 2026 18:05:14 -0600 Subject: [PATCH 06/33] not working --- .../paths/CPlayerStation-CShoot.path | 2 +- .../paths/HubCenter-PlayerStation.path | 2 +- .../paths/HubFarLeft-NeutralZone 1-2.path | 2 +- .../paths/HubFarLeft-NeutralZone 2-2.path | 2 +- .../paths/HubFarRight-NeutralZone 2-2.path | 2 +- .../paths/HubRight-PlayerStation.path | 2 +- .../paths/PlayerStation-Shoot.path | 2 +- .../deploy/pathplanner/paths/ReadyPush.path | 2 +- .../java/frc4388/robot/RobotContainer.java | 21 ++++++--- .../robot/constants/BuildConstants.java | 10 ++--- .../subsystems/swerve/SimpleSwerveSim.java | 43 +------------------ .../robot/subsystems/swerve/SwerveDrive.java | 1 - 12 files changed, 28 insertions(+), 63 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path index 7325176..dd06473 100644 --- a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path +++ b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path index cbd5c03..3307f61 100644 --- a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path @@ -104,7 +104,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path index cf8bda0..c2d5d1a 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path index 2cab886..ae1d06a 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path @@ -88,7 +88,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path index ddc66c0..79a2251 100644 --- a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path @@ -88,7 +88,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path index 0d931a0..ab1ab91 100644 --- a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path @@ -64,7 +64,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path index 61b8049..992c038 100644 --- a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path +++ b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/ReadyPush.path b/src/main/deploy/pathplanner/paths/ReadyPush.path index 3c8bced..4d01629 100644 --- a/src/main/deploy/pathplanner/paths/ReadyPush.path +++ b/src/main/deploy/pathplanner/paths/ReadyPush.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 00ab2c5..11e2a8e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,6 +40,7 @@ import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; +import frc4388.robot.subsystems.swerve.SimpleSwerveSim; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; @@ -69,6 +70,7 @@ public class RobotContainer { /* Subsystems */ // public final Lidar m_lidar = new Lidar(); public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID); + public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim(); //Testing of Colors public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); @@ -243,14 +245,19 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( + .onTrue(new InstantCommand( () -> { - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldPositions.HUB_POSITION, - ShooterConstants.AIM_LEAD_TIME.get() - ); - }, m_robotSwerveDrive) + m_robotSwerveSIM.driveFacingPosition( + FieldPositions.HUB_POSITION + ); + }) + // () -> { + // m_robotSwerveDrive.driveFacingPosition( + // getDeadbandedDriverController().getLeft(), + // FieldPositions.HUB_POSITION, + // ShooterConstants.AIM_LEAD_TIME.get() + // ); + // }, m_robotSwerveDrive) // () -> { // m_robotSwerveDrive.driveFacingVelocity( // getDeadbandedDriverController().getLeft(), diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 1a1f2b3..0f4ec98 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 152; - public static final String GIT_SHA = "4f361a805deb8cb48e62bc0a6c3a7406d6aa5f72"; - public static final String GIT_DATE = "2026-03-11 22:49:27 MDT"; + public static final int GIT_REVISION = 153; + public static final String GIT_SHA = "c4b8e2972e7be654bd544019760a174afc4871ae"; + public static final String GIT_DATE = "2026-03-12 00:08:36 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-12 00:02:41 MDT"; - public static final long BUILD_UNIX_TIME = 1773295361724L; + public static final String BUILD_DATE = "2026-03-12 18:03:03 MDT"; + public static final long BUILD_UNIX_TIME = 1773360183045L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java index 78fcd01..a79794c 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -132,48 +132,7 @@ public class SimpleSwerveSim implements SwerveIO { } - public synchronized void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - if (leftStick == null || fieldPos == null) return; - - // current robot speed (robot-relative) - Translation2d robotSpeed = new Translation2d(vx, vy); - - // lead the target by robot motion over aimLeadTime - Translation2d fieldPosLead = fieldPos.plus(robotSpeed.times(aimLeadTime)); - - // compute angle from robot to lead point (field frame) - Rotation2d toLead = fieldPosLead.minus(pose.getTranslation()).getAngle(); - // Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle(); - - // compute shortest angle error (ang_error = desired - current) normalized to [-pi,pi] - double currentYaw = pose.getRotation().getRadians(); - double desiredYaw = toLead.getRadians(); - double error = desiredYaw - currentYaw; - // normalize - while (error > Math.PI) error -= 2 * Math.PI; - while (error < -Math.PI) error += 2 * Math.PI; - - // simple P controller for rotation - final double KP_ROT = 2.0; // tune as needed - final double MAX_OMEGA = 6.0; // rad/s cap - double commandedOmega = KP_ROT * error; - if (commandedOmega > MAX_OMEGA) commandedOmega = MAX_OMEGA; - if (commandedOmega < -MAX_OMEGA) commandedOmega = -MAX_OMEGA; - this.omega = commandedOmega; - - // apply translational command from leftStick (assume stick in [-1,1]) - final double STICK_SPEED_MPS = 3.0; // 3 m/s at full stick; tune as needed - this.vx = leftStick.getX() * STICK_SPEED_MPS; - this.vy = leftStick.getY() * STICK_SPEED_MPS; - } - + @Override public synchronized void resetPose(Pose2d p) { if (p == null) return; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 3ee51e6..071a21d 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -362,7 +362,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // Drive with the robot facing towards a specific position public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { - // Get the current speed of the robot Translation2d robotSpeed = new Translation2d( chassisSpeeds.vxMetersPerSecond, From af3646ff4378382f3bfcce6ab7828823bb76be21 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 12 Mar 2026 18:06:12 -0600 Subject: [PATCH 07/33] Update RobotContainer.java --- src/main/java/frc4388/robot/RobotContainer.java | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 11e2a8e..582c218 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -246,17 +246,11 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) .onTrue(new InstantCommand( - () -> { - m_robotSwerveSIM.driveFacingPosition( - FieldPositions.HUB_POSITION - ); - }) - // () -> { - // m_robotSwerveDrive.driveFacingPosition( - // getDeadbandedDriverController().getLeft(), - // FieldPositions.HUB_POSITION, - // ShooterConstants.AIM_LEAD_TIME.get() - // ); + () -> m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldPositions.HUB_POSITION, + ShooterConstants.AIM_LEAD_TIME.get() + ), m_robotSwerveDrive) // }, m_robotSwerveDrive) // () -> { // m_robotSwerveDrive.driveFacingVelocity( From 0d98232f30a0663939bef0d21533e1603090c7cd Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 12 Mar 2026 18:13:33 -0600 Subject: [PATCH 08/33] fix swerve --- .../robot/constants/BuildConstants.java | 10 ++-- .../subsystems/swerve/SimpleSwerveSim.java | 56 ++++++------------- 2 files changed, 22 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 0f4ec98..fc42833 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 153; - public static final String GIT_SHA = "c4b8e2972e7be654bd544019760a174afc4871ae"; - public static final String GIT_DATE = "2026-03-12 00:08:36 MDT"; + public static final int GIT_REVISION = 155; + public static final String GIT_SHA = "af3646ff4378382f3bfcce6ab7828823bb76be21"; + public static final String GIT_DATE = "2026-03-12 18:06:12 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-12 18:03:03 MDT"; - public static final long BUILD_UNIX_TIME = 1773360183045L; + public static final String BUILD_DATE = "2026-03-12 18:11:48 MDT"; + public static final long BUILD_UNIX_TIME = 1773360708923L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java index a79794c..825569e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -10,21 +10,12 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; -/** - * Minimal swerve simulator implementation for SIM mode. - * - Accepts SwerveRequest controls (tries to extract ChassisSpeeds via reflection) - * - Integrates chassis speeds into a Pose2d each updateInputs call - * - Provides reset/tare/vision correction hooks - * - * Lightweight: does not simulate voltages or module dynamics. Suitable to make - * the robot move in the simulator and provide pose/velocity feedback. - */ public class SimpleSwerveSim implements SwerveIO { private Pose2d pose = new Pose2d(); private Pose2d lastPose = pose; - private double vx = 0.0; // m/s (robot-relative) - private double vy = 0.0; // m/s (robot-relative) - private double omega = 0.0; // rad/s (robot-relative) + private double vx = 0.0; + private double vy = 0.0; + private double omega = 0.0; private long lastTimeNs = System.nanoTime(); @@ -35,7 +26,6 @@ public class SimpleSwerveSim implements SwerveIO { public synchronized void setControl(SwerveRequest ctrl) { if (ctrl == null) return; - // Try to extract a ChassisSpeeds field first (common approach) ChassisSpeeds cs = tryGetSpeedsField(ctrl); if (cs != null) { vx = cs.vxMetersPerSecond; @@ -44,7 +34,6 @@ public class SimpleSwerveSim implements SwerveIO { return; } - // Fallbacks: try to pull numeric fields by common names try { Class cls = ctrl.getClass(); double vxF = tryGetDoubleField(ctrl, cls, "VelocityX", "velocityX", "velocityx", "VelX"); @@ -54,7 +43,6 @@ public class SimpleSwerveSim implements SwerveIO { vy = vyF; omega = rotF; } catch (Exception e) { - // Silently ignore reflection failures and keep previous speeds. } } @@ -67,11 +55,8 @@ public class SimpleSwerveSim implements SwerveIO { return (ChassisSpeeds) o; } } catch (NoSuchFieldException nsf) { - // ignore } catch (IllegalAccessException iae) { - // ignore } catch (SecurityException se) { - // ignore } return null; } @@ -86,11 +71,8 @@ public class SimpleSwerveSim implements SwerveIO { return ((Number) val).doubleValue(); } } catch (NoSuchFieldException nsf) { - // try next name } catch (IllegalAccessException iae) { - // try next name } catch (Throwable t) { - // ignore other reflection issues } } return 0.0; @@ -104,35 +86,22 @@ public class SimpleSwerveSim implements SwerveIO { double dt = Math.max(1e-6, (now - lastTimeNs) / 1.0e9); lastTimeNs = now; - // Save previous pose lastPose = pose; - // Compute robot-relative motion over dt - double dxRobot = vx * dt; - double dyRobot = vy * dt; + double dxField = vx * dt; + double dyField = vy * dt; double dTheta = omega * dt; - // Transform robot-relative delta into field frame using current rotation - Rotation2d r = pose.getRotation(); - double cos = r.getCos(); - double sin = r.getSin(); - - double dxField = dxRobot * cos - dyRobot * sin; - double dyField = dxRobot * sin + dyRobot * cos; - Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField)); - Rotation2d newRot = r.plus(Rotation2d.fromRadians(dTheta)); + Rotation2d newRot = pose.getRotation().plus(Rotation2d.fromRadians(dTheta)); pose = new Pose2d(newTrans, newRot); - // Populate provided SwerveState for callers state.lastPose = lastPose; state.currentPose = pose; state.speeds = new ChassisSpeeds(vx, vy, omega); state.odometryRate = dt; } - - @Override public synchronized void resetPose(Pose2d p) { if (p == null) return; @@ -161,8 +130,17 @@ public class SimpleSwerveSim implements SwerveIO { } } + public synchronized void pointAt(Translation2d target) { + if (target == null) return; + Translation2d toTarget = target.minus(pose.getTranslation()); + if (toTarget.getNorm() < 1e-9) return; + Rotation2d desired = toTarget.getAngle(); + pose = new Pose2d(pose.getTranslation(), desired); + lastPose = pose; + omega = 0.0; + } + @Override public synchronized void setLimits(double limitInAmps) { - // No-op for this simple sim } -} \ No newline at end of file +} From 2de8c605970a55114d3a66ebfb26160df3e0aa50 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 12 Mar 2026 18:35:44 -0600 Subject: [PATCH 09/33] Fix error --- src/main/java/frc4388/robot/RobotContainer.java | 10 ++++------ .../frc4388/robot/constants/BuildConstants.java | 10 +++++----- .../robot/subsystems/swerve/SimpleSwerveSim.java | 13 +++++++++++++ 3 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 582c218..c4eca1b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -245,13 +245,11 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .onTrue(new InstantCommand( - () -> m_robotSwerveDrive.driveFacingPosition( + .whileTrue(new RunCommand(() -> m_robotSwerveDrive.driveFacingPosition( getDeadbandedDriverController().getLeft(), - FieldPositions.HUB_POSITION, - ShooterConstants.AIM_LEAD_TIME.get() - ), m_robotSwerveDrive) - // }, m_robotSwerveDrive) + FieldPositions.HUB_POSITION, + ShooterConstants.AIM_LEAD_TIME.get() + ), m_robotSwerveDrive) // () -> { // m_robotSwerveDrive.driveFacingVelocity( // getDeadbandedDriverController().getLeft(), diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index fc42833..e426645 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 155; - public static final String GIT_SHA = "af3646ff4378382f3bfcce6ab7828823bb76be21"; - public static final String GIT_DATE = "2026-03-12 18:06:12 MDT"; + public static final int GIT_REVISION = 156; + public static final String GIT_SHA = "0d98232f30a0663939bef0d21533e1603090c7cd"; + public static final String GIT_DATE = "2026-03-12 18:13:33 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-12 18:11:48 MDT"; - public static final long BUILD_UNIX_TIME = 1773360708923L; + public static final String BUILD_DATE = "2026-03-12 18:30:13 MDT"; + public static final long BUILD_UNIX_TIME = 1773361813662L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java index 825569e..75542fd 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -78,6 +78,19 @@ public class SimpleSwerveSim implements SwerveIO { return 0.0; } + + public synchronized void pointAtXY(double x, double y) { + Translation2d target = new Translation2d(x, y); + Translation2d toTarget = target.minus(pose.getTranslation()); + if (toTarget.getNorm() < 1e-9) return; + Rotation2d desired = toTarget.getAngle(); + pose = new Pose2d(pose.getTranslation(), desired); + lastPose = pose; + vx = 0.0; + vy = 0.0; + omega = 0.0; + } + @Override public synchronized void updateInputs(SwerveState state) { if (state == null) return; From 07ec609b019fd8db439e14beb74c50fdbf75a091 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 14 Mar 2026 15:41:27 -0600 Subject: [PATCH 10/33] fix simulation auto --- simgui-ds.json | 5 ---- .../robot/constants/BuildConstants.java | 10 ++++---- .../subsystems/swerve/SimpleSwerveSim.java | 24 +++++++++++++++++-- 3 files changed, 27 insertions(+), 12 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 4a63cc1..73cc713 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index e426645..586e645 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 156; - public static final String GIT_SHA = "0d98232f30a0663939bef0d21533e1603090c7cd"; - public static final String GIT_DATE = "2026-03-12 18:13:33 MDT"; + public static final int GIT_REVISION = 157; + public static final String GIT_SHA = "2de8c605970a55114d3a66ebfb26160df3e0aa50"; + public static final String GIT_DATE = "2026-03-12 18:35:44 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-12 18:30:13 MDT"; - public static final long BUILD_UNIX_TIME = 1773361813662L; + public static final String BUILD_DATE = "2026-03-14 15:24:12 MDT"; + public static final long BUILD_UNIX_TIME = 1773523452704L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java index 75542fd..a374ac8 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; public class SimpleSwerveSim implements SwerveIO { @@ -100,9 +102,27 @@ public class SimpleSwerveSim implements SwerveIO { lastTimeNs = now; lastPose = pose; + double dxField; + double dyField; + + if (DriverStation.isAutonomous()) { + double dxRobot = vx * dt; + double dyRobot = vy * dt; + + Rotation2d r = pose.getRotation(); + double cos = r.getCos(); + double sin = r.getSin(); + + dxField = dxRobot * cos - dyRobot * sin; + dyField = dxRobot * sin + dyRobot * cos; + + } else { + dxField = vx * dt; + dyField = vy * dt; + + } + - double dxField = vx * dt; - double dyField = vy * dt; double dTheta = omega * dt; Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField)); From ebab028818fc0362daa4d7eb1010131819edd1cb Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 14 Mar 2026 18:29:15 -0600 Subject: [PATCH 11/33] WIP Tweaked shooter offset model and new progress with X position --- .../java/frc4388/robot/RobotContainer.java | 25 +++++++++---- .../robot/constants/BuildConstants.java | 10 ++--- .../robot/subsystems/shooter/Shooter.java | 5 ++- .../subsystems/shooter/ShooterConstants.java | 3 +- .../robot/subsystems/shooter/ShooterIO.java | 2 +- .../robot/subsystems/shooter/ShooterReal.java | 4 +- .../robot/subsystems/swerve/SwerveDrive.java | 37 +++++++++++++------ .../swerve/SwerveDriveConstants.java | 14 ++++--- .../robot/subsystems/swerve/SwerveIO.java | 6 +++ .../robot/subsystems/swerve/SwerveReal.java | 11 ++++++ .../frc4388/utility/status/CanDevice.java | 5 +++ 11 files changed, 87 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c4eca1b..1b8278b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -231,18 +231,27 @@ public class RobotContainer { m_robotSwerveDrive.shiftDownRot(); })); - //TEST - > Swerve drive pid on position + //TEST - > Defense: X position on wheels and swerve drive pid on position new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> { - currentPose = m_robotSwerveDrive.getCurrentPose(); - })) - .whileTrue(new RunCommand(() -> { - m_stayInPosition.goToTargetPose(currentPose); + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.defenseXPosition(); }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.softStop(); + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.stopDefenseXPosition(); + + // .onTrue(new InstantCommand(() -> { + // currentPose = m_robotSwerveDrive.getCurrentPose(); + // })) + // .whileTrue(new RunCommand(() -> { + // m_stayInPosition.goToTargetPose(currentPose); + // }, m_robotSwerveDrive)) + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.softStop(); + // })); })); + + // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) .whileTrue(new RunCommand(() -> m_robotSwerveDrive.driveFacingPosition( diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 586e645..d8311c9 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 157; - public static final String GIT_SHA = "2de8c605970a55114d3a66ebfb26160df3e0aa50"; - public static final String GIT_DATE = "2026-03-12 18:35:44 MDT"; + public static final int GIT_REVISION = 158; + public static final String GIT_SHA = "07ec609b019fd8db439e14beb74c50fdbf75a091"; + public static final String GIT_DATE = "2026-03-14 15:41:27 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 15:24:12 MDT"; - public static final long BUILD_UNIX_TIME = 1773523452704L; + public static final String BUILD_DATE = "2026-03-14 17:27:47 MDT"; + public static final long BUILD_UNIX_TIME = 1773530867415L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index ed625f9..0319706 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -89,8 +89,9 @@ public class Shooter extends SubsystemBase { shooterButtonReady = false; } - public AngularVelocity getBallVelocity() { - return RotationsPerSecond.of((state.motor1Velocity.in(RotationsPerSecond) + state.indexerVelocity.in(RotationsPerSecond))); + public double getBallVelocity() { + return Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) * ShooterConstants.SHOOTER_RADIUS * 2 * Math.PI; + //Math.abs(state.indexerForwardVelocity.in(RotationsPerSecond))*ShooterConstants.INDEXER_RADIUS) } @AutoLogOutput diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index d55e93b..f074a74 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -18,7 +18,8 @@ public class ShooterConstants { public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; public static final double T_CONSTANT = 2; - + public static final double SHOOTER_RADIUS = 2/39.37; + public static final double INDEXER_RADIUS = 0.625/39.37; public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60); public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42); // public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 9b32782..6be6431 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -26,7 +26,7 @@ public interface ShooterIO { AngularVelocity motor1Velocity = RotationsPerSecond.of(0); AngularVelocity motor2Velocity = RotationsPerSecond.of(0); - AngularVelocity indexerVelocity = RotationsPerSecond.of(0); + AngularVelocity indexerForwardVelocity = RotationsPerSecond.of(42.53); //guestimate double indexerOutput = 0; Current motor1Current = Amps.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 55a75d9..f173553 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -87,6 +87,9 @@ public class ShooterReal implements ShooterIO { public void setIndexerOutput(ShooterState state, double percentOutput) { state.indexerTargetOutput = percentOutput; m_indexerMotor.set(percentOutput); + if (state.indexerTargetOutput - state.indexerOutput > 0.05){ + state.indexerForwardVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); + } } @@ -95,7 +98,6 @@ public class ShooterReal implements ShooterIO { state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); - state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); state.indexerOutput = m_indexerMotor.get(); // state.indexerOutput = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 071a21d..69d865f 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -150,14 +150,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void setInitalPose(Pose2d startingAutoPose){ initalPose2d = startingAutoPose; } - // MIRA public void setOdoPose(Pose2d pose) { - // if (pose == null) return; - // io.tareEverything(); - // initalPose2d = pose; - // io.resetPose(pose); - // robotKnowsWhereItIs = true; - // rotTarget = pose.getRotation().getDegrees(); - // } // public void oneModuleTest(SwerveModule module, Translation2d leftStick, @@ -378,20 +370,41 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } + public void defenseXPosition(){ + io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_FRONT_STEER, Rotation2d.fromDegrees(45.0)); + io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER, Rotation2d.fromDegrees(-45.0)); + io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_BACK_STEER, Rotation2d.fromDegrees(-45.0)); + io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_BACK_STEER, Rotation2d.fromDegrees(45.0)); + } - public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, AngularVelocity ballVelocity, double distanceToHub) { + public void stopDefenseXPosition(){ + io.restoreSteerOffsets(); + } + + public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { + // Calculate the angle between the current position and the lead position + //Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle(); + Rotation2d ang = new Rotation2d(0,1); + System.out.println(ang); + + driveFieldAngle(leftStick, ang); + } + + + + public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, double ballVelocity, double distanceToHub) { Translation2d robotSpeed = new Translation2d( chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond ); - if (ballVelocity.in(RotationsPerSecond) > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){ - double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity.in(RotationsPerSecond))); + if (ballVelocity > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){ + double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); Logger.recordOutput("Aim Offset", aimOffset); - // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity.in(RotationsPerSecond)) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); + // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); // Logger.recordOutput("Aim Offset", aimOffset); } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index e295b7f..a0c47d0 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -75,9 +75,10 @@ public final class SwerveDriveConstants { // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); - private static final class ModuleSpecificConstants { //2025 + public static final class ModuleSpecificConstants { //2026 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); + public static final Angle FRONT_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); + public static Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -85,7 +86,8 @@ public final class SwerveDriveConstants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); + public static final Angle FRONT_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.465332+0.3+0.003174-0.0103); + public static Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -93,7 +95,8 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); + public static final Angle BACK_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.023438+0.5+0.0168-0.00562); + public static Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -101,7 +104,8 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); + public static final Angle BACK_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.029541+0.05-0.002197-0.00366); + public static Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java index fa7de1a..8d81397 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -7,8 +7,10 @@ import org.littletonrobotics.junction.AutoLog; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.CanDevice; public interface SwerveIO { @AutoLog @@ -25,6 +27,10 @@ public interface SwerveIO { public default void tareEverything() {} + public default void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {} + + public default void restoreSteerOffsets(){} + public default void resetPose(Pose2d pose) {} public default void addVisionMeasurement(List poses) {} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index c4ab4d6..ae1c594 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -11,8 +11,10 @@ import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import frc4388.robot.subsystems.vision.Vision; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; +import frc4388.utility.status.CanDevice; public class SwerveReal implements SwerveIO { SwerveDrivetrain swerveDriveTrain; @@ -55,6 +57,15 @@ public class SwerveReal implements SwerveIO { } } + @Override + public void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) { + } + + @Override + public void restoreSteerOffsets() { + + } + @Override public void setLimits(double limitInAmps) { for (SwerveModule module : swerveDriveTrain.getModules()) { diff --git a/src/main/java/frc4388/utility/status/CanDevice.java b/src/main/java/frc4388/utility/status/CanDevice.java index ef72647..ed49c4f 100644 --- a/src/main/java/frc4388/utility/status/CanDevice.java +++ b/src/main/java/frc4388/utility/status/CanDevice.java @@ -49,5 +49,10 @@ public class CanDevice { return s; } + + public Object getId() { + return id; + } + } From 8c8ac261397954f1b5a118cb96d4e5f60d70da81 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 14 Mar 2026 18:57:04 -0600 Subject: [PATCH 12/33] Finish X position and PID position --- .../java/frc4388/robot/RobotContainer.java | 28 ++++---- .../robot/commands/Swerve/StayInPosition.java | 66 +++++++++---------- .../robot/constants/BuildConstants.java | 10 +-- .../robot/subsystems/swerve/SwerveDrive.java | 29 ++++++-- .../swerve/SwerveDriveConstants.java | 12 ++-- .../robot/subsystems/swerve/SwerveIO.java | 4 -- .../robot/subsystems/swerve/SwerveReal.java | 9 --- 7 files changed, 76 insertions(+), 82 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b8278b..e33d8bb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -233,21 +233,21 @@ public class RobotContainer { //TEST - > Defense: X position on wheels and swerve drive pid on position new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .whileTrue(new RunCommand(() -> { - m_robotSwerveDrive.defenseXPosition(); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.stopDefenseXPosition(); - - // .onTrue(new InstantCommand(() -> { - // currentPose = m_robotSwerveDrive.getCurrentPose(); - // })) - // .whileTrue(new RunCommand(() -> { - // m_stayInPosition.goToTargetPose(currentPose); + // .whileTrue(new RunCommand(() -> { + // m_robotSwerveDrive.defenseXPosition(); // }, m_robotSwerveDrive)) - // .onFalse(new InstantCommand(() -> { - // m_robotSwerveDrive.softStop(); - // })); + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.stopDefenseXPosition(); + + .onTrue(new InstantCommand(() -> { + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); + })); diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java index 8d9be18..4b66de2 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -5,52 +5,46 @@ package frc4388.robot.commands.Swerve; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; import frc4388.robot.subsystems.swerve.SwerveDrive; public class StayInPosition extends PID { + SwerveDrive drive; - SwerveDrive drive; + public StayInPosition(SwerveDrive drive) { + super(0.3, 0.0, 0.0, 0.0, 1); + this.drive = drive; + addRequirements(drive); + } - /** Creates a new StayInPosition. */ - public StayInPosition(SwerveDrive drive) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.drive = drive; + public void goToTargetPose(Pose2d targetPose) { + Pose2d currentPose = drive.getCurrentPose(); - addRequirements(drive); - } + double translationX = targetPose.getX() - currentPose.getX(); + double translationY = targetPose.getY() - currentPose.getY(); - public void goToTargetPose(Pose2d targetPose){ - Pose2d currentPose = drive.getCurrentPose(); - double translationX = targetPose.getX() - currentPose.getX(); - double translationY = targetPose.getY() - currentPose.getY(); - Rotation2d deltaRotation = targetPose.getRotation().minus(currentPose.getRotation()); - Translation2d driveTranslation = new Translation2d(translationX, translationY); + double magnitude = Math.sqrt(translationX * translationX + translationY * translationY); + if (magnitude > 1.0) { + translationX /= magnitude; + translationY /= magnitude; + } - drive.driveFieldAngle(driveTranslation, deltaRotation); - } + Translation2d driveTranslation; + if (magnitude < 0.05) { + driveTranslation = new Translation2d(); + } else { + driveTranslation = new Translation2d(translationX, translationY); + } - @Override - public double getError() { - return 0; - // return targetAngle - drive.getGyroAngle(); - } + drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); + } - @Override - public void runWithOutput(double output) { - // drive.driveWithInput(new Pose2d(new Translation2d(0.0, 0.0), new Rotation2d(output / Math.abs(getError())))); - } + @Override + public double getError() { + return 0; + } - // @Override - // public boolean isFinished() { - // Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation(); - // double ballAngleDeg = m_lidar.getLatestBallAngleDegrees(); - - // // TODO: Tune - // return Math.abs(curRot.getDegrees() +ballAngleDeg) < 5; - // } - -} + @Override + public void runWithOutput(double output) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index d8311c9..44b0323 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 158; - public static final String GIT_SHA = "07ec609b019fd8db439e14beb74c50fdbf75a091"; - public static final String GIT_DATE = "2026-03-14 15:41:27 MDT"; + public static final int GIT_REVISION = 159; + public static final String GIT_SHA = "ebab028818fc0362daa4d7eb1010131819edd1cb"; + public static final String GIT_DATE = "2026-03-14 18:29:15 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 17:27:47 MDT"; - public static final long BUILD_UNIX_TIME = 1773530867415L; + public static final String BUILD_DATE = "2026-03-14 18:46:53 MDT"; + public static final long BUILD_UNIX_TIME = 1773535613285L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 69d865f..e90be1b 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -295,7 +295,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // SmartDashboard.putBoolean("drift correction", true); } - + public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) { + + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + + rotTarget = heading.getDegrees(); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD + ); + io.setControl(ctrl); + } public void driveIntake(Translation2d leftStick, boolean invertRotation){ // if (invert){ @@ -371,14 +391,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public void defenseXPosition(){ - io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_FRONT_STEER, Rotation2d.fromDegrees(45.0)); - io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER, Rotation2d.fromDegrees(-45.0)); - io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_BACK_STEER, Rotation2d.fromDegrees(-45.0)); - io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_BACK_STEER, Rotation2d.fromDegrees(45.0)); + io.setControl(new SwerveRequest.SwerveDriveBrake()); } public void stopDefenseXPosition(){ - io.restoreSteerOffsets(); + stopModules(); } public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index a0c47d0..0034c0d 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -77,8 +77,7 @@ public final class SwerveDriveConstants { public static final class ModuleSpecificConstants { //2026 //Front Left - public static final Angle FRONT_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); - public static Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -86,8 +85,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - public static final Angle FRONT_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.465332+0.3+0.003174-0.0103); - public static Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -95,8 +93,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - public static final Angle BACK_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.023438+0.5+0.0168-0.00562); - public static Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -104,8 +101,7 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - public static final Angle BACK_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.029541+0.05-0.002197-0.00366); - public static Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java index 8d81397..25db223 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -27,10 +27,6 @@ public interface SwerveIO { public default void tareEverything() {} - public default void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {} - - public default void restoreSteerOffsets(){} - public default void resetPose(Pose2d pose) {} public default void addVisionMeasurement(List poses) {} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index ae1c594..9574699 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -56,15 +56,6 @@ public class SwerveReal implements SwerveIO { swerveDriveTrain.addVisionMeasurement(pose, Utils.fpgaToCurrentTime(Vision.getTime())); } } - - @Override - public void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) { - } - - @Override - public void restoreSteerOffsets() { - - } @Override public void setLimits(double limitInAmps) { From f5ecfd0be19548215558eee7217cf6eb1b8c487b Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 14 Mar 2026 20:00:21 -0600 Subject: [PATCH 13/33] Finish simulation point to hub --- simgui-ds.json | 5 ++ .../robot/commands/Swerve/StayInPosition.java | 21 ++++--- .../robot/constants/BuildConstants.java | 10 ++-- .../subsystems/swerve/SimpleSwerveSim.java | 58 ++++++++++++++----- .../robot/subsystems/swerve/SwerveDrive.java | 9 +-- 5 files changed, 68 insertions(+), 35 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4a63cc1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java index 4b66de2..b306a27 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -4,6 +4,9 @@ package frc4388.robot.commands.Swerve; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.AutoLogOutput; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; @@ -11,6 +14,7 @@ import frc4388.robot.subsystems.swerve.SwerveDrive; public class StayInPosition extends PID { SwerveDrive drive; + Translation2d driveTranslation = new Translation2d(); public StayInPosition(SwerveDrive drive) { super(0.3, 0.0, 0.0, 0.0, 1); @@ -20,24 +24,23 @@ public class StayInPosition extends PID { public void goToTargetPose(Pose2d targetPose) { Pose2d currentPose = drive.getCurrentPose(); - double translationX = targetPose.getX() - currentPose.getX(); double translationY = targetPose.getY() - currentPose.getY(); - - double magnitude = Math.sqrt(translationX * translationX + translationY * translationY); - if (magnitude > 1.0) { - translationX /= magnitude; - translationY /= magnitude; + if (translationX > 0.8){ + translationX = 0.8; } - - Translation2d driveTranslation; - if (magnitude < 0.05) { + if (translationY > 0.8){ + translationY = 0.8; + } + if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) { driveTranslation = new Translation2d(); } else { driveTranslation = new Translation2d(translationX, translationY); } drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); + // If above doesn't work + //drive.driveFieldAngle(driveTranslation, targetPose.getRotation()); } @Override diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 44b0323..16f7820 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 159; - public static final String GIT_SHA = "ebab028818fc0362daa4d7eb1010131819edd1cb"; - public static final String GIT_DATE = "2026-03-14 18:29:15 MDT"; + public static final int GIT_REVISION = 160; + public static final String GIT_SHA = "8c8ac261397954f1b5a118cb96d4e5f60d70da81"; + public static final String GIT_DATE = "2026-03-14 18:57:04 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 18:46:53 MDT"; - public static final long BUILD_UNIX_TIME = 1773535613285L; + public static final String BUILD_DATE = "2026-03-14 19:59:14 MDT"; + public static final long BUILD_UNIX_TIME = 1773539954157L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java index a374ac8..3a177bb 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -24,27 +24,59 @@ public class SimpleSwerveSim implements SwerveIO { public SimpleSwerveSim() { } - @Override public synchronized void setControl(SwerveRequest ctrl) { if (ctrl == null) return; + // Handle FieldCentricFacingAngle — compute omega from target direction + if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) { + vx = facingAngle.VelocityX; + vy = facingAngle.VelocityY; + + // Simple P controller to rotate toward target + double currentAngle = pose.getRotation().getRadians(); + double targetAngle = facingAngle.TargetDirection.getRadians(); + double error = targetAngle - currentAngle; + + // Wrap error to [-pi, pi] + error = Math.atan2(Math.sin(error), Math.cos(error)); + + double kP = 5.0; // tune this — matches PathPlanner's rotation PID + omega = error * kP; + return; + } + + // Handle FieldCentric (normal driving with explicit rotational rate) + if (ctrl instanceof SwerveRequest.FieldCentric fc) { + vx = fc.VelocityX; + vy = fc.VelocityY; + omega = fc.RotationalRate; + return; + } + + if (ctrl instanceof SwerveRequest.RobotCentric rc) { + // rotate velocity into field frame + double cos = pose.getRotation().getCos(); + double sin = pose.getRotation().getSin(); + double vxRobot = rc.VelocityX; + double vyRobot = rc.VelocityY; + vx = vxRobot * cos - vyRobot * sin; + vy = vxRobot * sin + vyRobot * cos; + omega = rc.RotationalRate; + return; + } + + // Handle brake + if (ctrl instanceof SwerveRequest.SwerveDriveBrake) { + vx = 0; vy = 0; omega = 0; + return; + } + + // Fallback: your original reflection approach ChassisSpeeds cs = tryGetSpeedsField(ctrl); if (cs != null) { vx = cs.vxMetersPerSecond; vy = cs.vyMetersPerSecond; omega = cs.omegaRadiansPerSecond; - return; - } - - try { - Class cls = ctrl.getClass(); - double vxF = tryGetDoubleField(ctrl, cls, "VelocityX", "velocityX", "velocityx", "VelX"); - double vyF = tryGetDoubleField(ctrl, cls, "VelocityY", "velocityY", "velocityy", "VelY"); - double rotF = tryGetDoubleField(ctrl, cls, "RotationalRate", "rotationalRate", "rotationalrate", "omega", "Omega"); - vx = vxF; - vy = vyF; - omega = rotF; - } catch (Exception e) { } } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index e90be1b..d3664f3 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -297,11 +297,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) { - leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); - - rotTarget = heading.getDegrees(); - + rotTarget = heading.getDegrees(); var ctrl = new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) @@ -310,9 +306,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { SwerveDriveConstants.PIDConstants.AIM_kP.get(), SwerveDriveConstants.PIDConstants.AIM_kI.get(), SwerveDriveConstants.PIDConstants.AIM_kD.get() - // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP, - // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI, - // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD ); io.setControl(ctrl); } From aac68dad5eabfaa1e098ad8cf6e42cd3bddd1ef3 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 14 Mar 2026 20:20:55 -0600 Subject: [PATCH 14/33] Updates --- .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- .../robot/subsystems/swerve/SimpleSwerveSim.java | 9 +-------- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 16f7820..c2a2676 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 160; - public static final String GIT_SHA = "8c8ac261397954f1b5a118cb96d4e5f60d70da81"; - public static final String GIT_DATE = "2026-03-14 18:57:04 MDT"; + public static final int GIT_REVISION = 161; + public static final String GIT_SHA = "f5ecfd0be19548215558eee7217cf6eb1b8c487b"; + public static final String GIT_DATE = "2026-03-14 20:00:21 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 19:59:14 MDT"; - public static final long BUILD_UNIX_TIME = 1773539954157L; + public static final String BUILD_DATE = "2026-03-14 20:18:36 MDT"; + public static final long BUILD_UNIX_TIME = 1773541116232L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java index 3a177bb..44937da 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -27,25 +27,21 @@ public class SimpleSwerveSim implements SwerveIO { public synchronized void setControl(SwerveRequest ctrl) { if (ctrl == null) return; - // Handle FieldCentricFacingAngle — compute omega from target direction if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) { vx = facingAngle.VelocityX; vy = facingAngle.VelocityY; - // Simple P controller to rotate toward target double currentAngle = pose.getRotation().getRadians(); double targetAngle = facingAngle.TargetDirection.getRadians(); double error = targetAngle - currentAngle; - // Wrap error to [-pi, pi] error = Math.atan2(Math.sin(error), Math.cos(error)); - double kP = 5.0; // tune this — matches PathPlanner's rotation PID + double kP = 5.0; omega = error * kP; return; } - // Handle FieldCentric (normal driving with explicit rotational rate) if (ctrl instanceof SwerveRequest.FieldCentric fc) { vx = fc.VelocityX; vy = fc.VelocityY; @@ -54,7 +50,6 @@ public class SimpleSwerveSim implements SwerveIO { } if (ctrl instanceof SwerveRequest.RobotCentric rc) { - // rotate velocity into field frame double cos = pose.getRotation().getCos(); double sin = pose.getRotation().getSin(); double vxRobot = rc.VelocityX; @@ -65,13 +60,11 @@ public class SimpleSwerveSim implements SwerveIO { return; } - // Handle brake if (ctrl instanceof SwerveRequest.SwerveDriveBrake) { vx = 0; vy = 0; omega = 0; return; } - // Fallback: your original reflection approach ChassisSpeeds cs = tryGetSpeedsField(ctrl); if (cs != null) { vx = cs.vxMetersPerSecond; From 45bffde181b7666a394a605be87c43afc8dd234b Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 14 Mar 2026 20:34:55 -0600 Subject: [PATCH 15/33] Better Defense --- .../frc4388/robot/commands/Swerve/StayInPosition.java | 10 +++++----- .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java index b306a27..b43c576 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -26,16 +26,16 @@ public class StayInPosition extends PID { Pose2d currentPose = drive.getCurrentPose(); double translationX = targetPose.getX() - currentPose.getX(); double translationY = targetPose.getY() - currentPose.getY(); - if (translationX > 0.8){ - translationX = 0.8; + if (translationX > 0.2){ + translationX = 0.2; } - if (translationY > 0.8){ - translationY = 0.8; + if (translationY > 0.2){ + translationY = 0.2; } if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) { driveTranslation = new Translation2d(); } else { - driveTranslation = new Translation2d(translationX, translationY); + driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5); } drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index c2a2676..53210d2 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 161; - public static final String GIT_SHA = "f5ecfd0be19548215558eee7217cf6eb1b8c487b"; - public static final String GIT_DATE = "2026-03-14 20:00:21 MDT"; + public static final int GIT_REVISION = 162; + public static final String GIT_SHA = "aac68dad5eabfaa1e098ad8cf6e42cd3bddd1ef3"; + public static final String GIT_DATE = "2026-03-14 20:20:55 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 20:18:36 MDT"; - public static final long BUILD_UNIX_TIME = 1773541116232L; + public static final String BUILD_DATE = "2026-03-14 20:33:21 MDT"; + public static final long BUILD_UNIX_TIME = 1773542001176L; public static final int DIRTY = 1; private BuildConstants(){} From f4a42f02eed0fa3b0950a05da55174b9f4b89ce7 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 14 Mar 2026 20:39:45 -0600 Subject: [PATCH 16/33] Update StayInPosition.java --- .../java/frc4388/robot/commands/Swerve/StayInPosition.java | 5 ++++- .../java/frc4388/robot/subsystems/shooter/ShooterIO.java | 4 ++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java index b43c576..afbfaa3 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -27,15 +27,18 @@ public class StayInPosition extends PID { double translationX = targetPose.getX() - currentPose.getX(); double translationY = targetPose.getY() - currentPose.getY(); if (translationX > 0.2){ + //If below is changed make this change too so it never goes over 1 translationX = 0.2; } if (translationY > 0.2){ + //Same here translationY = 0.2; } if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) { driveTranslation = new Translation2d(); } else { - driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5); + //TODO: Tweak for best corrector against another bot + driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5); } drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 6be6431..4424848 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -19,14 +19,14 @@ public interface ShooterIO { // Angle shooterTargetPitch = Rotations.of(0); // Current pitchMotorCurrent = Amps.of(0); - AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(40); // For modeling purposes AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); double indexerTargetOutput = 0; AngularVelocity motor1Velocity = RotationsPerSecond.of(0); AngularVelocity motor2Velocity = RotationsPerSecond.of(0); - AngularVelocity indexerForwardVelocity = RotationsPerSecond.of(42.53); //guestimate + AngularVelocity indexerForwardVelocity = RotationsPerSecond.of(0); double indexerOutput = 0; Current motor1Current = Amps.of(0); From d5f316b0c1e86d8ec7e22ea0d9560079dc48695b Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sun, 15 Mar 2026 11:17:55 -0600 Subject: [PATCH 17/33] Fix offset model --- .../java/frc4388/robot/RobotContainer.java | 27 ++++++++++--------- .../robot/constants/BuildConstants.java | 10 +++---- .../robot/subsystems/swerve/SwerveDrive.java | 22 ++++++++------- 3 files changed, 31 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e33d8bb..9136311 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -254,20 +254,21 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand(() -> m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldPositions.HUB_POSITION, - ShooterConstants.AIM_LEAD_TIME.get() - ), m_robotSwerveDrive) - // () -> { - // m_robotSwerveDrive.driveFacingVelocity( - // getDeadbandedDriverController().getLeft(), + .whileTrue(new RunCommand( + // () -> m_robotSwerveDrive.driveFacingPosition( + // getDeadbandedDriverController().getLeft(), // FieldPositions.HUB_POSITION, - // ShooterConstants.AIM_LEAD_TIME.get(), - // m_robotShooter.getBallVelocity(), - // m_robotShooter.getDistanceToHub() - // ); - // }, m_robotSwerveDrive) + // ShooterConstants.AIM_LEAD_TIME.get() + // ), m_robotSwerveDrive) + () -> { + m_robotSwerveDrive.driveFacingVelocity( + getDeadbandedDriverController().getLeft(), + FieldPositions.HUB_POSITION, + ShooterConstants.AIM_LEAD_TIME.get(), + m_robotShooter.getBallVelocity(), + m_robotShooter.getDistanceToHub() + ); + }, m_robotSwerveDrive) ); // D-PAD fine alignment diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 53210d2..ff87427 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 162; - public static final String GIT_SHA = "aac68dad5eabfaa1e098ad8cf6e42cd3bddd1ef3"; - public static final String GIT_DATE = "2026-03-14 20:20:55 MDT"; + public static final int GIT_REVISION = 164; + public static final String GIT_SHA = "f4a42f02eed0fa3b0950a05da55174b9f4b89ce7"; + public static final String GIT_DATE = "2026-03-14 20:39:45 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 20:33:21 MDT"; - public static final long BUILD_UNIX_TIME = 1773542001176L; + public static final String BUILD_DATE = "2026-03-15 11:14:11 MDT"; + public static final long BUILD_UNIX_TIME = 1773594851399L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index d3664f3..9d99131 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -409,13 +409,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable { chassisSpeeds.vyMetersPerSecond ); - if (ballVelocity > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){ + if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity)); - fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Aim Offset", aimOffset); - // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); - // Logger.recordOutput("Aim Offset", aimOffset); + + fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); + Logger.recordOutput("Offset", aimOffset); + } @@ -424,6 +424,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + Logger.recordOutput("Aim at Offset", fieldPos); driveFieldAngle(leftStick, ang); } @@ -531,6 +532,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable { vision.setLastOdomPose(state.currentPose); setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); + // if (state.speeds != null) { + // this.chassisSpeeds = state.speeds; + // } else { + // this.chassisSpeeds = new ChassisSpeeds(); + // } + if (vision.isTag()) { Pose2d pose = vision.getPose2d(); if (!robotKnowsWhereItIs) { @@ -545,11 +552,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { vision.setLastOdomPose(state.currentPose); setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); - if (state.speeds != null) { - this.chassisSpeeds = state.speeds; - } else { - this.chassisSpeeds = new ChassisSpeeds(); - } } // if(e.isPresent()) From 86b36a687e0338ed3ae5c6d035fb0f5b7404c27b Mon Sep 17 00:00:00 2001 From: mimigamin Date: Mon, 16 Mar 2026 16:13:53 -0600 Subject: [PATCH 18/33] Remove indexer velocity --- src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java | 1 - .../java/frc4388/robot/subsystems/shooter/ShooterReal.java | 3 --- 2 files changed, 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 4424848..444aeb7 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -26,7 +26,6 @@ public interface ShooterIO { AngularVelocity motor1Velocity = RotationsPerSecond.of(0); AngularVelocity motor2Velocity = RotationsPerSecond.of(0); - AngularVelocity indexerForwardVelocity = RotationsPerSecond.of(0); double indexerOutput = 0; Current motor1Current = Amps.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index f173553..6ca3ce4 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -87,9 +87,6 @@ public class ShooterReal implements ShooterIO { public void setIndexerOutput(ShooterState state, double percentOutput) { state.indexerTargetOutput = percentOutput; m_indexerMotor.set(percentOutput); - if (state.indexerTargetOutput - state.indexerOutput > 0.05){ - state.indexerForwardVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); - } } From ed7d0907b189f1b732408164152aca99f1ca5b81 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 17 Mar 2026 00:31:53 -0600 Subject: [PATCH 19/33] TImer timers for hubshift depending on if u win or lose auto dread --- src/main/java/frc4388/robot/Robot.java | 9 + .../robot/constants/BuildConstants.java | 12 +- .../robot/subsystems/shooter/Shooter.java | 3 + .../utility/compute/HubShiftTimer.java | 154 ++++++++++++++++++ 4 files changed, 172 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/utility/compute/HubShiftTimer.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index de52ec1..1b78419 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -20,6 +20,8 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.HubShiftTimer; +import frc4388.utility.compute.HubShiftTimer.ShiftInfo; import frc4388.utility.compute.RobotTime; import frc4388.utility.compute.Trim; import frc4388.utility.status.FaultReporter; @@ -74,6 +76,11 @@ public class Robot extends LoggedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + //Hub Shift logs + ShiftInfo info = HubShiftTimer.getShiftInfo(); + Logger.recordOutput("HubShift/IsActive", info.isActive()); + Logger.recordOutput("HubShift/RemainingInShift", info.remainingInShift()); + Logger.recordOutput("HubShift/Phase", info.phase().name()); } /** * This function is called once each time the robot enters Disabled mode. @@ -113,6 +120,7 @@ public class Robot extends LoggedRobot { m_autonomousCommand.schedule(); } m_robotTime.startMatchTime(); + HubShiftTimer.initializeAuto(); } /** @@ -138,6 +146,7 @@ public class Robot extends LoggedRobot { } m_robotTime.startMatchTime(); + HubShiftTimer.initializeTeleop(); } /** diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9ff0c32..92740de 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 147; - public static final String GIT_SHA = "b073fad27703951ba251eaffc56d86f791e531e6"; - public static final String GIT_DATE = "2026-03-07 20:08:15 MST"; - public static final String GIT_BRANCH = "PikesPeak"; - public static final String BUILD_DATE = "2026-03-08 14:27:19 MDT"; - public static final long BUILD_UNIX_TIME = 1773001639109L; + public static final int GIT_REVISION = 148; + public static final String GIT_SHA = "5b85bb4ace55dd116c3e80c496ad3ebf986ebfc8"; + public static final String GIT_DATE = "2026-03-09 10:27:13 MDT"; + public static final String GIT_BRANCH = "Timer"; + public static final String BUILD_DATE = "2026-03-17 00:27:11 MDT"; + public static final long BUILD_UNIX_TIME = 1773728831770L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 15f0edf..5a8f84d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -19,6 +19,8 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.FieldPositions; +import frc4388.utility.compute.HubShiftTimer; +import frc4388.utility.compute.HubShiftTimer.ShiftInfo; public class Shooter extends SubsystemBase { public ShooterIO io; @@ -104,6 +106,7 @@ public class Shooter extends SubsystemBase { // FaultReporter.register(this); // TODO Implement fault reporter Logger.processInputs("Shooter", state); + io.updateInputs(state); diff --git a/src/main/java/frc4388/utility/compute/HubShiftTimer.java b/src/main/java/frc4388/utility/compute/HubShiftTimer.java new file mode 100644 index 0000000..6f91284 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/HubShiftTimer.java @@ -0,0 +1,154 @@ + +package frc4388.utility.compute; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +public class HubShiftTimer { + + public enum ShiftPhase { + DISABLED, + AUTO, + TRANSITION, // 0 – 10 s + SHIFT1, // 10 – 35 s + SHIFT2, // 35 – 60 s + SHIFT3, // 60 – 85 s + SHIFT4, // 85 – 110 s + ENDGAME // 110 – 140 s + } + + public record ShiftInfo( + ShiftPhase phase, + double elapsedInShift, + double remainingInShift, + boolean isActive) {} +//total teleop time + public static final double TELEOP_DURATION = 140.0; +//total auto time + public static final double AUTO_DURATION = 20.0; + +//shift start and end times for calculations + private static final double[] SHIFT_STARTS = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + private static final double[] SHIFT_ENDS = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + +//hub active schedule, true is active and false is inactive +//starts always as active becasue transition is first and is active, but then is inactive for winner or active for loser + private static final boolean[] WINNER_SCHEDULE = {true, false, true, false, true, true}; + private static final boolean[] LOSER_SCHEDULE = {true, true, false, true, false, true}; + +//shift phase names during teleop + private static final ShiftPhase[] SHIFT_PHASES = { + ShiftPhase.TRANSITION, + ShiftPhase.SHIFT1, + ShiftPhase.SHIFT2, + ShiftPhase.SHIFT3, + ShiftPhase.SHIFT4, + ShiftPhase.ENDGAME + }; + +//timer to track time + private static final Timer teleopTimer = new Timer(); + private static double timerOffset = 0.0; + +//fms syncing idk other team did it too + private static final double RESYNC_THRESHOLD = 3.0; + + +//call at start of auto to start timer + public static void initializeAuto() { + teleopTimer.restart(); + } + +//call at start of teleop to start timer again because sometimes delay between auto and telop + public static void initializeTeleop() { + timerOffset = 0.0; + teleopTimer.restart(); + } + + + +//returns the updated shift info based on the winner of auto + public static ShiftInfo getShiftInfo() { + if (!DriverStation.isEnabled()) { + return new ShiftInfo(ShiftPhase.DISABLED, 0.0, 0.0, false); + } + if (DriverStation.isAutonomousEnabled()) { + double autoElapsed = teleopTimer.get(); // timer restarts in initialize() + return new ShiftInfo( + ShiftPhase.AUTO, + autoElapsed, + Math.max(0.0, AUTO_DURATION - teleopTimer.get()), + true); + } + return computeTeleopShift(); + } + +//find auto winner, R = red wins, B = blue wins + public static Alliance autoWinnerAlliance() { + String msg = DriverStation.getGameSpecificMessage(); + if (msg != null && msg.length() > 0) { + char c = msg.charAt(0); + if (c == 'R') return Alliance.Red; + if (c == 'B') return Alliance.Blue; + } + // backup if no msg, returns auto winner as opposite of our alliance. if we red -> blue wins auto + Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue); + return (ours == Alliance.Blue) ? Alliance.Red : Alliance.Blue; + } + + + //return our schedule for the shifts + private static boolean[] getSchedule() { + Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue); + Alliance winner = autoWinnerAlliance(); + return (ours == winner) ? WINNER_SCHEDULE : LOSER_SCHEDULE; + } + +//time since start of teleop + private static double getTeleopElapsed() { + double localTime = teleopTimer.get() - timerOffset; + + // Re-sync to FMS time if we drift too far (only when FMS is attached) + if (DriverStation.isFMSAttached()) { + double fmsElapsed = TELEOP_DURATION - DriverStation.getMatchTime(); + if (fmsElapsed <= TELEOP_DURATION - 5.0 // ignore the first few seconds of jitter + && Math.abs(localTime - fmsElapsed) >= RESYNC_THRESHOLD) { + timerOffset += localTime - fmsElapsed; + localTime = fmsElapsed; + } + } + return Math.max(0.0, Math.min(TELEOP_DURATION, localTime)); + } + + private static ShiftInfo computeTeleopShift() { + boolean[] schedule = getSchedule(); + double elapsed = getTeleopElapsed(); + + // Find which shift we're in + int phaseIndex = SHIFT_STARTS.length - 1; // default to last shift if past all bounds + for (int i = 0; i < SHIFT_STARTS.length; i++) { + if (elapsed >= SHIFT_STARTS[i] && elapsed < SHIFT_ENDS[i]) { + phaseIndex = i; + break; + } + } + + double shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex]; + double shiftRemaining = SHIFT_ENDS[phaseIndex] - elapsed; + + // merge time for elapsed if same active/inactive + if (phaseIndex > 0 && schedule[phaseIndex] == schedule[phaseIndex - 1]) { + shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex - 1]; + } + + // merge time for remaining time if same active/inactive status + if (phaseIndex < SHIFT_ENDS.length - 1 && schedule[phaseIndex] == schedule[phaseIndex + 1]) { + shiftRemaining = SHIFT_ENDS[phaseIndex + 1] - elapsed; + } + + return new ShiftInfo( + SHIFT_PHASES[phaseIndex], + shiftElapsed, + shiftRemaining, + schedule[phaseIndex]); + } +} \ No newline at end of file From 03d95a273de6aed5161050e3939fd8de52103543 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Wed, 18 Mar 2026 12:02:24 -0600 Subject: [PATCH 20/33] Fix to robot java --- simgui-ds.json | 5 ----- src/main/java/frc4388/robot/Robot.java | 5 ----- .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- .../java/frc4388/robot/subsystems/shooter/Shooter.java | 6 +++++- 4 files changed, 10 insertions(+), 16 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 4a63cc1..73cc713 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1b78419..c6bf398 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -76,11 +76,6 @@ public class Robot extends LoggedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - //Hub Shift logs - ShiftInfo info = HubShiftTimer.getShiftInfo(); - Logger.recordOutput("HubShift/IsActive", info.isActive()); - Logger.recordOutput("HubShift/RemainingInShift", info.remainingInShift()); - Logger.recordOutput("HubShift/Phase", info.phase().name()); } /** * This function is called once each time the robot enters Disabled mode. diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 92740de..ae43c17 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 148; - public static final String GIT_SHA = "5b85bb4ace55dd116c3e80c496ad3ebf986ebfc8"; - public static final String GIT_DATE = "2026-03-09 10:27:13 MDT"; + public static final int GIT_REVISION = 149; + public static final String GIT_SHA = "ed7d0907b189f1b732408164152aca99f1ca5b81"; + public static final String GIT_DATE = "2026-03-17 00:31:53 MDT"; public static final String GIT_BRANCH = "Timer"; - public static final String BUILD_DATE = "2026-03-17 00:27:11 MDT"; - public static final long BUILD_UNIX_TIME = 1773728831770L; + public static final String BUILD_DATE = "2026-03-17 12:46:28 MDT"; + public static final long BUILD_UNIX_TIME = 1773773188939L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 5a8f84d..75e6225 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -104,7 +104,11 @@ public class Shooter extends SubsystemBase { @Override public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter - + //Hub Shift logs + ShiftInfo info = HubShiftTimer.getShiftInfo(); + Logger.recordOutput("HubShift/IsActive", info.isActive()); + Logger.recordOutput("HubShift/RemainingInShift", info.remainingInShift()); + Logger.recordOutput("HubShift/Phase", info.phase().name()); Logger.processInputs("Shooter", state); io.updateInputs(state); From 85fe11c8bdd51d8671e98047f88a2d42276f7094 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Wed, 18 Mar 2026 13:39:27 -0600 Subject: [PATCH 21/33] Check gear ratio --- .../frc4388/robot/constants/BuildConstants.java | 10 +++++----- .../subsystems/shooter/ShooterConstants.java | 5 +++-- .../robot/subsystems/swerve/SwerveDrive.java | 15 ++++++++------- 3 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index ff87427..e8d2e92 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 164; - public static final String GIT_SHA = "f4a42f02eed0fa3b0950a05da55174b9f4b89ce7"; - public static final String GIT_DATE = "2026-03-14 20:39:45 MDT"; + public static final int GIT_REVISION = 166; + public static final String GIT_SHA = "86b36a687e0338ed3ae5c6d035fb0f5b7404c27b"; + public static final String GIT_DATE = "2026-03-16 16:13:53 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-15 11:14:11 MDT"; - public static final long BUILD_UNIX_TIME = 1773594851399L; + public static final String BUILD_DATE = "2026-03-18 13:27:40 MDT"; + public static final long BUILD_UNIX_TIME = 1773862060483L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index f074a74..d486de3 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -15,8 +15,8 @@ import frc4388.utility.status.CanDevice; public class ShooterConstants { // Motor conversions - public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; - public static final double INDEXER_GEAR_RATIO = 1.; + public static final double SHOOTERMOTOR_GEAR_RATIO = 1.29; // TODO: supposed to be 9 rotations in to 7 out + public static final double INDEXER_GEAR_RATIO = 1.72; // TODO: change it is supposed to be 18 to 31 public static final double T_CONSTANT = 2; public static final double SHOOTER_RADIUS = 2/39.37; public static final double INDEXER_RADIUS = 0.625/39.37; @@ -25,6 +25,7 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35); // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0); + public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15); // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.); // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 9d99131..c471924 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -411,10 +411,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity)); - // double aimOffset = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); + double aimOffsetComplext = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Offset", aimOffset); + Logger.recordOutput("Offset Simple", aimOffset); + Logger.recordOutput("Offset Complex", aimOffsetComplext); } @@ -532,11 +533,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { vision.setLastOdomPose(state.currentPose); setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate); - // if (state.speeds != null) { - // this.chassisSpeeds = state.speeds; - // } else { - // this.chassisSpeeds = new ChassisSpeeds(); - // } + if (state.speeds != null) { + this.chassisSpeeds = state.speeds; + } else { + this.chassisSpeeds = new ChassisSpeeds(); + } if (vision.isTag()) { Pose2d pose = vision.getPose2d(); From 73c3849569508a7c6cc2d2dea7a8e441f08fd134 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 19 Mar 2026 14:21:46 -0600 Subject: [PATCH 22/33] Update gear ratios --- .../java/frc4388/robot/RobotContainer.java | 34 +++++++----- .../robot/constants/BuildConstants.java | 10 ++-- .../frc4388/robot/constants/Constants.java | 5 ++ .../robot/subsystems/intake/Intake.java | 7 +++ .../robot/subsystems/shooter/Shooter.java | 31 +++++++---- .../subsystems/shooter/ShooterConstants.java | 15 +++--- .../robot/subsystems/shooter/ShooterIO.java | 3 ++ .../robot/subsystems/shooter/ShooterReal.java | 54 +++++++++++++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 5 +- .../swerve/SwerveDriveConstants.java | 2 + 10 files changed, 126 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9136311..a9a600b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -233,20 +233,20 @@ public class RobotContainer { //TEST - > Defense: X position on wheels and swerve drive pid on position new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .whileTrue(new RunCommand(() -> { - // m_robotSwerveDrive.defenseXPosition(); - // }, m_robotSwerveDrive)) - // .onFalse(new InstantCommand(() -> { - // m_robotSwerveDrive.stopDefenseXPosition(); - - .onTrue(new InstantCommand(() -> { - currentPose = m_robotSwerveDrive.getCurrentPose(); - })) - .whileTrue(new RunCommand(() -> { - m_stayInPosition.goToTargetPose(currentPose); + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.defenseXPosition(); }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.softStop(); + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.stopDefenseXPosition(); + + // .onTrue(new InstantCommand(() -> { + // currentPose = m_robotSwerveDrive.getCurrentPose(); + // })) + // .whileTrue(new RunCommand(() -> { + // m_stayInPosition.goToTargetPose(currentPose); + // }, m_robotSwerveDrive)) + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.softStop(); })); @@ -352,6 +352,14 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotShooter.indexerStalled(); + })) + .onFalse(new InstantCommand(() -> { + m_robotShooter.spinUpIdle(); + })); + } diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index e8d2e92..b8c41f5 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 166; - public static final String GIT_SHA = "86b36a687e0338ed3ae5c6d035fb0f5b7404c27b"; - public static final String GIT_DATE = "2026-03-16 16:13:53 MDT"; + public static final int GIT_REVISION = 167; + public static final String GIT_SHA = "85fe11c8bdd51d8671e98047f88a2d42276f7094"; + public static final String GIT_DATE = "2026-03-18 13:39:27 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-18 13:27:40 MDT"; - public static final long BUILD_UNIX_TIME = 1773862060483L; + public static final String BUILD_DATE = "2026-03-18 16:02:33 MDT"; + public static final long BUILD_UNIX_TIME = 1773871353359L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 5c2a083..3c54d38 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -117,6 +117,11 @@ public final class Constants { // Operator ready to shoot, but the driver conditions are bad public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE; + // LED color for when the indexer was stuck on ball and going in reverse + public static final LEDPatterns INDEXER_REVERSE = LEDPatterns.SOLID_VIOLET; + + // LED color for when the indexer, intake roller, or shooter is not going at right speed for more than 5 seconds and is likely stalled + public static final LEDPatterns MOTOR_STALLED = LEDPatterns.SOLID_RED; public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE; // public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE; diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 8425c54..d293bd8 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -45,6 +45,13 @@ public class Intake extends SubsystemBase { io.setRollerOutput(state, 0); } + public double getRollerTarget() { + return state.rollerTargetOutput; + } + + public double getRollerSpeed() { + return state.rollerOutput; + } // public enum FieldZone { // // The robot should aim at the hub diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 0319706..e0d32a8 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -59,7 +59,8 @@ public class Shooter extends SubsystemBase { public enum ShooterMode { Shooting, ManualShoot, - Idle + Idle, + IndexerStall } private ShooterMode mode = ShooterMode.Idle; @@ -83,6 +84,10 @@ public class Shooter extends SubsystemBase { public void allowShooting() { shooterButtonReady = true; + } + + public void indexerStalled() { + mode = ShooterMode.IndexerStall; } public void denyShooting() { @@ -169,7 +174,7 @@ public class Shooter extends SubsystemBase { switch (bitmask) { case 0b000: // No errors but button is not pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; @@ -180,19 +185,19 @@ public class Shooter extends SubsystemBase { case 0b010: // Bad shooter velocity, button is not pressed case 0b011: // Bad shooter velocty, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); break; case 0b100: // Driver error, button is not pressed case 0b101: // Driver error, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); break; case 0b110: // Driver error, bad shooter vel, button is not pressed case 0b111: // Driver error, bad shooter vel, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); break; } @@ -207,7 +212,7 @@ public class Shooter extends SubsystemBase { switch (bitmask2) { case 0b000: // No errors but button is not pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED); break; @@ -218,7 +223,7 @@ public class Shooter extends SubsystemBase { case 0b010: // Bad shooter velocity, button is not pressed case 0b011: // Bad shooter velocty, button is pressed - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); break; @@ -244,12 +249,16 @@ public class Shooter extends SubsystemBase { // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get()) ); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; - - + case IndexerStall: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get())); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); + m_robotLED.setMode(Constants.LEDConstants.INDEXER_REVERSE); + break; } - + + io.motorStalled(state, m_Intake, m_robotLED); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index d486de3..4ef195a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -15,7 +15,7 @@ import frc4388.utility.status.CanDevice; public class ShooterConstants { // Motor conversions - public static final double SHOOTERMOTOR_GEAR_RATIO = 1.29; // TODO: supposed to be 9 rotations in to 7 out + public static final double SHOOTERMOTOR_GEAR_RATIO = 1.286; // TODO: supposed to be 9 rotations in to 7 out -- 0.77 or 1.29 public static final double INDEXER_GEAR_RATIO = 1.72; // TODO: change it is supposed to be 18 to 31 public static final double T_CONSTANT = 2; public static final double SHOOTER_RADIUS = 2/39.37; @@ -31,8 +31,8 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); - public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0); - public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", -1.5); + public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.4); + public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); @@ -56,8 +56,9 @@ public class ShooterConstants { // 0.318464*hubDistMeters + // 30.6293; double speed = - 5.6939*hubDistMeters + - 22.76545 + MODEL_TRIM.get(); + 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + MODEL_TRIM.get(); // double speed = // 0.00610938*hubDistMeters*hubDistMeters @@ -81,8 +82,8 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.08) - .withKI(0.15) + .withKP(0.05) + .withKI(0.20) .withKD(0.0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 444aeb7..b30bd2d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -7,6 +7,8 @@ import org.littletonrobotics.junction.AutoLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; public interface ShooterIO { @AutoLog @@ -49,6 +51,7 @@ public interface ShooterIO { public default void updateInputs(ShooterState state) {} + public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {} public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 6ca3ce4..3f46ae0 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -8,6 +8,10 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.Timer; +import frc4388.robot.constants.Constants; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.led.LED; public class ShooterReal implements ShooterIO { @@ -19,6 +23,12 @@ public class ShooterReal implements ShooterIO { VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0); // VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0); + private final Timer m_stallTimerShooter = new Timer(); + private final Timer m_stallTimerIndexer = new Timer(); + private final Timer m_stallTimerRoller = new Timer(); + private boolean m_shooterStalling = false; + private boolean m_indexerStalling = false; + private boolean m_rollerStalling = false; public ShooterReal( TalonFX shooter1Motor, @@ -41,6 +51,50 @@ public class ShooterReal implements ShooterIO { // m_indexerVelocity.Slot = 0; } + @Override + public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { + + // if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 20) { + // if (!m_shooterStalling) { + // m_shooterStalling = true; + // m_stallTimerShooter.restart(); + // } + // if (m_stallTimerShooter.hasElapsed(5.0)) { + // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + // } + // } else { + // m_shooterStalling = false; + // m_stallTimerShooter.stop(); + // } + + // if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.2) { + // if (!m_indexerStalling) { + // m_indexerStalling = true; + // m_stallTimerIndexer.restart(); + // } + // if (m_stallTimerIndexer.hasElapsed(5.0)) { + // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + // } + // } else { + // m_indexerStalling = false; + // m_stallTimerIndexer.stop(); + // } + + // if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.2) { + // if (!m_rollerStalling) { + // m_rollerStalling = true; + // m_stallTimerRoller.restart(); + // } + // if (m_stallTimerRoller.hasElapsed(5.0)) { + // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + // } + // } else { + // m_rollerStalling = false; + // m_stallTimerRoller.stop(); + // } + + } + @Override public void setShooterVelocity(ShooterState state, AngularVelocity target) { state.motor1TargetVelocity = target; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index c471924..7e3ce17 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -410,12 +410,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable { ); if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ - double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(Math.abs(ballVelocity)); - double aimOffsetComplext = (chassisSpeeds.vyMetersPerSecond * distanceToHub) / (Math.abs(ballVelocity) * Math.cos(Math.atan((-1.4478 - (2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583) - Math.sqrt(Math.pow(2.8956 + 4 * (0.03724333 * distanceToHub + 0.64797583), 2) - 8.3863)) / 2) / distanceToHub))); - + double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); Logger.recordOutput("Offset Simple", aimOffset); - Logger.recordOutput("Offset Complex", aimOffsetComplext); } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 0034c0d..5bd6369 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -73,6 +73,8 @@ public final class SwerveDriveConstants { public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; + public static ConfigurableDouble distanceTolerence = new ConfigurableDouble("Distance Tolerence", 0.5); + // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); public static final class ModuleSpecificConstants { //2026 From d1055a4f27a536e5588b29a826839fc970668fc1 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 19 Mar 2026 15:28:01 -0600 Subject: [PATCH 23/33] Implement shooting while moving vertically --- elastic-layout.json | 160 +++++++++++------- .../java/frc4388/robot/RobotContainer.java | 6 +- .../robot/subsystems/shooter/Shooter.java | 9 +- .../subsystems/shooter/ShooterConstants.java | 26 ++- .../robot/subsystems/swerve/SwerveDrive.java | 10 +- 5 files changed, 141 insertions(+), 70 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 53d53bb..82cf1df 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -8,10 +8,10 @@ "layouts": [ { "title": "Tag Processed", - "x": 384.0, + "x": 896.0, "y": 0.0, "width": 256.0, - "height": 256.0, + "height": 384.0, "type": "List Layout", "properties": { "label_position": "TOP" @@ -55,27 +55,11 @@ } ], "containers": [ - { - "title": "MatchTime", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Match Time", - "properties": { - "topic": "/AdvantageKit/DriverStation/MatchTime", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - }, { "title": "RetractedLimit", - "x": 0.0, - "y": 384.0, - "width": 128.0, + "x": 512.0, + "y": 0.0, + "width": 256.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -90,9 +74,9 @@ }, { "title": "Auto Chooser", - "x": 1024.0, - "y": 256.0, - "width": 384.0, + "x": 896.0, + "y": 384.0, + "width": 256.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { @@ -103,9 +87,9 @@ }, { "title": "Roller Percent Output", - "x": 0.0, + "x": 512.0, "y": 256.0, - "width": 256.0, + "width": 384.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -120,8 +104,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 128.0, - "y": 384.0, + "x": 768.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -134,8 +118,8 @@ }, { "title": "TRIM SHOOTER SPEED", - "x": 256.0, - "y": 256.0, + "x": 512.0, + "y": 384.0, "width": 384.0, "height": 128.0, "type": "Number Slider", @@ -151,8 +135,8 @@ }, { "title": "Mode", - "x": 256.0, - "y": 384.0, + "x": 512.0, + "y": 128.0, "width": 384.0, "height": 128.0, "type": "Text Display", @@ -164,30 +148,49 @@ } }, { - "title": "Time to rev", - "x": 768.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Large Text Display", + "title": "IsActive", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Boolean Box", "properties": { - "topic": "/AdvantageKit/RealOutputs/Time to rev", + "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive", "period": 0.06, - "data_type": "double" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Arm angle extended", - "x": 1152.0, - "y": 128.0, - "width": 256.0, + "title": "RemainingInShift", + "x": 0.0, + "y": 256.0, + "width": 384.0, "height": 128.0, - "type": "Text Display", + "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Arm angle extended", + "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift", "period": 0.06, "data_type": "double", - "show_submit_button": true + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Phase", + "x": 0.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/HubShift/Phase", + "period": 0.06, + "data_type": "string" } } ] @@ -527,8 +530,8 @@ }, { "title": "Shooter Idle max current", - "x": 384.0, - "y": 512.0, + "x": 1024.0, + "y": 384.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -555,8 +558,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 640.0, - "y": 512.0, + "x": 1024.0, + "y": 256.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -576,16 +579,59 @@ "layouts": [], "containers": [ { - "title": "Auto Chooser", + "title": "Hub Dist", + "x": 640.0, + "y": 128.0, + "width": 256.0, + "height": 256.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/Hub Dist", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Distance Tolerence", + "x": 256.0, + "y": 128.0, + "width": 256.0, + "height": 256.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Distance Tolerence", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Chassis positive speed offset", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Chassis positive speed offset", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Chassis negative speed offset", "x": 0.0, "y": 0.0, - "width": 640.0, - "height": 768.0, - "type": "ComboBox Chooser", + "width": 256.0, + "height": 256.0, + "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Auto Chooser", + "topic": "/SmartDashboard/Chassis negative speed offset", "period": 0.06, - "sort_options": false + "data_type": "double", + "show_submit_button": true } } ] diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a9a600b..dd82aa7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { // ); private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), + new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter), IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); @@ -315,7 +315,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.rollerStop(); - m_robotShooter.spinUpShooting(); + m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); @@ -352,7 +352,7 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> { m_robotShooter.indexerStalled(); })) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index e0d32a8..eebc943 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -33,6 +33,8 @@ public class Shooter extends SubsystemBase { public boolean badShooterVelocity; public double distanceToHub = 5; + public double chassisXSpeed = 0; + public Shooter( ShooterIO io, @@ -66,7 +68,8 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void spinUpShooting() { + public void spinUpShooting(double chassisXSpeed) { + this.chassisXSpeed = chassisXSpeed; this.mode = ShooterMode.Shooting; } @@ -157,14 +160,14 @@ public class Shooter extends SubsystemBase { //revtime calculations // double shooterAcceleration = - double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond); + double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub,chassisXSpeed).in(RotationsPerSecond); double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT)); // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend)); Logger.recordOutput("Time to rev", revTime); switch (mode) { case Shooting: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed)); int bitmask = ( (shooterButtonReady ? 1 : 0) + diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 4ef195a..c84e5fc 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -36,7 +36,8 @@ public class ShooterConstants { public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); - + public static final ConfigurableDouble CHASSIS_POS_OFFSET = new ConfigurableDouble("Chassis positive speed offset", 1.0); + public static final ConfigurableDouble CHASSIS_NEG_OFFSET = new ConfigurableDouble("Chassis negative speed offset", 1.0); // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); @@ -49,16 +50,29 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); // - public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) { // Model derived from points // double speed = // 1.11576*hubDistMeters*hubDistMeters + // 0.318464*hubDistMeters + // 30.6293; - double speed = + double speed; + if (Math.abs(chassisXSpeed) < 0.1) { + speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + 30.35696 + MODEL_TRIM.get(); + } else if (chassisXSpeed > 0) { // positive = further from hub + speed = + 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + (chassisXSpeed * CHASSIS_POS_OFFSET.get()) + MODEL_TRIM.get(); + } else { // negative = moving towards hub + speed = + 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + (chassisXSpeed * CHASSIS_NEG_OFFSET.get()) + MODEL_TRIM.get(); + } // double speed = // 0.00610938*hubDistMeters*hubDistMeters @@ -82,8 +96,8 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.05) - .withKI(0.20) + .withKP(0.02) + .withKI(0.15) .withKD(0.0); @@ -91,7 +105,7 @@ public class ShooterConstants { - public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 7e3ce17..06f9c06 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -412,7 +412,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Offset Simple", aimOffset); + Logger.recordOutput("Offset Value", aimOffset); } @@ -512,6 +512,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { ); // stop the modules without breaking } + public double chassisXSpeeds(){ + if (TimesNegativeOne.isRed) { + return chassisSpeeds.vxMetersPerSecond; + } else { + return -chassisSpeeds.vxMetersPerSecond; + } + } + public void stopModules() { // stopped = true; // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); From 917aaa774650423095bb33033fe57b1eb1c12a1a Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 19 Mar 2026 15:46:28 -0600 Subject: [PATCH 24/33] Revert "Implement shooting while moving vertically" This reverts commit d1055a4f27a536e5588b29a826839fc970668fc1. --- elastic-layout.json | 170 +++++++----------- .../java/frc4388/robot/RobotContainer.java | 6 +- .../robot/subsystems/shooter/Shooter.java | 9 +- .../subsystems/shooter/ShooterConstants.java | 26 +-- .../robot/subsystems/swerve/SwerveDrive.java | 10 +- 5 files changed, 75 insertions(+), 146 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 82cf1df..53d53bb 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -8,10 +8,10 @@ "layouts": [ { "title": "Tag Processed", - "x": 896.0, + "x": 384.0, "y": 0.0, "width": 256.0, - "height": 384.0, + "height": 256.0, "type": "List Layout", "properties": { "label_position": "TOP" @@ -56,10 +56,26 @@ ], "containers": [ { - "title": "RetractedLimit", - "x": 512.0, + "title": "MatchTime", + "x": 0.0, "y": 0.0, - "width": 256.0, + "width": 384.0, + "height": 256.0, + "type": "Match Time", + "properties": { + "topic": "/AdvantageKit/DriverStation/MatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "RetractedLimit", + "x": 0.0, + "y": 384.0, + "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -74,9 +90,9 @@ }, { "title": "Auto Chooser", - "x": 896.0, - "y": 384.0, - "width": 256.0, + "x": 1024.0, + "y": 256.0, + "width": 384.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { @@ -87,9 +103,9 @@ }, { "title": "Roller Percent Output", - "x": 512.0, + "x": 0.0, "y": 256.0, - "width": 384.0, + "width": 256.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -104,8 +120,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 768.0, - "y": 0.0, + "x": 128.0, + "y": 384.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -118,8 +134,8 @@ }, { "title": "TRIM SHOOTER SPEED", - "x": 512.0, - "y": 384.0, + "x": 256.0, + "y": 256.0, "width": 384.0, "height": 128.0, "type": "Number Slider", @@ -135,8 +151,8 @@ }, { "title": "Mode", - "x": 512.0, - "y": 128.0, + "x": 256.0, + "y": 384.0, "width": 384.0, "height": 128.0, "type": "Text Display", @@ -148,49 +164,30 @@ } }, { - "title": "IsActive", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "RemainingInShift", - "x": 0.0, - "y": 256.0, - "width": 384.0, - "height": 128.0, - "type": "Match Time", - "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - }, - { - "title": "Phase", - "x": 0.0, - "y": 384.0, - "width": 384.0, + "title": "Time to rev", + "x": 768.0, + "y": 128.0, + "width": 256.0, "height": 128.0, "type": "Large Text Display", "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/Phase", + "topic": "/AdvantageKit/RealOutputs/Time to rev", "period": 0.06, - "data_type": "string" + "data_type": "double" + } + }, + { + "title": "Arm angle extended", + "x": 1152.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Arm angle extended", + "period": 0.06, + "data_type": "double", + "show_submit_button": true } } ] @@ -530,8 +527,8 @@ }, { "title": "Shooter Idle max current", - "x": 1024.0, - "y": 384.0, + "x": 384.0, + "y": 512.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -558,8 +555,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 1024.0, - "y": 256.0, + "x": 640.0, + "y": 512.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -579,59 +576,16 @@ "layouts": [], "containers": [ { - "title": "Hub Dist", - "x": 640.0, - "y": 128.0, - "width": 256.0, - "height": 256.0, - "type": "Text Display", - "properties": { - "topic": "/AdvantageKit/RealOutputs/Hub Dist", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Distance Tolerence", - "x": 256.0, - "y": 128.0, - "width": 256.0, - "height": 256.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Distance Tolerence", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Chassis positive speed offset", - "x": 0.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Chassis positive speed offset", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Chassis negative speed offset", + "title": "Auto Chooser", "x": 0.0, "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Text Display", + "width": 640.0, + "height": 768.0, + "type": "ComboBox Chooser", "properties": { - "topic": "/SmartDashboard/Chassis negative speed offset", + "topic": "/SmartDashboard/Auto Chooser", "period": 0.06, - "data_type": "double", - "show_submit_button": true + "sort_options": false } } ] diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd82aa7..a9a600b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { // ); private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter), + new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); @@ -315,7 +315,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.rollerStop(); - m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()); + m_robotShooter.spinUpShooting(); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); @@ -352,7 +352,7 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> { m_robotShooter.indexerStalled(); })) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index eebc943..e0d32a8 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -33,8 +33,6 @@ public class Shooter extends SubsystemBase { public boolean badShooterVelocity; public double distanceToHub = 5; - public double chassisXSpeed = 0; - public Shooter( ShooterIO io, @@ -68,8 +66,7 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void spinUpShooting(double chassisXSpeed) { - this.chassisXSpeed = chassisXSpeed; + public void spinUpShooting() { this.mode = ShooterMode.Shooting; } @@ -160,14 +157,14 @@ public class Shooter extends SubsystemBase { //revtime calculations // double shooterAcceleration = - double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub,chassisXSpeed).in(RotationsPerSecond); + double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond); double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT)); // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend)); Logger.recordOutput("Time to rev", revTime); switch (mode) { case Shooting: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed)); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); int bitmask = ( (shooterButtonReady ? 1 : 0) + diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index c84e5fc..4ef195a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -36,8 +36,7 @@ public class ShooterConstants { public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); - public static final ConfigurableDouble CHASSIS_POS_OFFSET = new ConfigurableDouble("Chassis positive speed offset", 1.0); - public static final ConfigurableDouble CHASSIS_NEG_OFFSET = new ConfigurableDouble("Chassis negative speed offset", 1.0); + // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); @@ -50,29 +49,16 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); // - public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) { + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { // Model derived from points // double speed = // 1.11576*hubDistMeters*hubDistMeters + // 0.318464*hubDistMeters + // 30.6293; - double speed; - if (Math.abs(chassisXSpeed) < 0.1) { - speed = + double speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + 30.35696 + MODEL_TRIM.get(); - } else if (chassisXSpeed > 0) { // positive = further from hub - speed = - 0.0593402*hubDistMeters*hubDistMeters + - 4.90561*hubDistMeters + - 30.35696 + (chassisXSpeed * CHASSIS_POS_OFFSET.get()) + MODEL_TRIM.get(); - } else { // negative = moving towards hub - speed = - 0.0593402*hubDistMeters*hubDistMeters + - 4.90561*hubDistMeters + - 30.35696 + (chassisXSpeed * CHASSIS_NEG_OFFSET.get()) + MODEL_TRIM.get(); - } // double speed = // 0.00610938*hubDistMeters*hubDistMeters @@ -96,8 +82,8 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.02) - .withKI(0.15) + .withKP(0.05) + .withKI(0.20) .withKD(0.0); @@ -105,7 +91,7 @@ public class ShooterConstants { - public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02); + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 06f9c06..7e3ce17 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -412,7 +412,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Offset Value", aimOffset); + Logger.recordOutput("Offset Simple", aimOffset); } @@ -512,14 +512,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { ); // stop the modules without breaking } - public double chassisXSpeeds(){ - if (TimesNegativeOne.isRed) { - return chassisSpeeds.vxMetersPerSecond; - } else { - return -chassisSpeeds.vxMetersPerSecond; - } - } - public void stopModules() { // stopped = true; // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); From 529691a7a6e87520f57ae76a2eb1eb517cb5da80 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Thu, 19 Mar 2026 16:33:14 -0600 Subject: [PATCH 25/33] rumble --- src/main/java/frc4388/robot/Robot.java | 8 +++++++- .../java/frc4388/robot/constants/BuildConstants.java | 12 ++++++------ .../robot/subsystems/shooter/ShooterConstants.java | 2 +- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index c6bf398..cc9296f 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,6 +15,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; @@ -149,7 +150,12 @@ public class Robot extends LoggedRobot { */ @Override public void teleopPeriodic() { - // m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft()); + var info = HubShiftTimer.getShiftInfo(); + + double rumble = (info.remainingInShift() < 5.) ? 1 : 0; + + m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble); + m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble); } /** diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index ae43c17..2ac3242 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 149; - public static final String GIT_SHA = "ed7d0907b189f1b732408164152aca99f1ca5b81"; - public static final String GIT_DATE = "2026-03-17 00:31:53 MDT"; - public static final String GIT_BRANCH = "Timer"; - public static final String BUILD_DATE = "2026-03-17 12:46:28 MDT"; - public static final long BUILD_UNIX_TIME = 1773773188939L; + public static final int GIT_REVISION = 171; + public static final String GIT_SHA = "08f1e26f3a6a9eefad99783748191b0d98056b07"; + public static final String GIT_DATE = "2026-03-19 16:06:31 MDT"; + public static final String GIT_BRANCH = "gurt"; + public static final String BUILD_DATE = "2026-03-19 16:29:01 MDT"; + public static final long BUILD_UNIX_TIME = 1773959341836L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 4ef195a..4e90ce6 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -35,7 +35,7 @@ public class ShooterConstants { public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); - public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.); // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); From 4ffbe3f5954581f07ac26ee6174113c909cdbaa7 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 19 Mar 2026 18:32:02 -0600 Subject: [PATCH 26/33] add testing changes --- .../java/frc4388/robot/RobotContainer.java | 44 +++++----- .../robot/constants/BuildConstants.java | 10 +-- .../robot/subsystems/shooter/Shooter.java | 5 +- .../subsystems/shooter/ShooterConstants.java | 8 +- .../robot/subsystems/shooter/ShooterIO.java | 2 +- .../robot/subsystems/shooter/ShooterReal.java | 85 ++++++++++--------- .../robot/subsystems/swerve/SwerveDrive.java | 10 ++- 7 files changed, 90 insertions(+), 74 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a9a600b..97c86ba 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { // ); private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), + new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter), IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); @@ -231,22 +231,25 @@ public class RobotContainer { m_robotSwerveDrive.shiftDownRot(); })); - //TEST - > Defense: X position on wheels and swerve drive pid on position + //TEST - > X positino on wheels new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .whileTrue(new RunCommand(() -> { m_robotSwerveDrive.defenseXPosition(); }, m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> { m_robotSwerveDrive.stopDefenseXPosition(); + })); - // .onTrue(new InstantCommand(() -> { - // currentPose = m_robotSwerveDrive.getCurrentPose(); - // })) - // .whileTrue(new RunCommand(() -> { - // m_stayInPosition.goToTargetPose(currentPose); - // }, m_robotSwerveDrive)) - // .onFalse(new InstantCommand(() -> { - // m_robotSwerveDrive.softStop(); + //TEST - > PID positinon + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> { + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); })); @@ -254,19 +257,14 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - // () -> m_robotSwerveDrive.driveFacingPosition( - // getDeadbandedDriverController().getLeft(), - // FieldPositions.HUB_POSITION, - // ShooterConstants.AIM_LEAD_TIME.get() - // ), m_robotSwerveDrive) - () -> { - m_robotSwerveDrive.driveFacingVelocity( + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + })) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.driveFacingPosition( getDeadbandedDriverController().getLeft(), FieldPositions.HUB_POSITION, - ShooterConstants.AIM_LEAD_TIME.get(), - m_robotShooter.getBallVelocity(), - m_robotShooter.getDistanceToHub() + ShooterConstants.AIM_LEAD_TIME.get() ); }, m_robotSwerveDrive) ); @@ -315,7 +313,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.rollerStop(); - m_robotShooter.spinUpShooting(); + m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); @@ -352,7 +350,7 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> { m_robotShooter.indexerStalled(); })) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index b8c41f5..0eed6eb 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 167; - public static final String GIT_SHA = "85fe11c8bdd51d8671e98047f88a2d42276f7094"; - public static final String GIT_DATE = "2026-03-18 13:39:27 MDT"; + public static final int GIT_REVISION = 170; + public static final String GIT_SHA = "917aaa774650423095bb33033fe57b1eb1c12a1a"; + public static final String GIT_DATE = "2026-03-19 15:46:28 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-18 16:02:33 MDT"; - public static final long BUILD_UNIX_TIME = 1773871353359L; + public static final String BUILD_DATE = "2026-03-19 18:16:37 MDT"; + public static final long BUILD_UNIX_TIME = 1773965797063L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index e0d32a8..dd1b78a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -32,7 +32,7 @@ public class Shooter extends SubsystemBase { // Supplier m_swervePoseSupplier; public boolean badShooterVelocity; public double distanceToHub = 5; - + public double chassisXSpeed = 0; public Shooter( ShooterIO io, @@ -66,7 +66,8 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void spinUpShooting() { + public void spinUpShooting(double chassisXSpeed) { + this.chassisXSpeed = chassisXSpeed; this.mode = ShooterMode.Shooting; } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 4ef195a..01be9f6 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -35,7 +35,7 @@ public class ShooterConstants { public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); - public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1); // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); @@ -82,8 +82,8 @@ public class ShooterConstants { // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) - .withKP(0.05) - .withKI(0.20) + .withKP(0.02) + .withKI(0.15) .withKD(0.0); @@ -91,7 +91,7 @@ public class ShooterConstants { - public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08); + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index b30bd2d..96cbd19 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -21,7 +21,7 @@ public interface ShooterIO { // Angle shooterTargetPitch = Rotations.of(0); // Current pitchMotorCurrent = Amps.of(0); - AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(40); // For modeling purposes + AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); double indexerTargetOutput = 0; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 3f46ae0..8b7c445 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.RotationsPerSecond; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; @@ -29,6 +30,8 @@ public class ShooterReal implements ShooterIO { private boolean m_shooterStalling = false; private boolean m_indexerStalling = false; private boolean m_rollerStalling = false; + public String motorStall = ""; + public ShooterReal( TalonFX shooter1Motor, @@ -52,46 +55,52 @@ public class ShooterReal implements ShooterIO { } @Override - public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { - - // if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 20) { - // if (!m_shooterStalling) { - // m_shooterStalling = true; - // m_stallTimerShooter.restart(); - // } - // if (m_stallTimerShooter.hasElapsed(5.0)) { - // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - // } - // } else { - // m_shooterStalling = false; - // m_stallTimerShooter.stop(); - // } + public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { + if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) { + if (!m_shooterStalling) { + m_shooterStalling = true; + m_stallTimerShooter.restart(); + } + if (m_stallTimerShooter.hasElapsed(5.0)) { + m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + motorStall = "Shooter Stalled"; + System.out.println(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond))); + } + } else { + m_shooterStalling = false; + m_stallTimerShooter.reset(); + } - // if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.2) { - // if (!m_indexerStalling) { - // m_indexerStalling = true; - // m_stallTimerIndexer.restart(); - // } - // if (m_stallTimerIndexer.hasElapsed(5.0)) { - // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - // } - // } else { - // m_indexerStalling = false; - // m_stallTimerIndexer.stop(); - // } + if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) { + if (!m_indexerStalling) { + m_indexerStalling = true; + m_stallTimerIndexer.restart(); + } + if (m_stallTimerIndexer.hasElapsed(5.0)) { + m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + motorStall = "Indexer Stalled"; + System.out.println(Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput)); + } + } else { + m_indexerStalling = false; + m_stallTimerIndexer.reset(); + } - // if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.2) { - // if (!m_rollerStalling) { - // m_rollerStalling = true; - // m_stallTimerRoller.restart(); - // } - // if (m_stallTimerRoller.hasElapsed(5.0)) { - // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - // } - // } else { - // m_rollerStalling = false; - // m_stallTimerRoller.stop(); - // } + if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.4) { + if (!m_rollerStalling) { + m_rollerStalling = true; + m_stallTimerRoller.restart(); + } + if (m_stallTimerRoller.hasElapsed(5.0)) { + m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + motorStall = "Roller Stalled"; + System.out.println(Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed())); + } + } else { + m_rollerStalling = false; + m_stallTimerRoller.reset(); + } + Logger.recordOutput("Stalled Motor: ", motorStall); } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 7e3ce17..4ebbf14 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -166,6 +166,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // module.setDesiredState(state); // } + public double chassisXSpeeds(){ + if (TimesNegativeOne.isRed) { + return chassisSpeeds.vxMetersPerSecond; + } else { + return -chassisSpeeds.vxMetersPerSecond; + } + } + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve @@ -412,7 +420,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Offset Simple", aimOffset); + Logger.recordOutput("Offset Value", aimOffset); } From 69677d5ae9e63125a30857f5f7afa0756dfa9416 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Fri, 20 Mar 2026 15:40:07 -0600 Subject: [PATCH 27/33] Lots - shooting while moving corrected (vertical movements) - reverse indexer before shooting - defensive positions fixed values - stalling motors led fixed (except shooter) --- elastic-layout.json | 141 +++++++++++------- .../java/frc4388/robot/RobotContainer.java | 121 ++++++++++++++- .../robot/commands/Swerve/StayInPosition.java | 4 +- .../robot/constants/BuildConstants.java | 10 +- .../robot/subsystems/shooter/Shooter.java | 51 +++---- .../subsystems/shooter/ShooterConstants.java | 36 +++-- .../robot/subsystems/shooter/ShooterReal.java | 37 ++--- 7 files changed, 268 insertions(+), 132 deletions(-) diff --git a/elastic-layout.json b/elastic-layout.json index 53d53bb..8a56808 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -8,10 +8,10 @@ "layouts": [ { "title": "Tag Processed", - "x": 384.0, + "x": 896.0, "y": 0.0, "width": 256.0, - "height": 256.0, + "height": 384.0, "type": "List Layout", "properties": { "label_position": "TOP" @@ -55,27 +55,11 @@ } ], "containers": [ - { - "title": "MatchTime", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Match Time", - "properties": { - "topic": "/AdvantageKit/DriverStation/MatchTime", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - }, { "title": "RetractedLimit", - "x": 0.0, - "y": 384.0, - "width": 128.0, + "x": 512.0, + "y": 0.0, + "width": 256.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -90,9 +74,9 @@ }, { "title": "Auto Chooser", - "x": 1024.0, - "y": 256.0, - "width": 384.0, + "x": 896.0, + "y": 384.0, + "width": 256.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { @@ -103,9 +87,9 @@ }, { "title": "Roller Percent Output", - "x": 0.0, + "x": 512.0, "y": 256.0, - "width": 256.0, + "width": 384.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -120,8 +104,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 128.0, - "y": 384.0, + "x": 768.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -134,8 +118,8 @@ }, { "title": "TRIM SHOOTER SPEED", - "x": 256.0, - "y": 256.0, + "x": 512.0, + "y": 384.0, "width": 384.0, "height": 128.0, "type": "Number Slider", @@ -151,8 +135,8 @@ }, { "title": "Mode", - "x": 256.0, - "y": 384.0, + "x": 512.0, + "y": 128.0, "width": 384.0, "height": 128.0, "type": "Text Display", @@ -164,30 +148,49 @@ } }, { - "title": "Time to rev", - "x": 768.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Large Text Display", + "title": "IsActive", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Boolean Box", "properties": { - "topic": "/AdvantageKit/RealOutputs/Time to rev", + "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive", "period": 0.06, - "data_type": "double" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Arm angle extended", - "x": 1152.0, - "y": 128.0, - "width": 256.0, + "title": "RemainingInShift", + "x": 0.0, + "y": 256.0, + "width": 384.0, "height": 128.0, - "type": "Text Display", + "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Arm angle extended", + "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift", "period": 0.06, "data_type": "double", - "show_submit_button": true + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Phase", + "x": 0.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/HubShift/Phase", + "period": 0.06, + "data_type": "string" } } ] @@ -527,8 +530,8 @@ }, { "title": "Shooter Idle max current", - "x": 384.0, - "y": 512.0, + "x": 1024.0, + "y": 384.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -555,8 +558,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 640.0, - "y": 512.0, + "x": 1024.0, + "y": 256.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -575,12 +578,40 @@ "grid_layout": { "layouts": [], "containers": [ + { + "title": "Stalled Motor: ", + "x": 512.0, + "y": 128.0, + "width": 384.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/Stalled Motor: ", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Shooter idle % output", + "x": 128.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Shooter idle % output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, { "title": "Auto Chooser", - "x": 0.0, - "y": 0.0, - "width": 640.0, - "height": 768.0, + "x": 512.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, "type": "ComboBox Chooser", "properties": { "topic": "/SmartDashboard/Auto Chooser", diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97c86ba..53e4df3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { // ); private Command RobotRev = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter), + new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter), IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); @@ -149,7 +149,7 @@ public class RobotContainer { public RobotContainer() { - configureButtonBindings(); + configureSINGLEBindings(); // Called on first robot enable DeferredBlock.addBlock(() -> { @@ -313,7 +313,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); m_robotIntake.rollerStop(); - m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()); + m_robotShooter.spinUpShooting(); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); @@ -350,16 +350,125 @@ public class RobotContainer { // m_robotClimber.toggleDeployed(); // })); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + + } + + + private void configureSINGLEBindings() { + + //Driver controls + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + + + + // TEST-> the driver is holding the left trigger, drive slow and rotation up + // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + // .onTrue(new InstantCommand(() -> { + // m_robotSwerveDrive.setToSlow(); + // m_robotSwerveDrive.shiftUpRot(); + // })) + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.setToFast(); + // m_robotSwerveDrive.shiftDownRot(); + // })); + + //TEST - > X positino on wheels + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.defenseXPosition(); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.stopDefenseXPosition(); + })); + + //TEST - > PID positinon + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> { - m_robotShooter.indexerStalled(); + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); + + })); + + + + // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotSwerveDrive.setToSlow(); + })) + .whileTrue(new RunCommand(() -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldPositions.HUB_POSITION, + ShooterConstants.AIM_LEAD_TIME.get() + ); + }, m_robotSwerveDrive) + ); + + // D-PAD fine alignment + new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) + .whileTrue(new RunCommand( + () -> m_robotSwerveDrive.driveFine( + new Translation2d( + 1, + Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) + ), + getDeadbandedDriverController().getRight(), 0.15 + ), m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + + + //allow shooting with right trigger + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotShooter.allowShooting(); + })).onFalse(new InstantCommand(() -> { + m_robotShooter.denyShooting(); + })); + + + + //set shooter ready (rev) with left trigger hold + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + m_robotIntake.rollerStop(); + m_robotShooter.spinUpShooting(); })) .onFalse(new InstantCommand(() -> { m_robotShooter.spinUpIdle(); })); + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Extending); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + })); - } + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracting); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Idle); + })); + + } //.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java index afbfaa3..7b826a7 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -34,7 +34,7 @@ public class StayInPosition extends PID { //Same here translationY = 0.2; } - if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) { + if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) { driveTranslation = new Translation2d(); } else { //TODO: Tweak for best corrector against another bot @@ -42,8 +42,6 @@ public class StayInPosition extends PID { } drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); - // If above doesn't work - //drive.driveFieldAngle(driveTranslation, targetPose.getRotation()); } @Override diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 0eed6eb..9f111da 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 170; - public static final String GIT_SHA = "917aaa774650423095bb33033fe57b1eb1c12a1a"; - public static final String GIT_DATE = "2026-03-19 15:46:28 MDT"; + public static final int GIT_REVISION = 171; + public static final String GIT_SHA = "4ffbe3f5954581f07ac26ee6174113c909cdbaa7"; + public static final String GIT_DATE = "2026-03-19 18:32:02 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-19 18:16:37 MDT"; - public static final long BUILD_UNIX_TIME = 1773965797063L; + public static final String BUILD_DATE = "2026-03-20 15:31:47 MDT"; + public static final long BUILD_UNIX_TIME = 1774042307703L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index dd1b78a..52fbcd3 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -13,12 +13,14 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.utility.compute.FieldPositions; +import frc4388.utility.compute.TimesNegativeOne; public class Shooter extends SubsystemBase { public ShooterIO io; @@ -34,6 +36,8 @@ public class Shooter extends SubsystemBase { public double distanceToHub = 5; public double chassisXSpeed = 0; + + public Shooter( ShooterIO io, SwerveDrive swerveDrive, @@ -59,15 +63,13 @@ public class Shooter extends SubsystemBase { public enum ShooterMode { Shooting, ManualShoot, - Idle, - IndexerStall + Idle } private ShooterMode mode = ShooterMode.Idle; private boolean shooterButtonReady = false; - public void spinUpShooting(double chassisXSpeed) { - this.chassisXSpeed = chassisXSpeed; + public void spinUpShooting() { this.mode = ShooterMode.Shooting; } @@ -87,10 +89,6 @@ public class Shooter extends SubsystemBase { shooterButtonReady = true; } - public void indexerStalled() { - mode = ShooterMode.IndexerStall; - } - public void denyShooting() { shooterButtonReady = false; } @@ -122,26 +120,20 @@ public class Shooter extends SubsystemBase { // Get robot positon and speeds ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; - double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2)); - double AngSpeed = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI)); Pose2d robotPose2d = m_SwerveDrive.getPose2d(); + if (TimesNegativeOne.isRed) { + chassisXSpeed = chassisSpeeds.vxMetersPerSecond; + } else { + chassisXSpeed = -chassisSpeeds.vxMetersPerSecond; + } - // Calculate aim lead - // Get the current speed of the robot - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - - // Calculate a point to aim ahead of the actual position. - Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation()); - + Translation2d fieldPos = robotPose2d.getTranslation(); // Get the robot's aim distance to hub - distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); + distanceToHub = (fieldPos.minus(FieldPositions.HUB_POSITION).getNorm()); - //Center of hub to cameras in inches + //Center of hub to cameras in meters Logger.recordOutput("Hub Dist", distanceToHub); boolean driverError = @@ -158,14 +150,14 @@ public class Shooter extends SubsystemBase { //revtime calculations // double shooterAcceleration = - double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond); + double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond); double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT)); // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend)); Logger.recordOutput("Time to rev", revTime); switch (mode) { case Shooting: - io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed)); int bitmask = ( (shooterButtonReady ? 1 : 0) + @@ -175,18 +167,18 @@ public class Shooter extends SubsystemBase { switch (bitmask) { case 0b000: // No errors but button is not pressed - io.setIndexerOutput(state, 0); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; - case 0b001: // No errors and shoot button is pressed + case 0b001: // No errors and shoot button is pressed io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; case 0b010: // Bad shooter velocity, button is not pressed case 0b011: // Bad shooter velocty, button is pressed - io.setIndexerOutput(state, 0); + io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); break; @@ -253,11 +245,6 @@ public class Shooter extends SubsystemBase { io.setIndexerOutput(state, 0); m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN); break; - case IndexerStall: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_IDLE_PERCENT_OUTPUT.get())); - io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get()); - m_robotLED.setMode(Constants.LEDConstants.INDEXER_REVERSE); - break; } io.motorStalled(state, m_Intake, m_robotLED); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 01be9f6..3198f2e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -2,6 +2,8 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; @@ -31,10 +33,14 @@ public class ShooterConstants { // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10); public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4); - public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.4); + public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.2); public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); + public static final double NEG_OFFSET = 8.; + public static final double POS_OFFSET = 8.; + + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1); // Shoot mode tolerances @@ -49,21 +55,23 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); // - public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { - // Model derived from points - // double speed = - // 1.11576*hubDistMeters*hubDistMeters + - // 0.318464*hubDistMeters + - // 30.6293; - double speed = - 0.0593402*hubDistMeters*hubDistMeters + + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) { + double speed = 0; + + if (Math.abs(chassisXSpeed) < 0.1){ + speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + 30.35696 + MODEL_TRIM.get(); - - // double speed = - // 0.00610938*hubDistMeters*hubDistMeters - // 5.65235*hubDistMeters + - // 22.82825; + } else if (chassisXSpeed > 0){ + speed = 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + chassisXSpeed * POS_OFFSET + MODEL_TRIM.get(); + + } else { // Negative is closer to hub + speed = 0.0593402*hubDistMeters*hubDistMeters + + 4.90561*hubDistMeters + + 30.35696 + chassisXSpeed * NEG_OFFSET + MODEL_TRIM.get(); + } double max = SHOOTER_MAX_VELOCITY.get(); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 8b7c445..f78a10a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -55,21 +55,24 @@ public class ShooterReal implements ShooterIO { } @Override - public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { - if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) { - if (!m_shooterStalling) { - m_shooterStalling = true; - m_stallTimerShooter.restart(); - } - if (m_stallTimerShooter.hasElapsed(5.0)) { - m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - motorStall = "Shooter Stalled"; - System.out.println(Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond))); - } - } else { - m_shooterStalling = false; - m_stallTimerShooter.reset(); - } + public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { + motorStall = ""; + // if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) { + // if (!m_shooterStalling) { + // m_shooterStalling = true; + // m_stallTimerShooter.restart(); + // } + // if (m_stallTimerShooter.hasElapsed(5.0)) { + // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + // motorStall = "Shooter Stalled"; + // System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)))); + // System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))); + // System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond))); + // } + // } else { + // m_shooterStalling = false; + // m_stallTimerShooter.reset(); + // } if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) { if (!m_indexerStalling) { @@ -79,7 +82,7 @@ public class ShooterReal implements ShooterIO { if (m_stallTimerIndexer.hasElapsed(5.0)) { m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); motorStall = "Indexer Stalled"; - System.out.println(Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput)); + System.out.println("Indexer Stalled: " + (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput))); } } else { m_indexerStalling = false; @@ -94,7 +97,7 @@ public class ShooterReal implements ShooterIO { if (m_stallTimerRoller.hasElapsed(5.0)) { m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); motorStall = "Roller Stalled"; - System.out.println(Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed())); + System.out.println("Roller Stalled: " + (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()))); } } else { m_rollerStalling = false; From 95c8a167a56fa0c68e486c7a097dc4d31a3e290b Mon Sep 17 00:00:00 2001 From: mimigamin Date: Fri, 20 Mar 2026 20:01:06 -0600 Subject: [PATCH 28/33] Drive shoot auto and shooter tolerence --- .../pathplanner/autos/DriveShoot-Hori.auto | 38 ++++++ .../paths/Shoot driving across.path | 102 +++++++++++++++ .../paths/Shoot driving vertical.path | 118 ++++++++++++++++++ .../java/frc4388/robot/RobotContainer.java | 16 +++ .../robot/constants/BuildConstants.java | 10 +- .../robot/subsystems/shooter/Shooter.java | 12 +- .../subsystems/shooter/ShooterConstants.java | 11 +- .../robot/subsystems/swerve/SwerveDrive.java | 90 ++++++++----- 8 files changed, 351 insertions(+), 46 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto create mode 100644 src/main/deploy/pathplanner/paths/Shoot driving across.path create mode 100644 src/main/deploy/pathplanner/paths/Shoot driving vertical.path diff --git a/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto new file mode 100644 index 0000000..cd4f4d0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Shoot driving across" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot Driving" + } + }, + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot driving across.path b/src/main/deploy/pathplanner/paths/Shoot driving across.path new file mode 100644 index 0000000..9d737a3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot driving across.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": null, + "nextControl": { + "x": 1.9932621113872893, + "y": 6.3390412980405895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 1.6628461538461545 + }, + "prevControl": { + "x": 1.936134248900721, + "y": 1.9128215679513527 + }, + "nextControl": { + "x": 1.9291221613556924, + "y": 1.4128707397409563 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": { + "x": 1.9198835421653975, + "y": 6.331901987278464 + }, + "nextControl": { + "x": 1.9453728680910158, + "y": 6.8312518588753814 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 1.6628461538461545 + }, + "prevControl": { + "x": 1.9337461053582308, + "y": 1.9128436544318115 + }, + "nextControl": { + "x": 1.9315103048981825, + "y": 1.4128486532604976 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9326282051282067, + "y": 6.5815769230769225 + }, + "prevControl": { + "x": 1.8878500514131944, + "y": 6.335619778537727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot driving vertical.path b/src/main/deploy/pathplanner/paths/Shoot driving vertical.path new file mode 100644 index 0000000..59da45c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Shoot driving vertical.path @@ -0,0 +1,118 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.956, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.707930530821583, + "y": 4.031008683647302 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3, + "y": 4.0 + }, + "prevControl": { + "x": 1.5499999999999998, + "y": 4.0 + }, + "nextControl": { + "x": 1.05, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9559102564102573, + "y": 4.0 + }, + "prevControl": { + "x": 2.7118639603594925, + "y": 3.9457652751099213 + }, + "nextControl": { + "x": 3.199956552461022, + "y": 4.054234724890079 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3, + "y": 4.0 + }, + "prevControl": { + "x": 1.5434594129947146, + "y": 4.0568112156556175 + }, + "nextControl": { + "x": 1.0565405870052855, + "y": 3.9431887843443825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 4.0 + }, + "prevControl": { + "x": 2.7532300973825357, + "y": 3.9599423520140395 + }, + "nextControl": { + "x": 3.2467699026174643, + "y": 4.040057647985961 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3, + "y": 4.0 + }, + "prevControl": { + "x": 1.526511041060335, + "y": 4.105795785727803 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 53e4df3..7f73bd5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -5,9 +5,14 @@ /* the project. */ /*----------------------------------------------------------------------------*/ +// 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 frc4388.robot; import java.io.File; +import java.util.Optional; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -24,6 +29,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -129,6 +135,15 @@ public class RobotContainer { IntakeExtended, new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake) ); + + private Command RobotShootDriving = new SequentialCommandGroup( + new InstantCommand(() -> + m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION) + ), + new WaitCommand(99) // stays on for the duration of the auto segment + ).finallyDo((interrupted) -> + m_robotSwerveDrive.disableRotationOverride() + ); private Command IntakeRetracted = new SequentialCommandGroup( new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) @@ -170,6 +185,7 @@ public class RobotContainer { NamedCommands.registerCommand("Robot Shoot", RobotShoot); // NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Intake Extended", IntakeExtended); + NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving); DriverStation.silenceJoystickConnectionWarning(true); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9f111da..b220d26 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 171; - public static final String GIT_SHA = "4ffbe3f5954581f07ac26ee6174113c909cdbaa7"; - public static final String GIT_DATE = "2026-03-19 18:32:02 MDT"; + public static final int GIT_REVISION = 172; + public static final String GIT_SHA = "69677d5ae9e63125a30857f5f7afa0756dfa9416"; + public static final String GIT_DATE = "2026-03-20 15:40:07 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-20 15:31:47 MDT"; - public static final long BUILD_UNIX_TIME = 1774042307703L; + public static final String BUILD_DATE = "2026-03-20 19:59:37 MDT"; + public static final long BUILD_UNIX_TIME = 1774058377112L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 52fbcd3..9522450 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -9,6 +9,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; @@ -114,12 +115,19 @@ public class Shooter extends SubsystemBase { public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter + Logger.processInputs("Shooter", state); io.updateInputs(state); // Get robot positon and speeds ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds; + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION); + Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle(); Pose2d robotPose2d = m_SwerveDrive.getPose2d(); @@ -171,8 +179,10 @@ public class Shooter extends SubsystemBase { m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; - case 0b001: // No errors and shoot button is pressed + case 0b001: // No errors and shoot button is pressed + if (ang.getDegrees() < ShooterConstants.AIM_ANGLE.get()){ // robot is aimed at hub io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); + } m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 3198f2e..816b953 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -2,8 +2,6 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; @@ -37,8 +35,8 @@ public class ShooterConstants { public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0); - public static final double NEG_OFFSET = 8.; - public static final double POS_OFFSET = 8.; + public static final ConfigurableDouble NEG_OFFSET = new ConfigurableDouble("Negative offset", 8.); + public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.); public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1); @@ -47,6 +45,7 @@ public class ShooterConstants { public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); + public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 5); public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); @@ -65,12 +64,12 @@ public class ShooterConstants { } else if (chassisXSpeed > 0){ speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + - 30.35696 + chassisXSpeed * POS_OFFSET + MODEL_TRIM.get(); + 30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get(); } else { // Negative is closer to hub speed = 0.0593402*hubDistMeters*hubDistMeters + 4.90561*hubDistMeters + - 30.35696 + chassisXSpeed * NEG_OFFSET + MODEL_TRIM.get(); + 30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get(); } double max = SHOOTER_MAX_VELOCITY.get(); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 4ebbf14..fbea572 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems.swerve; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -20,6 +21,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.trajectory.PathPlannerTrajectory; import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -65,6 +67,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + private final PIDController m_rotationOverridePID = new PIDController( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + private boolean m_useRotationOverride = false; + private Translation2d m_rotationOverrideTarget = new Translation2d(); + /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) { // public SwerveDrive(SwerveDrivetrain @@ -83,6 +93,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // Handle exception as needed config = null; } + + PPHolonomicDriveController driveController = new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), // Translation PID + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID (used when override is OFF) + ); + driveController.setRotationTargetOverride(() -> { + if (!m_useRotationOverride) return Optional.empty(); + Rotation2d targetAngle = getPose2d() + .getTranslation() + .minus(m_rotationOverrideTarget) + .getAngle(); + return Optional.of(targetAngle); + }); + // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { @@ -94,11 +118,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds() .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. // Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for - // holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants - ), + driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...) config, // The robot configuration () -> { // Boolean supplier that controls when the path will be mirrored for the red @@ -235,6 +255,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } + + + public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) { + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(chassisSpeeds.vxMetersPerSecond) + .withVelocityY(chassisSpeeds.vyMetersPerSecond) + .withTargetDirection(ang); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + ); + io.setControl(ctrl); + } public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical // reason to have a robot @@ -408,33 +449,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } - - - public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, double ballVelocity, double distanceToHub) { - - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - - if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){ - double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity)); - fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); - Logger.recordOutput("Offset Value", aimOffset); - - } - - - - Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); - - Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); - - Logger.recordOutput("Aim at Offset", fieldPos); - - driveFieldAngle(leftStick, ang); - } - public Pose2d getCurrentPose(){ return state.currentPose; } @@ -526,6 +540,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { softStop(); } + public void enableRotationOverride(Translation2d fieldTarget) { + m_rotationOverrideTarget = fieldTarget; + m_useRotationOverride = true; + } + + public void disableRotationOverride() { + m_useRotationOverride = false; + } + @Override public void periodic() { // This method will be called once per scheduler run\ @@ -663,5 +686,4 @@ public class SwerveDrive extends SubsystemBase implements Queryable { return status; } -} - +} \ No newline at end of file From 8c24514fe01de207983461a7fa75c2266198430c Mon Sep 17 00:00:00 2001 From: mimigamin Date: Fri, 20 Mar 2026 20:33:15 -0600 Subject: [PATCH 29/33] fix auto --- src/main/java/frc4388/robot/RobotContainer.java | 7 +++---- .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- .../frc4388/robot/subsystems/swerve/SwerveDrive.java | 11 +++++++++-- 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7f73bd5..bc6ded9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,10 +137,9 @@ public class RobotContainer { ); private Command RobotShootDriving = new SequentialCommandGroup( - new InstantCommand(() -> - m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION) - ), - new WaitCommand(99) // stays on for the duration of the auto segment + new RunCommand(() -> + m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION) + ).withTimeout(20) ).finallyDo((interrupted) -> m_robotSwerveDrive.disableRotationOverride() ); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index b220d26..77790bb 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 172; - public static final String GIT_SHA = "69677d5ae9e63125a30857f5f7afa0756dfa9416"; - public static final String GIT_DATE = "2026-03-20 15:40:07 MDT"; + public static final int GIT_REVISION = 173; + public static final String GIT_SHA = "95c8a167a56fa0c68e486c7a097dc4d31a3e290b"; + public static final String GIT_DATE = "2026-03-20 20:01:06 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-20 19:59:37 MDT"; - public static final long BUILD_UNIX_TIME = 1774058377112L; + public static final String BUILD_DATE = "2026-03-20 20:29:10 MDT"; + public static final long BUILD_UNIX_TIME = 1774060150289L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index fbea572..b771081 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -540,8 +540,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { softStop(); } - public void enableRotationOverride(Translation2d fieldTarget) { - m_rotationOverrideTarget = fieldTarget; + public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) { + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); + System.out.println("field pos lead: " + fieldPosLead); + Logger.recordOutput("Auto Aim", fieldPosLead); + m_rotationOverrideTarget = fieldPosLead; m_useRotationOverride = true; } From 89a1f34a4a4751acadc1509890fed15d6d870930 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Fri, 20 Mar 2026 23:50:03 -0600 Subject: [PATCH 30/33] fix rumble --- src/main/java/frc4388/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index cc9296f..95a7513 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -152,7 +152,7 @@ public class Robot extends LoggedRobot { public void teleopPeriodic() { var info = HubShiftTimer.getShiftInfo(); - double rumble = (info.remainingInShift() < 5.) ? 1 : 0; + double rumble = (info.remainingInShift() < 5. && info.remainingInShift() > 0.1) ? 1 : 0; m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble); m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble); From 3c1b94a2d86a624a9320493a0240ee552491beca Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 21 Mar 2026 13:04:40 -0600 Subject: [PATCH 31/33] summary --- simgui-ds.json | 5 +++ .../pathplanner/autos/DriveShoot-Hori.auto | 18 +++++++---- src/main/java/frc4388/robot/Robot.java | 4 +-- .../java/frc4388/robot/RobotContainer.java | 2 +- .../robot/constants/BuildConstants.java | 12 +++---- .../robot/subsystems/shooter/Shooter.java | 8 ++--- .../subsystems/shooter/ShooterConstants.java | 2 +- .../robot/subsystems/shooter/ShooterReal.java | 32 +++++++++---------- 8 files changed, 46 insertions(+), 37 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4a63cc1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto index cd4f4d0..8765ce9 100644 --- a/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto +++ b/src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto @@ -4,10 +4,22 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Robot Rev Up" + } + }, { "type": "parallel", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Robot Shoot" + } + }, { "type": "path", "data": { @@ -19,12 +31,6 @@ "data": { "name": "Robot Shoot Driving" } - }, - { - "type": "named", - "data": { - "name": "Robot Shoot" - } } ] } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 95a7513..42c6f2c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -154,8 +154,8 @@ public class Robot extends LoggedRobot { double rumble = (info.remainingInShift() < 5. && info.remainingInShift() > 0.1) ? 1 : 0; - m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble); - m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble); + // m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble); + // m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bc6ded9..46eb3b0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -153,7 +153,7 @@ public class RobotContainer { //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed), new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), - new WaitCommand(2), + new WaitCommand(5), IntakeRetracted, new WaitCommand(5), new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 2ac3242..8126120 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 171; - public static final String GIT_SHA = "08f1e26f3a6a9eefad99783748191b0d98056b07"; - public static final String GIT_DATE = "2026-03-19 16:06:31 MDT"; - public static final String GIT_BRANCH = "gurt"; - public static final String BUILD_DATE = "2026-03-19 16:29:01 MDT"; - public static final long BUILD_UNIX_TIME = 1773959341836L; + public static final int GIT_REVISION = 180; + public static final String GIT_SHA = "89a1f34a4a4751acadc1509890fed15d6d870930"; + public static final String GIT_DATE = "2026-03-20 23:50:03 MDT"; + public static final String GIT_BRANCH = "MiraOrg"; + public static final String BUILD_DATE = "2026-03-21 13:00:47 MDT"; + public static final long BUILD_UNIX_TIME = 1774119647384L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 36df512..a131d10 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -133,8 +133,7 @@ public class Shooter extends SubsystemBase { chassisSpeeds.vyMetersPerSecond ); Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION); - Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle(); - + Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle().minus(m_SwerveDrive.getPose2d().getRotation()); Pose2d robotPose2d = m_SwerveDrive.getPose2d(); if (TimesNegativeOne.isRed) { @@ -154,7 +153,8 @@ public class Shooter extends SubsystemBase { // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | - distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); + distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get() | + Math.abs(ang.getDegrees()) > ShooterConstants.AIM_ANGLE.get(); double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; @@ -186,9 +186,7 @@ public class Shooter extends SubsystemBase { break; case 0b001: // No errors and shoot button is pressed - if (ang.getDegrees() < ShooterConstants.AIM_ANGLE.get()){ // robot is aimed at hub io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get()); - } m_robotLED.setMode(Constants.LEDConstants.OPREADY); break; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 816b953..6c1b4a9 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -45,7 +45,7 @@ public class ShooterConstants { public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8); - public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 5); + public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 15); public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index f78a10a..b7e43d4 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -57,22 +57,22 @@ public class ShooterReal implements ShooterIO { @Override public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) { motorStall = ""; - // if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) { - // if (!m_shooterStalling) { - // m_shooterStalling = true; - // m_stallTimerShooter.restart(); - // } - // if (m_stallTimerShooter.hasElapsed(5.0)) { - // m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); - // motorStall = "Shooter Stalled"; - // System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)))); - // System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))); - // System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond))); - // } - // } else { - // m_shooterStalling = false; - // m_stallTimerShooter.reset(); - // } + if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) { + if (!m_shooterStalling) { + m_shooterStalling = true; + m_stallTimerShooter.restart(); + } + if (m_stallTimerShooter.hasElapsed(5.0)) { + m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED); + motorStall = "Shooter Stalled"; + System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)))); + System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond))); + System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond))); + } + } else { + m_shooterStalling = false; + m_stallTimerShooter.reset(); + } if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) { if (!m_indexerStalling) { From 75cab187e2225e51259336e5569f15c41f9169d5 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 21 Mar 2026 13:38:34 -0600 Subject: [PATCH 32/33] New shooter model and auto tester --- .../paths/Shoot driving across.path | 8 +- .../paths/Shoot driving vertical.path | 118 ------------------ .../robot/constants/BuildConstants.java | 10 +- .../subsystems/shooter/ShooterConstants.java | 2 +- .../robot/subsystems/shooter/ShooterReal.java | 2 + .../robot/subsystems/swerve/SwerveDrive.java | 35 +++--- .../swerve/SwerveDriveConstants.java | 3 +- 7 files changed, 29 insertions(+), 149 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/Shoot driving vertical.path diff --git a/src/main/deploy/pathplanner/paths/Shoot driving across.path b/src/main/deploy/pathplanner/paths/Shoot driving across.path index 9d737a3..4af7874 100644 --- a/src/main/deploy/pathplanner/paths/Shoot driving across.path +++ b/src/main/deploy/pathplanner/paths/Shoot driving across.path @@ -53,11 +53,11 @@ }, "prevControl": { "x": 1.9337461053582308, - "y": 1.9128436544318115 + "y": 1.912843654431812 }, "nextControl": { "x": 1.9315103048981825, - "y": 1.4128486532604976 + "y": 1.4128486532604971 }, "isLocked": false, "linkedName": null @@ -81,7 +81,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 0.8, "maxAcceleration": 10.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, @@ -95,7 +95,7 @@ "reversed": false, "folder": null, "idealStartingState": { - "velocity": 0, + "velocity": 0.0, "rotation": 180.0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Shoot driving vertical.path b/src/main/deploy/pathplanner/paths/Shoot driving vertical.path deleted file mode 100644 index 59da45c..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot driving vertical.path +++ /dev/null @@ -1,118 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.956, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.707930530821583, - "y": 4.031008683647302 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3, - "y": 4.0 - }, - "prevControl": { - "x": 1.5499999999999998, - "y": 4.0 - }, - "nextControl": { - "x": 1.05, - "y": 4.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9559102564102573, - "y": 4.0 - }, - "prevControl": { - "x": 2.7118639603594925, - "y": 3.9457652751099213 - }, - "nextControl": { - "x": 3.199956552461022, - "y": 4.054234724890079 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3, - "y": 4.0 - }, - "prevControl": { - "x": 1.5434594129947146, - "y": 4.0568112156556175 - }, - "nextControl": { - "x": 1.0565405870052855, - "y": 3.9431887843443825 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 4.0 - }, - "prevControl": { - "x": 2.7532300973825357, - "y": 3.9599423520140395 - }, - "nextControl": { - "x": 3.2467699026174643, - "y": 4.040057647985961 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3, - "y": 4.0 - }, - "prevControl": { - "x": 1.526511041060335, - "y": 4.105795785727803 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, - "maxAngularVelocity": 600.0, - "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 8126120..b6a032f 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 180; - public static final String GIT_SHA = "89a1f34a4a4751acadc1509890fed15d6d870930"; - public static final String GIT_DATE = "2026-03-20 23:50:03 MDT"; + public static final int GIT_REVISION = 181; + public static final String GIT_SHA = "3c1b94a2d86a624a9320493a0240ee552491beca"; + public static final String GIT_DATE = "2026-03-21 13:04:40 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-21 13:00:47 MDT"; - public static final long BUILD_UNIX_TIME = 1774119647384L; + public static final String BUILD_DATE = "2026-03-21 13:30:10 MDT"; + public static final long BUILD_UNIX_TIME = 1774121410165L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 6c1b4a9..fdf0af5 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -39,7 +39,7 @@ public class ShooterConstants { public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.); - public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1); + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.1); // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index b7e43d4..f92a760 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -141,6 +141,8 @@ public class ShooterReal implements ShooterIO { // Math.abs(currentLimit.in(Amps)) > current && // Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity // ) { + state.motor1TargetVelocity = RotationsPerSecond.of(percentOutput); + state.motor2TargetVelocity = RotationsPerSecond.of(percentOutput); m_shooter1Motor.set(percentOutput); m_shooter2Motor.set(percentOutput); // } else { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index b771081..29a3686 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -416,19 +416,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // Drive with the robot facing towards a specific position public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { - // Get the current speed of the robot - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - - // Calculate a point to aim ahead of the actual position. - Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); - - // Calculate the angle between the current position and the lead position + Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond); + double yDistance = Math.abs(getPose2d().getTranslation().getY() - fieldPos.getY()); + if ((chassisSpeeds.vyMetersPerSecond >0 &&getPose2d().getTranslation().getY() >4)||(chassisSpeeds.vyMetersPerSecond <0 &&getPose2d().getTranslation().getY() <4)){ + if (Math.abs(chassisSpeeds.vyMetersPerSecond) > 0.2) { + if (TimesNegativeOne.isRed){ + robotSpeedYOnly = new Translation2d(-SwerveDriveConstants.FAR_OFFSET.get() * yDistance * (getPose2d().getTranslation().getX()-7.28989525), chassisSpeeds.vyMetersPerSecond); + } else { + robotSpeedYOnly = new Translation2d((getPose2d().getTranslation().getX())* yDistance* SwerveDriveConstants.FAR_OFFSET.get(), chassisSpeeds.vyMetersPerSecond); + } + } } + Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos); Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); - - + Pose2d fieldPosLeadLog = new Pose2d(fieldPosLead, new Rotation2d()); + Logger.recordOutput("Lead Aim", fieldPosLeadLog); driveFieldAngle(leftStick, ang); } @@ -541,13 +542,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) { - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); - System.out.println("field pos lead: " + fieldPosLead); - Logger.recordOutput("Auto Aim", fieldPosLead); + Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond); + + Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos); m_rotationOverrideTarget = fieldPosLead; m_useRotationOverride = true; } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 5bd6369..597b7a3 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -73,8 +73,7 @@ public final class SwerveDriveConstants { public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; - public static ConfigurableDouble distanceTolerence = new ConfigurableDouble("Distance Tolerence", 0.5); - + public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05); // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); public static final class ModuleSpecificConstants { //2026 From d639b3076d10f39eb5bc18c42c3dc5e707e991ff Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 21 Mar 2026 15:49:44 -0600 Subject: [PATCH 33/33] Ramp --- .../pathplanner/autos/ShootPreloadBump.auto | 43 +++++++++++ .../deploy/pathplanner/paths/Bump test.path | 75 +++++++++++++++++++ .../pathplanner/paths/BumpToCenter.path | 54 +++++++++++++ .../java/frc4388/robot/RobotContainer.java | 30 +++++++- .../robot/constants/BuildConstants.java | 10 +-- .../robot/constants/FieldConstants.java | 21 ++++-- .../robot/subsystems/swerve/SwerveDrive.java | 14 ++++ 7 files changed, 234 insertions(+), 13 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/ShootPreloadBump.auto create mode 100644 src/main/deploy/pathplanner/paths/Bump test.path create mode 100644 src/main/deploy/pathplanner/paths/BumpToCenter.path diff --git a/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto new file mode 100644 index 0000000..43867c9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootPreloadBump.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Extended" + } + }, + { + "type": "path", + "data": { + "pathName": "Bump test" + } + }, + { + "type": "named", + "data": { + "name": "BumpOffsetForward" + } + }, + { + "type": "path", + "data": { + "pathName": "BumpToCenter" + } + }, + { + "type": "path", + "data": { + "pathName": "HubFarLeft-NeutralZone 2-2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bump test.path b/src/main/deploy/pathplanner/paths/Bump test.path new file mode 100644 index 0000000..dfb810c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bump test.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4908076923076936, + "y": 6.633345195729537 + }, + "prevControl": null, + "nextControl": { + "x": 2.3977564102564113, + "y": 6.0868195547038955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4908076923076936, + "y": 5.6745769230769225 + }, + "prevControl": { + "x": 2.6330609248104158, + "y": 5.743196664476699 + }, + "nextControl": { + "x": 3.7815128205128214, + "y": 5.651320512820514 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.967615384615385, + "y": 5.709461538461539 + }, + "prevControl": { + "x": 4.746653846153847, + "y": 5.430384615384616 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.5211640211640187, + "rotationDegrees": -112.5 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -91.24536426676825 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.59775645089275 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BumpToCenter.path b/src/main/deploy/pathplanner/paths/BumpToCenter.path new file mode 100644 index 0000000..7051d4f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BumpToCenter.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.935255041518388, + "y": 5.6650177935943065 + }, + "prevControl": null, + "nextControl": { + "x": 6.073930090574693, + "y": 5.873030367178768 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.559893238434164, + "y": 7.547876631079477 + }, + "prevControl": { + "x": 7.1940806642941855, + "y": 7.182064056939502 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.98776039963968 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -91.19348942398209 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 46eb3b0..2559999 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.Swerve.StayInPosition; import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.subsystems.intake.Intake; @@ -155,7 +156,7 @@ public class RobotContainer { new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter), new WaitCommand(5), IntakeRetracted, - new WaitCommand(5), + new WaitCommand(10), new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter), new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter) ); @@ -185,7 +186,29 @@ public class RobotContainer { // NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Intake Extended", IntakeExtended); NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving); + + NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed)); + NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter)); + NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter)); + NamedCommands.registerCommand("SpinUpShooting", new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter)); + NamedCommands.registerCommand("SpinUpIdle", new InstantCommand(() -> m_robotShooter.spinUpIdle(), m_robotShooter)); + NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> { + if (TimesNegativeOne.isRed) { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED); + } else { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE); + } + })); + + NamedCommands.registerCommand("BumpOffsetReverse", new InstantCommand(() -> { + if (!TimesNegativeOne.isRed) { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED); + } else { + m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE); + } + })); + DriverStation.silenceJoystickConnectionWarning(true); @@ -372,8 +395,11 @@ public class RobotContainer { private void configureSINGLEBindings() { //Driver controls + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED))); new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index b6a032f..b2c6e49 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 181; - public static final String GIT_SHA = "3c1b94a2d86a624a9320493a0240ee552491beca"; - public static final String GIT_DATE = "2026-03-21 13:04:40 MDT"; + public static final int GIT_REVISION = 182; + public static final String GIT_SHA = "75cab187e2225e51259336e5569f15c41f9169d5"; + public static final String GIT_DATE = "2026-03-21 13:38:34 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-21 13:30:10 MDT"; - public static final long BUILD_UNIX_TIME = 1774121410165L; + public static final String BUILD_DATE = "2026-03-21 15:45:59 MDT"; + public static final long BUILD_UNIX_TIME = 1774129559544L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java index f5a7245..49c75c0 100644 --- a/src/main/java/frc4388/robot/constants/FieldConstants.java +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -1,18 +1,27 @@ package frc4388.robot.constants; -import java.util.Arrays; +import static edu.wpi.first.units.Units.Meters; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; public final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + public static final double BUMP_OFFSET = 0.4; + public static final Transform2d BUMP_OFFSET_RED = new Transform2d( + Meters.of(BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + public static final Transform2d BUMP_OFFSET_BLUE = new Transform2d( + Meters.of(-BUMP_OFFSET), + Meters.of(0), + new Rotation2d() + ); + // Test april tag field layout // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( // Arrays.asList(new AprilTag[] { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 29a3686..5523eec 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -24,6 +24,7 @@ import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.AngularVelocity; @@ -433,6 +434,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } + public void offsetOdoPosition(Transform2d offset) { + // Manually performing an addittion on the pose + // WHY doesn't WPILIB have the ability to not transform poses + Pose2d new_pose = new Pose2d( + new Translation2d( + state.currentPose.getX() + offset.getX(), + state.currentPose.getY() + offset.getY() + ), + state.currentPose.getRotation() + ); + this.io.resetPose(new_pose); + } + public void defenseXPosition(){ io.setControl(new SwerveRequest.SwerveDriveBrake()); }