From 3d7601387a953b000cbc7c919fb33c900b5c5537 Mon Sep 17 00:00:00 2001 From: Mehooliu Date: Sat, 21 Feb 2026 17:26:10 -0500 Subject: [PATCH] fixed intake ramp, shooter, and drive PID --- .../pathplanner/paths/test_1_Meter.path | 10 +- .../swerve/neo/controllerproperties.json | 2 +- .../deploy/swerve/neo/modules/backleft.json | 2 +- .../deploy/swerve/neo/modules/backright.json | 2 +- .../deploy/swerve/neo/modules/frontleft.json | 2 +- .../deploy/swerve/neo/modules/frontright.json | 2 +- .../swerve/neo/modules/pidfproperties.json | 12 +- src/main/java/frc/robot/Constants.java | 54 ++- src/main/java/frc/robot/Robot.java | 11 +- src/main/java/frc/robot/RobotContainer.java | 444 +++++++++--------- .../robot/subsystems/ClimberSubsystem.java | 2 +- .../frc/robot/subsystems/IntakeSubsystem.java | 18 +- .../robot/subsystems/ShooterSubsystem.java | 94 +++- .../robot/subsystems/TargetingSubsystems.java | 17 +- .../swervedrive/SwerveSubsystem.java | 2 +- 15 files changed, 382 insertions(+), 292 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/test_1_Meter.path b/src/main/deploy/pathplanner/paths/test_1_Meter.path index e92f212..69305a4 100644 --- a/src/main/deploy/pathplanner/paths/test_1_Meter.path +++ b/src/main/deploy/pathplanner/paths/test_1_Meter.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.075, - "y": 4.75 + "x": 2.0, + "y": 7.0 }, "prevControl": { - "x": 2.380178890876563, - "y": 5.341144901610018 + "x": 0.8572206587126847, + "y": 7.0 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/swerve/neo/controllerproperties.json b/src/main/deploy/swerve/neo/controllerproperties.json index 669097e..edebd2f 100644 --- a/src/main/deploy/swerve/neo/controllerproperties.json +++ b/src/main/deploy/swerve/neo/controllerproperties.json @@ -2,7 +2,7 @@ "angleJoystickRadiusDeadband": 0.5, "heading": { "p": 0.4, - "i": 0, + "i": 0.01, "d": 0.01 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/modules/backleft.json b/src/main/deploy/swerve/neo/modules/backleft.json index f3da6a7..bc637be 100644 --- a/src/main/deploy/swerve/neo/modules/backleft.json +++ b/src/main/deploy/swerve/neo/modules/backleft.json @@ -3,7 +3,7 @@ "front": -10.9, "left": 10.9 }, - "absoluteEncoderOffset": 216.38671875, + "absoluteEncoderOffset": 215.947265625, "drive": { "type": "sparkflex_neo", "id": 12, diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json index 945939f..95b169c 100644 --- a/src/main/deploy/swerve/neo/modules/backright.json +++ b/src/main/deploy/swerve/neo/modules/backright.json @@ -3,7 +3,7 @@ "front": -10.9, "left": -10.9 }, - "absoluteEncoderOffset": 288.720703125, + "absoluteEncoderOffset": 287.578125, "drive": { "type": "sparkflex_neo", "id": 11, diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json index 4658b57..add715d 100644 --- a/src/main/deploy/swerve/neo/modules/frontleft.json +++ b/src/main/deploy/swerve/neo/modules/frontleft.json @@ -3,7 +3,7 @@ "front": 10.9, "left": 10.9 }, - "absoluteEncoderOffset": 196.96287536621094, + "absoluteEncoderOffset": 36.03515625, "drive": { "type": "sparkflex_neo", "id": 13, diff --git a/src/main/deploy/swerve/neo/modules/frontright.json b/src/main/deploy/swerve/neo/modules/frontright.json index 1bfbd3b..73e61f9 100644 --- a/src/main/deploy/swerve/neo/modules/frontright.json +++ b/src/main/deploy/swerve/neo/modules/frontright.json @@ -3,7 +3,7 @@ "front": 10.9, "left": -10.9 }, - "absoluteEncoderOffset": 318.427734375, + "absoluteEncoderOffset": 318.779296875, "drive": { "type": "sparkflex_neo", "id": 10, diff --git a/src/main/deploy/swerve/neo/modules/pidfproperties.json b/src/main/deploy/swerve/neo/modules/pidfproperties.json index c65ca10..90d33df 100644 --- a/src/main/deploy/swerve/neo/modules/pidfproperties.json +++ b/src/main/deploy/swerve/neo/modules/pidfproperties.json @@ -1,16 +1,16 @@ { "drive": { - "p": 0.001, + "p": 0.15, "i": 0, "d": 0, - "f": 0, + "f": 0.15, "iz": 0 }, "angle": { - "p": 0.002, - "i": 0.0, - "d": 0.0, - "f": 0, + "p": 0.008, + "i": 0.00001, + "d": 0, + "f": 0.2, "iz": 0 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7e952c1..60c1364 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -68,60 +68,66 @@ public final class Constants { } public static class ShooterConstants { - private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -5000) + private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000) .withWidget(BuiltInWidgets.kNumberBar).getEntry(); public static double SHOOTER_RPM; public static void updateShooterRPM() { - SHOOTER_RPM = shooterRPM.getDouble(-5000); + SHOOTER_RPM = shooterRPM.getDouble(-4000); } public static final int CENTER_SHOOTER_MOTOR_ID = 42; public static final int LEFT_SHOOTER_MOTOR_ID = 41; public static final int RIGHT_SHOOTER_MOTOR_ID = 40; public static final int INDEXER_MOTOR_ID = 43; - public static double INDEXER_AND_RAMP_MOTOR_RPM; - public static double SHOOTER_MOTOR_P = 0.001; + public static double SHOOTER_MOTOR_P = 0.0018; public static double SHOOTER_MOTOR_I = 0; public static double SHOOTER_MOTOR_D = 0; - private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer Speed", 2000) - .withWidget(BuiltInWidgets.kNumberBar).getEntry(); + public static double INDEXER_MOTOR_P = 0.0001; + public static double INDEXER_MOTOR_I = 0; + public static double INDEXER_MOTOR_D = 0; + + + /*private static GenericEntry indexerAndRampRPM = programmingTab.add("Desired Ramp + Indexer RPM", 2000) + .withWidget(BuiltInWidgets.kNumberBar).getEntry();*/ + + + public static double INDEXER_AND_RAMP_MOTOR_RPM = 10000; // this method called in robot periodic so values updated in elastic are // constantly read and applied to RAMP_MOTOR_SPEED - public static void updateIndexerAndRampMotorRPM() { + /*public static void updateIndexerAndRampMotorRPM() { INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000); - } + }*/ } public static class IntakeConstants { - public static double INTAKE_WHEELS_MOTOR_SPEED; - private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000) - .withWidget(BuiltInWidgets.kNumberBar).getEntry(); - public static double INTAKE_WHEELS_MOTOR_RPM; + /* private static GenericEntry intakeRPM = programmingTab.add("Desired Intake RPM", -1000) + .withWidget(BuiltInWidgets.kNumberBar).getEntry(); */ + public static double INTAKE_WHEELS_MOTOR_RPM = -7000; - public static void updateIntakeWheelsRPM() { + /*public static void updateIntakeWheelsRPM() { INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000); - } + }*/ public static final int INTAKE_WHEELS_MOTOR_ID = 50; public static final int INTAKE_ROTATOR_MOTOR_ID = 51; public static class IntakeRotatorPID { - public static final double INTAKE_ROTATOR_P = 0.01; + public static final double INTAKE_ROTATOR_P = 0.03; public static final double INTAKE_ROTATOR_I = 0; public static final double INTAKE_ROTATOR_D = 0; } - public static final double INTAKE_MOTOR_P = 0.01; - public static final double INTAKE_MOTOR_I = 0; - public static final double INTAKE_MOTOR_D = 0; + public static final double INTAKE_MOTOR_P = 0.0001; + public static final double INTAKE_MOTOR_I = 0; + public static final double INTAKE_MOTOR_D = 0; - public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535; - public static final double INTAKE_MIDDLE_ENCODER_VALUE = 1.2550222873687744; + public static final double INTAKE_COLLECT_ENCODER_VALUE = 5; + public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5; public static final double INTAKE_RETRACT_ENCODER_VALUE = 0; } @@ -135,10 +141,10 @@ public final class Constants { public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90)); public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90)); - public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera"); - public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera"); - public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera"); - public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Arducam_OV9281_USB_Camera"); + public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Rear Left Camera"); + public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Rear Right Camera"); + public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera"); + public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera"); public static final Pose2d HUB_POSE = new Pose2d(4.625, 4.03, new Rotation2d()); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c06db05..834b5d5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import org.photonvision.targeting.PhotonPipelineResult; + import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.networktables.GenericEntry; @@ -36,6 +38,7 @@ public class Robot extends TimedRobot { private static NetworkTable table; private static GenericEntry distanceFromLimelight; + public Robot() { instance = this; } @@ -63,6 +66,8 @@ public class Robot extends TimedRobot { if (isSimulation()) { DriverStation.silenceJoystickConnectionWarning(true); } + + m_robotContainer.getSwerveDrive().zeroGyroWithAlliance(); } /** @@ -86,10 +91,10 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - // Constants.ShooterConstants.getRampAndIndexerMotorSpeed(); - Constants.IntakeConstants.updateIntakeWheelsRPM(); + //Constants.ShooterConstants.getRampAndIndexerMotorSpeed(); + //Constants.IntakeConstants.updateIntakeWheelsRPM(); Constants.ShooterConstants.updateShooterRPM(); - Constants.ShooterConstants.updateIndexerAndRampMotorRPM(); + //Constants.ShooterConstants.updateIndexerAndRampMotorRPM(); TargetingSubsystems.updateRobotPose(Constants.TargetingConstants.ORANGE_PHOTON_CAM, Constants.TargetingConstants.ORANGE_PHOTON_ESTIMATOR, m_robotContainer.getSwerveDrive()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b715c8a..a139d09 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -51,246 +51,256 @@ import frc.robot.subsystems.ClimberSubsystem; */ public class RobotContainer { - // Replace with CommandPS4Controller or CommandJoystick if needed - final CommandXboxController driverXbox = new CommandXboxController(0); - // The robot's subsystems and commands are defined here... - private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), - "swerve/neo")); + // Replace with CommandPS4Controller or CommandJoystick if needed + final CommandXboxController driverXbox = new CommandXboxController(0); + final CommandXboxController operatorXbox = new CommandXboxController(1); - // Establish a Sendable Chooser that will be able to be sent to the - // SmartDashboard, allowing selection of desired auto - private final SendableChooser autoChooser; + // The robot's subsystems and commands are defined here... + private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), + "swerve/neo")); - private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); - // private final TargetingSubsystems m_TargetingSubsystems = new - // TargetingSubsystems(); - private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); - private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); + // Establish a Sendable Chooser that will be able to be sent to the + // SmartDashboard, allowing selection of desired auto + private final SendableChooser autoChooser; - /** - * Converts driver input into a field-relative ChassisSpeeds that is controlled - * by angular velocity. - */ - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) - .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); + private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); + // private final TargetingSubsystems m_TargetingSubsystems = new + // TargetingSubsystems(); + private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); + private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); - /** - * Clone's the angular velocity input stream and converts it to a fieldRelative - * input stream. - */ - SwerveInputStream driveDirectAngle = driveAngularVelocity.copy() - .withControllerHeadingAxis(driverXbox::getRightX, - driverXbox::getRightY) - .headingWhile(true); + /** + * Converts driver input into a field-relative ChassisSpeeds that is controlled + * by angular velocity. + */ + SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> driverXbox.getLeftY() * -1, + () -> driverXbox.getLeftX() * -1) + .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); - /** - * Clone's the angular velocity input stream and converts it to a robotRelative - * input stream. - */ - SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false) - .allianceRelativeControl(true); + /** + * Clone's the angular velocity input stream and converts it to a fieldRelative + * input stream. + */ + SwerveInputStream driveDirectAngle = driveAngularVelocity.copy() + .withControllerHeadingAxis(driverXbox::getRightX, + driverXbox::getRightY) + .headingWhile(true); - SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX()) - .withControllerRotationAxis(() -> driverXbox.getRawAxis( - 2)) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - // Derive the heading axis with math! - SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() - .withControllerHeadingAxis(() -> Math.sin( - driverXbox.getRawAxis( - 2) * - Math.PI) - * - (Math.PI * - 2), - () -> Math.cos( - driverXbox.getRawAxis( - 2) * - Math.PI) - * - (Math.PI * - 2)) - .headingWhile(true) - .translationHeadingOffset(true) - .translationHeadingOffset(Rotation2d.fromDegrees( - 0)); + /** + * Clone's the angular velocity input stream and converts it to a robotRelative + * input stream. + */ + SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false) + .allianceRelativeControl(true); - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - // Configure the trigger bindings - configureBindings(); - DriverStation.silenceJoystickConnectionWarning(true); + SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX()) + .withControllerRotationAxis(() -> driverXbox.getRawAxis( + 2)) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + // Derive the heading axis with math! + SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() + .withControllerHeadingAxis(() -> Math.sin( + driverXbox.getRawAxis( + 2) * + Math.PI) + * + (Math.PI * + 2), + () -> Math.cos( + driverXbox.getRawAxis( + 2) * + Math.PI) + * + (Math.PI * + 2)) + .headingWhile(true) + .translationHeadingOffset(true) + .translationHeadingOffset(Rotation2d.fromDegrees( + 0)); - // Create the NamedCommands that will be used in PathPlanner - NamedCommands.registerCommand("test", Commands.print("I EXIST")); + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the trigger bindings + configureBindings(); + DriverStation.silenceJoystickConnectionWarning(true); - // Have the autoChooser pull in all PathPlanner autos as options - autoChooser = AutoBuilder.buildAutoChooser(); + // Create the NamedCommands that will be used in PathPlanner + NamedCommands.registerCommand("test", Commands.print("I EXIST")); - // Set the default auto (do nothing) - autoChooser.setDefaultOption("Do Nothing", Commands.none()); + // Have the autoChooser pull in all PathPlanner autos as options + autoChooser = AutoBuilder.buildAutoChooser(); - // Add a simple auto option to have the robot drive forward for 1 second then - // stop - autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1)); + // Set the default auto (do nothing) + autoChooser.setDefaultOption("Do Nothing", Commands.none()); - // Put the autoChooser on the SmartDashboard - SmartDashboard.putData("Auto Chooser", autoChooser); + // Add a simple auto option to have the robot drive forward for 1 second then + // stop + autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1)); + // Put the autoChooser on the SmartDashboard + SmartDashboard.putData("Auto Chooser", autoChooser); + + } + + /** + * Use this method to define your trigger->command mappings. Triggers can be + * created via the + * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with + * an arbitrary predicate, or via the + * named factories in + * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses + * for + * {@link CommandXboxController + * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4} + * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick + * Flight joysticks}. + */ + private void configureBindings() { + Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); + Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); + Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); + Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( + driveDirectAngle); + Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); + Command driveFieldOrientedAnglularVelocityKeyboard = drivebase + .driveFieldOriented(driveAngularVelocityKeyboard); + Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( + driveDirectAngleKeyboard); + + driverXbox.leftTrigger().whileTrue(m_IntakeSubsystem.startIntakeMotorCommand()) + .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); + driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand().andThen(m_ShooterSubsystem.reverseIndexerAndRampMotorRPMCommand())) + .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand().andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand())); + // command for + // full shooting system including linear actuators + driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); + // .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); + + driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); + + driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()) + .onFalse(m_ClimberSubsystem.stopClimberCommand()); + driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()) + .onFalse(m_ClimberSubsystem.stopClimberCommand()); + driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand()); + driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand()); + driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true)); + driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false)); + driverXbox.rightStick().onTrue(climbCommand()); + + // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); + driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand() + .andThen(m_IntakeSubsystem.deployintakeCommand())); + // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(), + // () -> -driverXbox.getLeftX())); + + // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE)); + + operatorXbox.x().whileTrue(m_ShooterSubsystem.testLeftShooterCommand()) + .onFalse(m_ShooterSubsystem.stopLeftShooterCommand()); + operatorXbox.y().whileTrue(m_ShooterSubsystem.testCenterShooterCommand()) + .onFalse(m_ShooterSubsystem.stopCenterShooterCommand()); + operatorXbox.b().whileTrue(m_ShooterSubsystem.testRightShooterCommand()) + .onFalse(m_ShooterSubsystem.stopRightShooterCommand()); + operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand()) + .onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand()); + + if (RobotBase.isSimulation()) { + drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); + } else { + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); } - /** - * Use this method to define your trigger->command mappings. Triggers can be - * created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with - * an arbitrary predicate, or via the - * named factories in - * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses - * for - * {@link CommandXboxController - * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4} - * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick - * Flight joysticks}. - */ - private void configureBindings() { - Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); - Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); - Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( - driveDirectAngle); - Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); - Command driveFieldOrientedAnglularVelocityKeyboard = drivebase - .driveFieldOriented(driveAngularVelocityKeyboard); - Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( - driveDirectAngleKeyboard); + if (Robot.isSimulation()) { + Pose2d target = new Pose2d(new Translation2d(1, 4), + Rotation2d.fromDegrees(90)); + // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target); + driveDirectAngleKeyboard.driveToPose(() -> target, + new ProfiledPIDController(5, + 0, + 0, + new Constraints(5, 2)), + new ProfiledPIDController(5, + 0, + 0, + new Constraints(Units.degreesToRadians(360), + Units.degreesToRadians(180)))); + driverXbox.start() + .onTrue(Commands.runOnce(() -> drivebase + .resetOdometry(new Pose2d(3, 3, new Rotation2d())))); + driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); + driverXbox.button(2) + .whileTrue(Commands.runEnd( + () -> driveDirectAngleKeyboard.driveToPoseEnabled(true), + () -> driveDirectAngleKeyboard.driveToPoseEnabled(false))); - driverXbox.leftTrigger().whileTrue(m_IntakeSubsystem.startIntakeMotorCommand()) - .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); - driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand()) - .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()); - - // command for - // full shooting system including linear actuators - driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand() - .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly())); - - driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); - - driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()) - .onFalse(m_ClimberSubsystem.stopClimberCommand()); - driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()) - .onFalse(m_ClimberSubsystem.stopClimberCommand()); - driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand()); - driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand()); - driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true)); - driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false)); - driverXbox.rightStick().onTrue(climbCommand()); - - // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); - driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand() - .andThen(m_IntakeSubsystem.deployintakeCommand())); - // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(), - // () -> -driverXbox.getLeftX())); - - // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE)); - - if (RobotBase.isSimulation()) { - drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); - } else { - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); - } - - if (Robot.isSimulation()) { - Pose2d target = new Pose2d(new Translation2d(1, 4), - Rotation2d.fromDegrees(90)); - // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target); - driveDirectAngleKeyboard.driveToPose(() -> target, - new ProfiledPIDController(5, - 0, - 0, - new Constraints(5, 2)), - new ProfiledPIDController(5, - 0, - 0, - new Constraints(Units.degreesToRadians(360), - Units.degreesToRadians(180)))); - driverXbox.start() - .onTrue(Commands.runOnce(() -> drivebase - .resetOdometry(new Pose2d(3, 3, new Rotation2d())))); - driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); - driverXbox.button(2) - .whileTrue(Commands.runEnd( - () -> driveDirectAngleKeyboard.driveToPoseEnabled(true), - () -> driveDirectAngleKeyboard.driveToPoseEnabled(false))); - - // driverXbox.b().whileTrue( - // drivebase.driveToPose( - // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) - // ); - - } - if (DriverStation.isTest()) { - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command - // above! - - driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.back().whileTrue(drivebase.centerModulesCommand()); - driverXbox.leftBumper().onTrue(Commands.none()); - driverXbox.rightBumper().onTrue(Commands.none()); - } else { - driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); - driverXbox.start().whileTrue(Commands.none()); - driverXbox.back().whileTrue(Commands.none()); - driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.rightBumper().onTrue(Commands.none()); - } + // driverXbox.b().whileTrue( + // drivebase.driveToPose( + // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) + // ); } + if (DriverStation.isTest()) { + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command + // above! - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // Pass in the selected auto from the SmartDashboard as our desired autnomous - // commmand - return autoChooser.getSelected(); + driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); + driverXbox.back().whileTrue(drivebase.centerModulesCommand()); + driverXbox.leftBumper().onTrue(Commands.none()); + driverXbox.rightBumper().onTrue(Commands.none()); + } else { + driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); + driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); + driverXbox.start().whileTrue(Commands.none()); + driverXbox.back().whileTrue(Commands.none()); + driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); + driverXbox.rightBumper().onTrue(Commands.none()); } - public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); - } + } - public SwerveSubsystem getSwerveDrive() { - return drivebase; - } + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // Pass in the selected auto from the SmartDashboard as our desired autnomous + // commmand + return autoChooser.getSelected(); + } - public Command climbCommand() { - if (driverXbox.getRightY() > -0.5) { - return m_ClimberSubsystem.lowerRobotCommand(); - } else if (driverXbox.getRightX() < 0.5) { - return m_ClimberSubsystem.liftRobotCommand(); - } else - return m_ClimberSubsystem.stopClimberCommand(); + public void setMotorBrake(boolean brake) { + drivebase.setMotorBrake(brake); + } - } + public SwerveSubsystem getSwerveDrive() { + return drivebase; + } - public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup( - // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE), - m_ShooterSubsystem.shootFuelCommand(), - m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); + public Command climbCommand() { + if (driverXbox.getRightY() > -0.5) { + return m_ClimberSubsystem.lowerRobotCommand(); + } else if (driverXbox.getRightX() < 0.5) { + return m_ClimberSubsystem.liftRobotCommand(); + } else + return m_ClimberSubsystem.stopClimberCommand(); + + } + + public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup( + // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE), + m_ShooterSubsystem.shootFuelCommand(), + m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 384f309..2192787 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -50,6 +50,6 @@ public class ClimberSubsystem extends SubsystemBase{ @Override public void periodic() { - SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition()); + SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getAngle()); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 6952760..e57676d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -31,9 +31,14 @@ public class IntakeSubsystem extends SubsystemBase { public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig(); public IntakeSubsystem() { - intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P, - Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I, - Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D); + intakeRotatorConfig.closedLoop + .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P) + .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I) + .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D) + + .p(.06, ClosedLoopSlot.kSlot1) + .i(0, ClosedLoopSlot.kSlot1) + .d(0, ClosedLoopSlot.kSlot1); intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); @@ -43,7 +48,7 @@ public class IntakeSubsystem extends SubsystemBase { Constants.IntakeConstants.INTAKE_MOTOR_D); intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); - intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController(); + intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController(); } public void startIntakeMotor() { @@ -71,8 +76,7 @@ public class IntakeSubsystem extends SubsystemBase { } public void deployIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, - ControlType.kPosition); + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot0); } public Command deployintakeCommand() { @@ -80,7 +84,7 @@ public class IntakeSubsystem extends SubsystemBase { } public void retractIntake() { - intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition); + intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE, ControlType.kPosition, ClosedLoopSlot.kSlot1); } public Command retractIntakeCommand() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 854521c..2e68527 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -30,8 +30,8 @@ public class ShooterSubsystem extends SubsystemBase { private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID, MotorType.kBrushless); - //private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID, - // MotorType.kBrushless); + private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID, + MotorType.kBrushless); private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID, MotorType.kBrushless); @@ -42,8 +42,8 @@ public class ShooterSubsystem extends SubsystemBase { private static SparkClosedLoopController centerShooterMotorPIDController; public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig(); - //private static SparkClosedLoopController leftShooterMotorPIDController; - //public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig(); + private static SparkClosedLoopController leftShooterMotorPIDController; + public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig(); private static SparkClosedLoopController rightShooterMotorPIDController; public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig(); @@ -59,13 +59,13 @@ public class ShooterSubsystem extends SubsystemBase { com.revrobotics.PersistMode.kNoPersistParameters); centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController(); - /*leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, + leftShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, Constants.ShooterConstants.SHOOTER_MOTOR_I, Constants.ShooterConstants.SHOOTER_MOTOR_D); leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController(); - */ + rightShooterMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, Constants.ShooterConstants.SHOOTER_MOTOR_I, Constants.ShooterConstants.SHOOTER_MOTOR_D); @@ -73,9 +73,9 @@ public class ShooterSubsystem extends SubsystemBase { com.revrobotics.PersistMode.kNoPersistParameters); rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController(); - indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.SHOOTER_MOTOR_P, - Constants.ShooterConstants.SHOOTER_MOTOR_I, - Constants.ShooterConstants.SHOOTER_MOTOR_D); + indexerAndRampMotorConfig.closedLoop.pid(0.0001, + 0, + 0); indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController(); @@ -83,7 +83,7 @@ public class ShooterSubsystem extends SubsystemBase { //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT, - // MotorType.kBrushless); + // MotorType.kBrushless); //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT, //MotorType.kBrushless); @@ -95,10 +95,54 @@ public class ShooterSubsystem extends SubsystemBase { public void setShooterMotorsRPM() { centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); - //leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); } + //test individual motor code + public void setLeftShooterMotorRPM() { + leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + } + public Command testLeftShooterCommand() { + return runOnce(() -> setLeftShooterMotorRPM()); + } + + public void setRightShooterMotorRPM() { + rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + } + public Command testRightShooterCommand() { + return runOnce(() -> setRightShooterMotorRPM()); + } + + public void setCenterShooterMotorRPM() { + centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM,ControlType.kVelocity); + } + public Command testCenterShooterCommand() { + return runOnce(() -> setCenterShooterMotorRPM()); + } + + public void stopLeftShooterMotorRPM() { + leftShooterMotor.set(0); + } + public Command stopLeftShooterCommand() { + return runOnce(() -> stopLeftShooterMotorRPM()); + } + + public void stopCenterShooterMotorRPM() { + centerShooterMotor.set(0); + } + public Command stopCenterShooterCommand() { + return runOnce(() -> stopCenterShooterMotorRPM()); + } + + public void stopRightShooterMotorRPM() { + rightShooterMotor.set(0); + } + public Command stopRightShooterCommand() { + return runOnce(() -> stopRightShooterMotorRPM()); + } + + public double getShooterMotorRPM() { return rightShooterMotor.getEncoder().getVelocity(); } @@ -107,11 +151,25 @@ public class ShooterSubsystem extends SubsystemBase { indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM, ControlType.kVelocity); } - /* public Command shootFuelCommand() { - return run(() -> setShooterMotorsRPM()) - .until(() -> { - return (getShooterMotorRPM() >= Constants.ShooterConstants.SHOOTER_RPM); - }) + public void reverseIndexerAndRampMotorRPM() { + indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1, ControlType.kVelocity); + } + + public Command reverseIndexerAndRampMotorRPMCommand() { + return runOnce(() -> reverseIndexerAndRampMotorRPM()); + } + + public Command setIndexerAndRampMotorRPMCommand() { + return runOnce(() -> setIndexerAndRampMotorRPM()); + } + + public Command stopIndexerAndRampMotorCommand() { + return runOnce(()-> indexerAndRampMotor.set(0)); + } + + /*public Command shootFuelCommand() { + return runOnce(() -> setShooterMotorsRPM()) + .until(() -> {return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM * 0.9);}) .andThen(() -> setIndexerAndRampMotorRPM()); } */ @@ -125,7 +183,7 @@ public class ShooterSubsystem extends SubsystemBase { public void stopShooters() { centerShooterMotor.set(0); - //leftShooterMotor.set(0); + leftShooterMotor.set(0); rightShooterMotor.set(0); indexerAndRampMotor.set(0); @@ -159,6 +217,8 @@ public class ShooterSubsystem extends SubsystemBase { @Override public void periodic() { + + SmartDashboard.putNumber("Shooter Velocity", leftShooterMotor.getEncoder().getVelocity()); /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get()); SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get()); currentPotentiometerPosition = leftPotentiometer.get(); */ diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java index bf6fca5..330a444 100644 --- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java +++ b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java @@ -171,16 +171,21 @@ public class TargetingSubsystems extends SubsystemBase { * } */ - public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, - SwerveSubsystem swerveDrive) { - Optional result = poseEstimator.update(camera.getLatestResult()); + public static Optional visionEst = Optional.empty(); - if (result.isPresent()) { - EstimatedRobotPose estimatedPose = result.get(); + public static void updateRobotPose(PhotonCamera camera, PhotonPoseEstimator poseEstimator, SwerveSubsystem swerveDrive) { + /* for (var result : camera.getAllUnreadResults()) { + visionEst = poseEstimator.estimateCoprocMultiTagPose(result); + + if (visionEst.isPresent()) { + EstimatedRobotPose estimatedPose = visionEst.get(); swerveDrive.getSwerveDrive() .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds); } - } + + + }*/ +} @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index d677507..bb15541 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -146,7 +146,7 @@ public class SwerveSubsystem extends SubsystemBase { // When vision is enabled we must manually update odometry in SwerveDrive if (visionDriveTest) { swerveDrive.updateOdometry(); - vision.updatePoseEstimation(swerveDrive); + //vision.updatePoseEstimation(swerveDrive); } }