From 0c0579dd01e574268cb197451b23b9a2b9eacbc4 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:42:45 -0400 Subject: [PATCH 01/25] CAN Remappings --- src/main/java/frc/robot/config/CANMappings.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/config/CANMappings.java b/src/main/java/frc/robot/config/CANMappings.java index 5659ca6..4c23b3d 100644 --- a/src/main/java/frc/robot/config/CANMappings.java +++ b/src/main/java/frc/robot/config/CANMappings.java @@ -3,13 +3,13 @@ public class CANMappings { public static final int HOPPER_MOTOR_ID = 1; - public static final int KICKER_FRONT_MOTOR_ID = 15; // Bottom - public static final int KICKER_BACK_MOTOR_ID = 16; + public static final int KICKER_RIGHT_MOTOR_ID = 16; // Bottom + public static final int KICKER_LEFT_MOTOR_ID = 2; - public static final int INTAKE_MOTOR_ID = 3; + public static final int INTAKE_MOTOR_ID = 19; - public static final int FLYWHEEL_RIGHT_MOTOR_ID = 19; - public static final int FLYWHEEL_LEFT_MOTOR_ID = 2; + public static final int FLYWHEEL_RIGHT_MOTOR_ID = 15; + public static final int FLYWHEEL_LEFT_MOTOR_ID = 3; public static final int PIGEON_CAN_ID = 0; } From b3eabaa82a3e5043105fa44d0fdedf31b9cd9612 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:43:39 -0400 Subject: [PATCH 02/25] Comment out Vision Add Pose Reset --- .../subsystems/CommandSwerveDrivetrain.java | 111 ++++++++++-------- 1 file changed, 61 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 9fa0962..528c0e1 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -31,6 +31,8 @@ import frc.robot.config.VisionConfig; import java.util.Optional; import java.util.function.Supplier; + +import frc.robot.helpers.ShotCalculator; import org.photonvision.PhotonPoseEstimator; /** @@ -301,58 +303,58 @@ public void periodic() { }); } // 1. Get all results since the last loop and apply if estimator is available - if (m_FrontPhotonPoseEstimator != null) { - var frontResults = Vision.FrontCameraApril.getAllUnreadResults(); - - for (var result : frontResults) { - // 2. Use the 2026 explicit methods to calculate pose - var visionEst = m_FrontPhotonPoseEstimator.estimateCoprocMultiTagPose(result); - - // Fallback to single tag if multi-tag isn't available - if (visionEst.isEmpty()) { - var bestTarget = result.getBestTarget(); - if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { - visionEst = m_FrontPhotonPoseEstimator.estimateLowestAmbiguityPose(result); - } - } - - // 3. Apply the successful estimation to the CTRE odometry and log to file - visionEst.ifPresent( - est -> { - Pose2d pose = est.estimatedPose.toPose2d(); - double ts = est.timestampSeconds; - SignalLogger.writeStruct("Vision/Front/Pose", Pose2d.struct, pose); - SignalLogger.writeDouble("Vision/Front/Timestamp", ts, "seconds"); - addVisionMeasurement(pose, ts, VisionConfig.FRONT_VISION_STDDEVS); - }); - } - } - if (m_RearPhotonPoseEstimator != null) { - var rearResults = Vision.RearCameraApril.getAllUnreadResults(); - - for (var result : rearResults) { - // 2. Use the 2026 explicit methods to calculate pose - var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result); +// if (m_FrontPhotonPoseEstimator != null) { +// var frontResults = Vision.FrontCameraApril.getAllUnreadResults(); +// +// for (var result : frontResults) { +// // 2. Use the 2026 explicit methods to calculate pose +// var visionEst = m_FrontPhotonPoseEstimator.estimateCoprocMultiTagPose(result); +// +// // Fallback to single tag if multi-tag isn't available +// if (visionEst.isEmpty()) { +// var bestTarget = result.getBestTarget(); +// if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { +// visionEst = m_FrontPhotonPoseEstimator.estimateLowestAmbiguityPose(result); +// } +// } - // Fallback to single tag if multi-tag isn't available - if (visionEst.isEmpty()) { - var bestTarget = result.getBestTarget(); - if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { - visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result); - } - } +// // 3. Apply the successful estimation to the CTRE odometry and log to file +// visionEst.ifPresent( +// est -> { +// Pose2d pose = est.estimatedPose.toPose2d(); +// double ts = est.timestampSeconds; +// SignalLogger.writeStruct("Vision/Front/Pose", Pose2d.struct, pose); +// SignalLogger.writeDouble("Vision/Front/Timestamp", ts, "seconds"); +// addVisionMeasurement(pose, ts, VisionConfig.FRONT_VISION_STDDEVS); +// }); +// } +// } +// if (m_RearPhotonPoseEstimator != null) { +// var rearResults = Vision.RearCameraApril.getAllUnreadResults(); +// +// for (var result : rearResults) { +// // 2. Use the 2026 explicit methods to calculate pose +// var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result); +// +// // Fallback to single tag if multi-tag isn't available +// if (visionEst.isEmpty()) { +// var bestTarget = result.getBestTarget(); +// if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { +// visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result); +// } +// } - // 3. Apply the successful estimation to the CTRE odometry and log to file - visionEst.ifPresent( - est -> { - Pose2d pose = est.estimatedPose.toPose2d(); - double ts = est.timestampSeconds; - SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose); - SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds"); - addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS); - }); - } - } +// // 3. Apply the successful estimation to the CTRE odometry and log to file +// visionEst.ifPresent( +// est -> { +// Pose2d pose = est.estimatedPose.toPose2d(); +// double ts = est.timestampSeconds; +// SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose); +// SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds"); +// addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS); +// }); + //} + //} } private void startSimThread() { @@ -416,4 +418,13 @@ public void addVisionMeasurement( public Optional samplePoseAt(double timestampSeconds) { return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); } + + public void zeroPigeon() { + this.resetRotation(ShotCalculator.calculatePigeonZero()); + } + + public void setPose(Pose2d pose) { + this.resetPose(pose); + } + ; } From ef28ad1124b57e259dfb11a24da862229aceca8e Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:44:02 -0400 Subject: [PATCH 03/25] Flywheel Updates --- src/main/java/frc/robot/subsystems/Flywheel.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index 9c8d2e6..034b2f8 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -1,8 +1,8 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -76,14 +76,14 @@ public Flywheel() { public void shoot(double speed) { speed = Math.abs(speed); - flywheelRight.setControl(new MotionMagicVelocityVoltage(speed)); - flywheelLeft.setControl(new MotionMagicVelocityVoltage(-speed)); + flywheelRight.setControl(new MotionMagicVelocityVoltage(-speed)); + flywheelLeft.setControl(new MotionMagicVelocityVoltage(speed)); } public void outtake(double speed) { speed = Math.abs(speed); - flywheelRight.setControl(new DutyCycleOut(-speed)); - flywheelLeft.setControl(new DutyCycleOut(speed)); + flywheelRight.setControl(new VoltageOut(speed)); + flywheelLeft.setControl(new VoltageOut(-speed)); } public void stop() { From 8053126e0375b6fc2f7718efc2d695b5feb12060 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:44:28 -0400 Subject: [PATCH 04/25] Add Pass --- src/main/java/frc/robot/commands/FlywheelCommand.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/FlywheelCommand.java b/src/main/java/frc/robot/commands/FlywheelCommand.java index 3198ba2..fd07815 100644 --- a/src/main/java/frc/robot/commands/FlywheelCommand.java +++ b/src/main/java/frc/robot/commands/FlywheelCommand.java @@ -15,7 +15,8 @@ public static enum Position { TRENCH_SHOT, CALCULATED_SHOT, DEFAULT_SHOT, - OUTTAKE + OUTTAKE, + PASS } private Flywheel subsystem; @@ -40,6 +41,11 @@ public void execute() { flywheelState = "Hub Shot"; break; + case PASS: + subsystem.shoot(FlywheelConfig.FLYWHEEL_PASS_SPEED); + flywheelState = "Pass"; + break; + // Spins flywheels at proper speed for shooting from tower case TOWER_SHOT: subsystem.shoot(FlywheelConfig.FLYWHEEL_TOWER_SHOT_SPEED); From cdec0a3a0906ee339109f15697ddfc4d644d92e9 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:44:40 -0400 Subject: [PATCH 05/25] Add Pass Speed --- src/main/java/frc/robot/config/FlywheelConfig.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/config/FlywheelConfig.java b/src/main/java/frc/robot/config/FlywheelConfig.java index 834d2fc..aef3e20 100644 --- a/src/main/java/frc/robot/config/FlywheelConfig.java +++ b/src/main/java/frc/robot/config/FlywheelConfig.java @@ -33,6 +33,7 @@ public class FlywheelConfig { public static final double FLYWHEEL_HUB_SHOT_SPEED = 43; public static final double FLYWHEEL_TOWER_SHOT_SPEED = 65; public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 67; + public static final double FLYWHEEL_PASS_SPEED = 0; public static final double FLYWHEEL_OUTTAKE_SPEED = 0; public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 40; From bcc5d4984871c2742c7399bf3f9f953c91260e05 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:45:36 -0400 Subject: [PATCH 06/25] Add Gyro Yaw Log --- src/main/java/frc/robot/Robot.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 93db8a3..6eec04a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -47,6 +47,7 @@ public void robotPeriodic() { SmartDashboard.putString("Hopper State", Hopper.hopperState); SmartDashboard.putString("Intake State", Intake.intakeState); SmartDashboard.putString("Kicker State", KickerCommand.kickerState); + SmartDashboard.putNumber("Gyro Yaw", drivetrain.getPigeon2().getRotation2d().getDegrees()); SmartDashboard.putBoolean( "Flywheel On", !(FlywheelCommand.flywheelState.equals("none") From 5e26b463ac167c891b1005c41065c7d96ab97739 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:46:49 -0400 Subject: [PATCH 07/25] Add Operator Controller Add DriveState Fix Pigeon Zero (hopefully) Remove SOTM Add Operator Resets --- src/main/java/frc/robot/RobotContainer.java | 31 +++++++++++---------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca31d9c..288c765 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathCommand; @@ -18,6 +19,7 @@ import frc.robot.config.HopperConfig; import frc.robot.config.IntakeConfig; import frc.robot.config.TunerConstants; +import frc.robot.helpers.ShotCalculator; import frc.robot.subsystems.*; @Logged @@ -28,8 +30,10 @@ public class RobotContainer { private Intake intake = new Intake(); private Kicker kicker = new Kicker(); private CommandXboxController controller = new CommandXboxController(1); + private CommandXboxController operator = new CommandXboxController(2); private final SendableChooser autoChooser; public static boolean isExtended = false; + private SwerveDrivetrain.SwerveDriveState driveState = drivetrain.getState(); private Command shootGroup = Commands.parallel(new KickerCommand(kicker, KickerCommand.Position.INTAKE)) @@ -186,7 +190,7 @@ private void configureBindings() { .withDeadline(Commands.waitSeconds(2)) .andThen(Commands.runOnce(() -> isExtended = !isExtended))); // Back button = Zero Pigeon - controller.back().onTrue(Commands.runOnce(drivetrain::seedFieldCentric)); + controller.back().onTrue(Commands.runOnce(drivetrain::zeroPigeon)); // Right Bumper = Flywheel Toggle for hub shot speeds controller .rightBumper() @@ -203,19 +207,18 @@ private void configureBindings() { new FlywheelCommand(flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain)); // Down Plus = Zero hopper controller.povDown().onTrue(Commands.runOnce(() -> hopper.zero(), hopper)); - // B = Shoot on the move toggle - placeholder button - controller - .b() - .toggleOnTrue( - Commands.parallel( - new DrivetrainCommand( - drivetrain, - DrivetrainCommand.Position.SOTM, - controller::getLeftX, - controller::getLeftY, - controller::getRightX), - new FlywheelCommand( - flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain))); + + /* Operator */ + operator + .leftBumper() + .onTrue( + Commands.runOnce( + () -> drivetrain.setPose(ShotCalculator.calculateLeftCornerRobotPosition()))); + operator + .rightBumper() + .onTrue( + Commands.runOnce( + () -> drivetrain.setPose(ShotCalculator.calculateRightCornerRobotPosition()))); } public Command getAutonomousCommand() { From 7fb872545bd87e2d73f43e506102fa95bbe3f9b8 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:47:40 -0400 Subject: [PATCH 08/25] Calculate Left Corner Robot Position Calculate Right Corner Robot Position Calculate Pigeon Zero --- .../frc/robot/helpers/ShotCalculator.java | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/main/java/frc/robot/helpers/ShotCalculator.java b/src/main/java/frc/robot/helpers/ShotCalculator.java index b6f85b9..c2a970e 100644 --- a/src/main/java/frc/robot/helpers/ShotCalculator.java +++ b/src/main/java/frc/robot/helpers/ShotCalculator.java @@ -1,5 +1,6 @@ package frc.robot.helpers; +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.interpolation.InterpolatingDoubleTreeMap; @@ -48,6 +49,42 @@ public static Translation2d calculateHubPosition() { return new Translation2d(Units.inchesToMeters(182.11), Units.inchesToMeters(158.84)); } + public static Pose2d calculateLeftCornerRobotPosition() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + if (alliance.get() == DriverStation.Alliance.Blue) { + return new Pose2d(new Translation2d(0.6096, 7.5692), Rotation2d.kZero); + } else if (alliance.get() == DriverStation.Alliance.Red) { + return new Pose2d(new Translation2d(15.9258, 0.4826), Rotation2d.k180deg); + } + } + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.kZero); + } + + public static Pose2d calculateRightCornerRobotPosition() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + if (alliance.get() == DriverStation.Alliance.Blue) { + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.kZero); + } else if (alliance.get() == DriverStation.Alliance.Red) { + return new Pose2d(new Translation2d(15.9258, 7.5692), Rotation2d.k180deg); + } + } + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.kZero); + } + + public static Rotation2d calculatePigeonZero() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + if (alliance.get() == DriverStation.Alliance.Blue) { + return Rotation2d.kZero; + } else if (alliance.get() == DriverStation.Alliance.Red) { + return Rotation2d.k180deg; + } + } + return Rotation2d.kZero; + } + public static Rotation2d getRotationTowardsHub(Translation2d hubpose, Translation2d currentpose) { double deltaX = hubpose.getX() - currentpose.getX(); double deltaY = hubpose.getY() - currentpose.getY(); From 997c562537e46b992cd3841547b7f68e4ace06d0 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:48:18 -0400 Subject: [PATCH 09/25] Drivetrain Logger (doesn't work, committed for future use) --- .../robot/loggers/SwerveDrivetrainLogger.java | 82 +++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 src/main/java/frc/robot/loggers/SwerveDrivetrainLogger.java diff --git a/src/main/java/frc/robot/loggers/SwerveDrivetrainLogger.java b/src/main/java/frc/robot/loggers/SwerveDrivetrainLogger.java new file mode 100644 index 0000000..3725d82 --- /dev/null +++ b/src/main/java/frc/robot/loggers/SwerveDrivetrainLogger.java @@ -0,0 +1,82 @@ +package frc.robot.loggers; + +import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.RobotController; + +@CustomLoggerFor(SwerveDrivetrain.class) +public class SwerveDrivetrainLogger extends ClassSpecificLogger { + public SwerveDrivetrainLogger() { + super(SwerveDrivetrain.class); + } + + String[] moduleNames = {"FL", "FR", "BL", "BR"}; + + @Override + protected void update(EpilogueBackend backend, SwerveDrivetrain drivetrain) { + backend.log("Pose", drivetrain.getState().Pose, Pose2d.struct); + backend.log("Speeds", drivetrain.getState().Speeds, ChassisSpeeds.struct); + backend.log("ModuleStates", drivetrain.getState().ModuleStates, SwerveModuleState.struct); + backend.log("ModuleTargets", drivetrain.getState().ModuleTargets, SwerveModuleState.struct); + backend.log( + "ModulePositions", drivetrain.getState().ModulePositions, SwerveModulePosition.struct); + backend.log("RawHeading", drivetrain.getState().RawHeading, Rotation2d.struct); + backend.log("Timestamp", drivetrain.getState().Timestamp); + backend.log("OdometryPeriod", drivetrain.getState().OdometryPeriod); + backend.log("SuccessfulDaqs", drivetrain.getState().SuccessfulDaqs); + backend.log("FailedDaqs", drivetrain.getState().FailedDaqs); + backend.log("Pigeon Yaw", drivetrain.getPigeon2().getYaw().getValue()); + backend.log( + "Pigeon Angular Velocity Z World", + drivetrain.getPigeon2().getAngularVelocityZWorld().getValue()); + backend.log("Pigeon Supply Voltage", drivetrain.getPigeon2().getSupplyVoltage().getValue()); + backend.log( + "Operator Forward Direction", drivetrain.getOperatorForwardDirection(), Rotation2d.struct); + backend.log("Battery Voltage", RobotController.getBatteryVoltage()); + var modules = drivetrain.getModules(); + + for (int i = 0; i < modules.length; i++) { + var module = modules[i]; + backend.log( + moduleNames[i] + " Drive Supply Current", + module.getDriveMotor().getSupplyCurrent().getValue()); + backend.log( + moduleNames[i] + " Steer Supply Current", + module.getSteerMotor().getSupplyCurrent().getValue()); + backend.log( + moduleNames[i] + " Drive Stator Current", + module.getDriveMotor().getStatorCurrent().getValue()); + backend.log( + moduleNames[i] + " Steer Stator Current", + module.getSteerMotor().getStatorCurrent().getValue()); + backend.log( + moduleNames[i] + " Drive Supply Voltage", + module.getDriveMotor().getSupplyVoltage().getValue()); + backend.log( + moduleNames[i] + " Steer Supply Voltage", + module.getSteerMotor().getSupplyVoltage().getValue()); + backend.log( + moduleNames[i] + " Drive Closed Loop Error", + module.getDriveMotor().getClosedLoopError().getValue()); + backend.log( + moduleNames[i] + " Steer Closed Loop Error", + module.getSteerMotor().getClosedLoopError().getValue()); + } + + double totalDriveSupply = 0; + double totalSteerSupply = 0; + + for (var module : modules) { + totalDriveSupply += module.getDriveMotor().getSupplyCurrent().getValueAsDouble(); + totalSteerSupply += module.getSteerMotor().getSupplyCurrent().getValueAsDouble(); + } + backend.log("Total Supply Current", totalDriveSupply + totalSteerSupply); + } +} From fdf51126d5302c3fa0ac9c58ff14f4c763a817e4 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:48:41 -0400 Subject: [PATCH 10/25] Comment Out Vision --- src/main/java/frc/robot/subsystems/Vision.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 7918de2..dbe7b97 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -5,8 +5,8 @@ import org.photonvision.PhotonCamera; public class Vision extends SubsystemBase { - public static final PhotonCamera FrontCameraApril = - new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME); - public static final PhotonCamera RearCameraApril = - new PhotonCamera(VisionConfig.REAR_CAMERA_NAME); +// public static final PhotonCamera FrontCameraApril = +// new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME); +// public static final PhotonCamera RearCameraApril = +// new PhotonCamera(VisionConfig.REAR_CAMERA_NAME); } From 44dc3e93367f22bab92eda6a8131306bdff85322 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:50:05 -0400 Subject: [PATCH 11/25] Make Changes According to Shooter Changes --- .../java/frc/robot/config/KickerConfig.java | 20 ++-- .../java/frc/robot/subsystems/Kicker.java | 106 +++++++----------- 2 files changed, 52 insertions(+), 74 deletions(-) diff --git a/src/main/java/frc/robot/config/KickerConfig.java b/src/main/java/frc/robot/config/KickerConfig.java index 470957f..b3aa279 100644 --- a/src/main/java/frc/robot/config/KickerConfig.java +++ b/src/main/java/frc/robot/config/KickerConfig.java @@ -1,12 +1,12 @@ package frc.robot.config; public class KickerConfig { - public static final double KICKER_FRONT_STATOR_CURRENT_LIMIT = 60; - public static final double KICKER_FRONT_SUPPLY_CURRENT_LIMIT = 50; - public static final double KICKER_FRONT_GEAR_RATIO = 3; - public static final double KICKER_BACK_STATOR_CURRENT_LIMIT = 60; - public static final double KICKER_BACK_SUPPLY_CURRENT_LIMIT = 50; - public static final double KICKER_BACK_GEAR_RATIO = 3; + public static final double KICKER_RIGHT_STATOR_CURRENT_LIMIT = 60; + public static final double KICKER_RIGHT_SUPPLY_CURRENT_LIMIT = 50; + public static final double KICKER_RIGHT_GEAR_RATIO = 3; + public static final double KICKER_LEFT_STATOR_CURRENT_LIMIT = 60; + public static final double KICKER_LEFT_SUPPLY_CURRENT_LIMIT = 50; + public static final double KICKER_LEFT_GEAR_RATIO = 3; // Set public static final double KICKER_P = 0; @@ -19,9 +19,9 @@ public class KickerConfig { public static final double KICKER_INTAKE_SPEED = 11; public static final double KICKER_OUTTAKE_SPEED = 11; - public static final double KICKER_FRONT_INTAKE_SPEED = 0; - public static final double KICKER_FRONT_OUTTAKE_SPEED = 0; + public static final double KICKER_RIGHT_INTAKE_SPEED = 0; + public static final double KICKER_RIGHT_OUTTAKE_SPEED = 0; - public static final double KICKER_BACK_INTAKE_SPEED = 0; - public static final double KICKER_BACK_OUTTAKE_SPEED = 0; + public static final double KICKER_LEFT_INTAKE_SPEED = 0; + public static final double KICKER_LEFT_OUTTAKE_SPEED = 0; } diff --git a/src/main/java/frc/robot/subsystems/Kicker.java b/src/main/java/frc/robot/subsystems/Kicker.java index e9c97b5..e4072e5 100644 --- a/src/main/java/frc/robot/subsystems/Kicker.java +++ b/src/main/java/frc/robot/subsystems/Kicker.java @@ -1,8 +1,6 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -14,88 +12,68 @@ @Logged public class Kicker extends SubsystemBase { - protected TalonFX kickerFront; - protected TalonFX kickerBack; + protected TalonFX kickerRight; + protected TalonFX kickerLeft; public Kicker() { - kickerFront = new TalonFX(CANMappings.KICKER_FRONT_MOTOR_ID); - kickerBack = new TalonFX(CANMappings.KICKER_BACK_MOTOR_ID); - TalonFXConfiguration kickerFrontConfig = new TalonFXConfiguration(); - TalonFXConfiguration kickerBackConfig = new TalonFXConfiguration(); + kickerRight = new TalonFX(CANMappings.KICKER_RIGHT_MOTOR_ID); + kickerLeft = new TalonFX(CANMappings.KICKER_LEFT_MOTOR_ID); + TalonFXConfiguration kickerRightConfig = new TalonFXConfiguration(); + TalonFXConfiguration kickerLeftConfig = new TalonFXConfiguration(); - kickerFrontConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - kickerFrontConfig.CurrentLimits.StatorCurrentLimitEnable = true; - kickerBackConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - kickerBackConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kickerRightConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + kickerRightConfig.CurrentLimits.StatorCurrentLimitEnable = true; + kickerLeftConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + kickerLeftConfig.CurrentLimits.StatorCurrentLimitEnable = true; - kickerFrontConfig.CurrentLimits.SupplyCurrentLimit = - KickerConfig.KICKER_FRONT_SUPPLY_CURRENT_LIMIT; - kickerFrontConfig.CurrentLimits.StatorCurrentLimit = - KickerConfig.KICKER_FRONT_STATOR_CURRENT_LIMIT; - kickerBackConfig.CurrentLimits.SupplyCurrentLimit = - KickerConfig.KICKER_BACK_SUPPLY_CURRENT_LIMIT; - kickerBackConfig.CurrentLimits.StatorCurrentLimit = - KickerConfig.KICKER_BACK_STATOR_CURRENT_LIMIT; + kickerRightConfig.CurrentLimits.SupplyCurrentLimit = + KickerConfig.KICKER_RIGHT_SUPPLY_CURRENT_LIMIT; + kickerRightConfig.CurrentLimits.StatorCurrentLimit = + KickerConfig.KICKER_RIGHT_STATOR_CURRENT_LIMIT; + kickerLeftConfig.CurrentLimits.SupplyCurrentLimit = + KickerConfig.KICKER_LEFT_SUPPLY_CURRENT_LIMIT; + kickerLeftConfig.CurrentLimits.StatorCurrentLimit = + KickerConfig.KICKER_LEFT_STATOR_CURRENT_LIMIT; - kickerFrontConfig.Slot0.kP = KickerConfig.KICKER_P; - kickerFrontConfig.Slot0.kI = KickerConfig.KICKER_I; - kickerFrontConfig.Slot0.kD = KickerConfig.KICKER_D; - kickerFrontConfig.Slot0.kS = KickerConfig.KICKER_S; - kickerFrontConfig.Slot0.kV = KickerConfig.KICKER_V; - kickerFrontConfig.Slot0.kA = KickerConfig.KICKER_A; + kickerRightConfig.Feedback.SensorToMechanismRatio = KickerConfig.KICKER_RIGHT_GEAR_RATIO; + kickerRightConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + kickerRightConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + kickerLeftConfig.Feedback.SensorToMechanismRatio = KickerConfig.KICKER_LEFT_GEAR_RATIO; + kickerLeftConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + kickerLeftConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - kickerFrontConfig.Feedback.SensorToMechanismRatio = KickerConfig.KICKER_FRONT_GEAR_RATIO; - kickerFrontConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - kickerFrontConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - kickerBackConfig.Feedback.SensorToMechanismRatio = KickerConfig.KICKER_BACK_GEAR_RATIO; - kickerBackConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - kickerBackConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - kickerFront.getConfigurator().apply(kickerFrontConfig); - kickerBack.getConfigurator().apply(kickerBackConfig); + kickerRight.getConfigurator().apply(kickerRightConfig); + kickerLeft.getConfigurator().apply(kickerLeftConfig); } public void intake(double speed) { speed = Math.abs(speed); - kickerFront.setControl(new VoltageOut(-speed)); - kickerBack.setControl(new VoltageOut(speed)); - } - - public void intake(double frontSpeed, double backSpeed) { - frontSpeed = Math.abs(frontSpeed); - backSpeed = Math.abs(backSpeed); - kickerFront.setControl(new MotionMagicVelocityVoltage(-frontSpeed)); - kickerBack.setControl(new MotionMagicVelocityVoltage(backSpeed)); - } - - public void intakeDutyCycleOut(double speed) { - speed = Math.abs(speed); - kickerFront.setControl(new DutyCycleOut(-speed)); - kickerBack.setControl(new DutyCycleOut(speed)); + kickerRight.setControl(new VoltageOut(speed)); + kickerLeft.setControl(new VoltageOut(-speed)); } - public void intakeDutyCycleOut(double frontSpeed, double backSpeed) { - frontSpeed = Math.abs(frontSpeed); - backSpeed = Math.abs(backSpeed); - kickerFront.setControl(new DutyCycleOut(-frontSpeed)); - kickerBack.setControl(new DutyCycleOut(backSpeed)); + public void intake(double rightSpeed, double leftSpeed) { + rightSpeed = Math.abs(rightSpeed); + leftSpeed = Math.abs(leftSpeed); + kickerRight.setControl(new VoltageOut(rightSpeed)); + kickerLeft.setControl(new VoltageOut(-leftSpeed)); } public void outtake(double speed) { speed = Math.abs(speed); - kickerFront.setControl(new DutyCycleOut(speed)); - kickerBack.setControl(new DutyCycleOut(-speed)); + kickerRight.setControl(new VoltageOut(-speed)); + kickerLeft.setControl(new VoltageOut(speed)); } - public void outtake(double frontSpeed, double backSpeed) { - frontSpeed = Math.abs(frontSpeed); - backSpeed = Math.abs(backSpeed); - kickerFront.setControl(new DutyCycleOut(frontSpeed)); - kickerBack.setControl(new DutyCycleOut(-backSpeed)); + public void outtake(double rightSpeed, double leftSpeed) { + rightSpeed = Math.abs(rightSpeed); + leftSpeed = Math.abs(leftSpeed); + kickerRight.setControl(new VoltageOut(-rightSpeed)); + kickerLeft.setControl(new VoltageOut(leftSpeed)); } public void stop() { - kickerFront.stopMotor(); - kickerBack.stopMotor(); + kickerRight.stopMotor(); + kickerLeft.stopMotor(); } } From 8b845618b2a99688d405b6e847ce4b8007018047 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:51:33 -0400 Subject: [PATCH 12/25] Drive Remove Vision --- .../subsystems/CommandSwerveDrivetrain.java | 103 +++++++++--------- 1 file changed, 51 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 528c0e1..b386970 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -29,10 +29,9 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.config.TunerConstants.TunerSwerveDrivetrain; import frc.robot.config.VisionConfig; +import frc.robot.helpers.ShotCalculator; import java.util.Optional; import java.util.function.Supplier; - -import frc.robot.helpers.ShotCalculator; import org.photonvision.PhotonPoseEstimator; /** @@ -303,58 +302,58 @@ public void periodic() { }); } // 1. Get all results since the last loop and apply if estimator is available -// if (m_FrontPhotonPoseEstimator != null) { -// var frontResults = Vision.FrontCameraApril.getAllUnreadResults(); -// -// for (var result : frontResults) { -// // 2. Use the 2026 explicit methods to calculate pose -// var visionEst = m_FrontPhotonPoseEstimator.estimateCoprocMultiTagPose(result); -// -// // Fallback to single tag if multi-tag isn't available -// if (visionEst.isEmpty()) { -// var bestTarget = result.getBestTarget(); -// if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { -// visionEst = m_FrontPhotonPoseEstimator.estimateLowestAmbiguityPose(result); -// } -// } + // if (m_FrontPhotonPoseEstimator != null) { + // var frontResults = Vision.FrontCameraApril.getAllUnreadResults(); + // + // for (var result : frontResults) { + // // 2. Use the 2026 explicit methods to calculate pose + // var visionEst = m_FrontPhotonPoseEstimator.estimateCoprocMultiTagPose(result); + // + // // Fallback to single tag if multi-tag isn't available + // if (visionEst.isEmpty()) { + // var bestTarget = result.getBestTarget(); + // if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { + // visionEst = m_FrontPhotonPoseEstimator.estimateLowestAmbiguityPose(result); + // } + // } -// // 3. Apply the successful estimation to the CTRE odometry and log to file -// visionEst.ifPresent( -// est -> { -// Pose2d pose = est.estimatedPose.toPose2d(); -// double ts = est.timestampSeconds; -// SignalLogger.writeStruct("Vision/Front/Pose", Pose2d.struct, pose); -// SignalLogger.writeDouble("Vision/Front/Timestamp", ts, "seconds"); -// addVisionMeasurement(pose, ts, VisionConfig.FRONT_VISION_STDDEVS); -// }); -// } -// } -// if (m_RearPhotonPoseEstimator != null) { -// var rearResults = Vision.RearCameraApril.getAllUnreadResults(); -// -// for (var result : rearResults) { -// // 2. Use the 2026 explicit methods to calculate pose -// var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result); -// -// // Fallback to single tag if multi-tag isn't available -// if (visionEst.isEmpty()) { -// var bestTarget = result.getBestTarget(); -// if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { -// visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result); -// } -// } + // // 3. Apply the successful estimation to the CTRE odometry and log to file + // visionEst.ifPresent( + // est -> { + // Pose2d pose = est.estimatedPose.toPose2d(); + // double ts = est.timestampSeconds; + // SignalLogger.writeStruct("Vision/Front/Pose", Pose2d.struct, pose); + // SignalLogger.writeDouble("Vision/Front/Timestamp", ts, "seconds"); + // addVisionMeasurement(pose, ts, VisionConfig.FRONT_VISION_STDDEVS); + // }); + // } + // } + // if (m_RearPhotonPoseEstimator != null) { + // var rearResults = Vision.RearCameraApril.getAllUnreadResults(); + // + // for (var result : rearResults) { + // // 2. Use the 2026 explicit methods to calculate pose + // var visionEst = m_RearPhotonPoseEstimator.estimateCoprocMultiTagPose(result); + // + // // Fallback to single tag if multi-tag isn't available + // if (visionEst.isEmpty()) { + // var bestTarget = result.getBestTarget(); + // if (bestTarget != null && bestTarget.getPoseAmbiguity() < 0.2) { + // visionEst = m_RearPhotonPoseEstimator.estimateLowestAmbiguityPose(result); + // } + // } -// // 3. Apply the successful estimation to the CTRE odometry and log to file -// visionEst.ifPresent( -// est -> { -// Pose2d pose = est.estimatedPose.toPose2d(); -// double ts = est.timestampSeconds; -// SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose); -// SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds"); -// addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS); -// }); - //} - //} + // // 3. Apply the successful estimation to the CTRE odometry and log to file + // visionEst.ifPresent( + // est -> { + // Pose2d pose = est.estimatedPose.toPose2d(); + // double ts = est.timestampSeconds; + // SignalLogger.writeStruct("Vision/Rear/Pose", Pose2d.struct, pose); + // SignalLogger.writeDouble("Vision/Rear/Timestamp", ts, "seconds"); + // addVisionMeasurement(pose, ts, VisionConfig.REAR_VISION_STDDEVS); + // }); + // } + // } } private void startSimThread() { From 3fc4c50980ea4db2de90bd5ac93e9d4831116d1b Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 9 Apr 2026 19:52:08 -0400 Subject: [PATCH 13/25] Spotless --- src/main/java/frc/robot/subsystems/Vision.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index dbe7b97..cc029f4 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -1,12 +1,10 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.config.VisionConfig; -import org.photonvision.PhotonCamera; public class Vision extends SubsystemBase { -// public static final PhotonCamera FrontCameraApril = -// new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME); -// public static final PhotonCamera RearCameraApril = -// new PhotonCamera(VisionConfig.REAR_CAMERA_NAME); + // public static final PhotonCamera FrontCameraApril = + // new PhotonCamera(VisionConfig.FRONT_CAMERA_NAME); + // public static final PhotonCamera RearCameraApril = + // new PhotonCamera(VisionConfig.REAR_CAMERA_NAME); } From 5f264c62f02475cc2f44575eb2a0e49ba511be75 Mon Sep 17 00:00:00 2001 From: Yaypixel Date: Fri, 10 Apr 2026 13:32:31 -0400 Subject: [PATCH 14/25] dcmp-1auto --- ...f Copy of center to back outpost side.auto | 94 +++++++++++++++++++ .../autos/center to back depot side.auto | 84 ++++++++++++++++- .../paths/1 Depot side center.path | 12 +-- .../paths/2 Center back depot.path | 10 +- .../pathplanner/paths/3 deopt side.path | 68 ++++++++++++++ .../pathplanner/paths/3 to middle depot.path | 21 +++-- src/main/deploy/pathplanner/paths/4 BACK.path | 54 +++++++++++ 7 files changed, 323 insertions(+), 20 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Copy of Copy of center to back outpost side.auto create mode 100644 src/main/deploy/pathplanner/paths/3 deopt side.path create mode 100644 src/main/deploy/pathplanner/paths/4 BACK.path diff --git a/src/main/deploy/pathplanner/autos/Copy of Copy of center to back outpost side.auto b/src/main/deploy/pathplanner/autos/Copy of Copy of center to back outpost side.auto new file mode 100644 index 0000000..633ebd4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of Copy of center to back outpost side.auto @@ -0,0 +1,94 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "idle" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "1 Depot side center" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "hopper deploy" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "2 Center back depot" + } + }, + { + "type": "named", + "data": { + "name": "rev shooter" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "hopper retract" + } + }, + { + "type": "named", + "data": { + "name": "shoot long" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "idle" + } + }, + { + "type": "path", + "data": { + "pathName": "3 to middle depot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/center to back depot side.auto b/src/main/deploy/pathplanner/autos/center to back depot side.auto index c136d35..34bc2df 100644 --- a/src/main/deploy/pathplanner/autos/center to back depot side.auto +++ b/src/main/deploy/pathplanner/autos/center to back depot side.auto @@ -92,10 +92,92 @@ "name": "idle" } }, + { + "type": "named", + "data": { + "name": "hopper deploy" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3 to middle depot" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "hopper deploy" + } + } + ] + } + } + ] + } + }, { "type": "path", "data": { - "pathName": "3 to middle depot" + "pathName": "4 BACK" + } + }, + { + "type": "named", + "data": { + "name": "rev shooter" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "hopper retract" + } + }, + { + "type": "named", + "data": { + "name": "shoot long" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "idle" } } ] diff --git a/src/main/deploy/pathplanner/paths/1 Depot side center.path b/src/main/deploy/pathplanner/paths/1 Depot side center.path index d07f9ee..dbfd989 100644 --- a/src/main/deploy/pathplanner/paths/1 Depot side center.path +++ b/src/main/deploy/pathplanner/paths/1 Depot side center.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.753707692307692, - "y": 5.793184615384616 + "x": 7.851106480189731, + "y": 5.075377139136905 }, "prevControl": { - "x": 7.802546153846153, - "y": 7.760676923076924 + "x": 7.899944941728192, + "y": 7.042869446829213 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.6025082236842106, - "rotationDegrees": -58.149754025069136 + "rotationDegrees": 118.78427882606655 } ], "constraintZones": [], @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -86.47765706868994 + "rotation": 86.14377074484123 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/2 Center back depot.path b/src/main/deploy/pathplanner/paths/2 Center back depot.path index f146a1c..f8d7227 100644 --- a/src/main/deploy/pathplanner/paths/2 Center back depot.path +++ b/src/main/deploy/pathplanner/paths/2 Center back depot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.649053846153848, - "y": 5.96063076923077 + "x": 7.983806857638889, + "y": 5.125840711805556 }, "prevControl": null, "nextControl": { - "x": 7.537423076923078, - "y": 7.593230769230768 + "x": 7.959846780443948, + "y": 7.401181404234872 }, "isLocked": false, "linkedName": null @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -90.07765154134242 + "rotation": 88.84965610304204 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 deopt side.path b/src/main/deploy/pathplanner/paths/3 deopt side.path new file mode 100644 index 0000000..32b5e00 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 deopt side.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5394806134259262, + "y": 7.305488208912037 + }, + "prevControl": null, + "nextControl": { + "x": 6.734254267939816, + "y": 7.836736689814815 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.227675347222223, + "y": 4.233865523726852 + }, + "prevControl": { + "x": 6.016475043402778, + "y": 7.055512080439814 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6592348421926909, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.50938390003228 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -61.449999218114584 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 to middle depot.path b/src/main/deploy/pathplanner/paths/3 to middle depot.path index 0f58d28..ecc511d 100644 --- a/src/main/deploy/pathplanner/paths/3 to middle depot.path +++ b/src/main/deploy/pathplanner/paths/3 to middle depot.path @@ -8,27 +8,32 @@ }, "prevControl": null, "nextControl": { - "x": 7.8697757523148155, - "y": 7.872494429976852 + "x": 8.846519748263889, + "y": 7.56884888599537 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.4424543547453705, - "y": 5.424040581597223 + "x": 7.944440538194444, + "y": 4.273100622106481 }, "prevControl": { - "x": 7.478799149635494, - "y": 5.17669657846718 + "x": 7.885784722222223, + "y": 6.953094039351852 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 88.7512237047608 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -42,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -61.20160855081938 + "rotation": 88.62549580029474 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/4 BACK.path b/src/main/deploy/pathplanner/paths/4 BACK.path new file mode 100644 index 0000000..1ad974b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/4 BACK.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.939126085069445, + "y": 4.265883463541667 + }, + "prevControl": null, + "nextControl": { + "x": 7.9645829716435195, + "y": 5.088836371527778 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9148384693287035, + "y": 7.351153139467592 + }, + "prevControl": { + "x": 7.125949146412037, + "y": 7.907136791087963 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -73.10245753971793 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 95.62030751363861 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 98c3ec9508145aeae13efa53993689163ef32a58 Mon Sep 17 00:00:00 2001 From: Yaypixel Date: Fri, 10 Apr 2026 13:44:05 -0400 Subject: [PATCH 15/25] PRELOAD AUTO --- src/main/java/frc/robot/RobotContainer.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca31d9c..152f8b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -219,7 +219,14 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return + new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain) + .withDeadline(Commands.waitSeconds(3)).andThen( + Commands.parallel( + new KickerCommand(kicker, KickerCommand.Position.INTAKE), + new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), + new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain)) + .withDeadline(Commands.waitSeconds(8))); } public CommandSwerveDrivetrain getDrivetrain() { From f0e5c66cf3d340f959d882a776f5a3c32a0f5c3a Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 10 Apr 2026 14:15:39 -0400 Subject: [PATCH 16/25] Configs --- .../java/frc/robot/config/FlywheelConfig.java | 18 +++++++++--------- .../java/frc/robot/config/HopperConfig.java | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/config/FlywheelConfig.java b/src/main/java/frc/robot/config/FlywheelConfig.java index aef3e20..de2fe3d 100644 --- a/src/main/java/frc/robot/config/FlywheelConfig.java +++ b/src/main/java/frc/robot/config/FlywheelConfig.java @@ -12,27 +12,27 @@ public class FlywheelConfig { // Set public static final double FLYWHEEL_RIGHT_MAX_CRUISE_VELOCITY = 3000; public static final double FLYWHEEL_RIGHT_TARGET_ACCELERATION = 500; - public static final double FLYWHEEL_RIGHT_P = 0.5; + public static final double FLYWHEEL_RIGHT_P = 0.25; public static final double FLYWHEEL_RIGHT_I = 0; public static final double FLYWHEEL_RIGHT_D = 0; - public static final double FLYWHEEL_RIGHT_S = 0.18; - public static final double FLYWHEEL_RIGHT_V = 0.115; + public static final double FLYWHEEL_RIGHT_S = 0.3; + public static final double FLYWHEEL_RIGHT_V = 0.123; public static final double FLYWHEEL_RIGHT_A = 0; // Set public static final double FLYWHEEL_LEFT_MAX_CRUISE_VELOCITY = 3000; public static final double FLYWHEEL_LEFT_TARGET_ACCELERATION = 500; - public static final double FLYWHEEL_LEFT_P = 0.5; + public static final double FLYWHEEL_LEFT_P = 0.25; public static final double FLYWHEEL_LEFT_I = 0; public static final double FLYWHEEL_LEFT_D = 0; - public static final double FLYWHEEL_LEFT_S = 0.18; - public static final double FLYWHEEL_LEFT_V = 0.115; + public static final double FLYWHEEL_LEFT_S = 0.3; + public static final double FLYWHEEL_LEFT_V = 123; public static final double FLYWHEEL_LEFT_A = 0; // Set - public static final double FLYWHEEL_HUB_SHOT_SPEED = 43; - public static final double FLYWHEEL_TOWER_SHOT_SPEED = 65; - public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 67; + public static final double FLYWHEEL_HUB_SHOT_SPEED = 30; + public static final double FLYWHEEL_TOWER_SHOT_SPEED = 20; + public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 20; public static final double FLYWHEEL_PASS_SPEED = 0; public static final double FLYWHEEL_OUTTAKE_SPEED = 0; diff --git a/src/main/java/frc/robot/config/HopperConfig.java b/src/main/java/frc/robot/config/HopperConfig.java index 2a1e41f..cc0ece1 100644 --- a/src/main/java/frc/robot/config/HopperConfig.java +++ b/src/main/java/frc/robot/config/HopperConfig.java @@ -22,7 +22,7 @@ public class HopperConfig { public static final double HOPPER_TOLERANCE = 0.2; // Set - public static final double HOPPER_EXTEND_ROTATION = -17; + public static final double HOPPER_EXTEND_ROTATION = -16.729; public static final double HOPPER_RETRACT_ROTATION = -0.1; public static final double HOPPER_RETRACT_FOR_NEUTRAL_MODE_ROTATION = 1.752; } From a7f052ad0090645214efe1862085c0a943d1116b Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 10 Apr 2026 23:17:19 -0400 Subject: [PATCH 17/25] Auto Fixes --- .../paths/1 Depot side center.path | 17 +++++--------- .../paths/2 Center back depot.path | 23 +++++++++++-------- .../pathplanner/paths/3 to middle depot.path | 4 ++-- src/main/deploy/pathplanner/paths/4 BACK.path | 12 +++++----- 4 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/1 Depot side center.path b/src/main/deploy/pathplanner/paths/1 Depot side center.path index dbfd989..7e1947a 100644 --- a/src/main/deploy/pathplanner/paths/1 Depot side center.path +++ b/src/main/deploy/pathplanner/paths/1 Depot side center.path @@ -16,24 +16,19 @@ }, { "anchor": { - "x": 7.851106480189731, - "y": 5.075377139136905 + "x": 7.87243987587277, + "y": 4.4607059736229635 }, "prevControl": { - "x": 7.899944941728192, - "y": 7.042869446829213 + "x": 7.921695112490301, + "y": 7.2964003103180755 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6025082236842106, - "rotationDegrees": 118.78427882606655 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -47,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 86.14377074484123 + "rotation": -93.09405805891707 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/2 Center back depot.path b/src/main/deploy/pathplanner/paths/2 Center back depot.path index f8d7227..dc6af98 100644 --- a/src/main/deploy/pathplanner/paths/2 Center back depot.path +++ b/src/main/deploy/pathplanner/paths/2 Center back depot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.983806857638889, - "y": 5.125840711805556 + "x": 7.9850232738557025, + "y": 4.650690457719163 }, "prevControl": null, "nextControl": { - "x": 7.959846780443948, - "y": 7.401181404234872 + "x": 8.006132660977503, + "y": 7.394910783553141 }, "isLocked": false, "linkedName": null @@ -20,15 +20,20 @@ "y": 7.456715595885093 }, "prevControl": { - "x": 4.350690457723854, - "y": 7.556749418150089 + "x": 7.54876260667184, + "y": 7.5989681923972086 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 87.27578727667913 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -42,13 +47,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -81.599031837346 + "rotation": 94.47880231914529 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 88.84965610304204 + "rotation": -87.20729763428666 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 to middle depot.path b/src/main/deploy/pathplanner/paths/3 to middle depot.path index ecc511d..19f0e70 100644 --- a/src/main/deploy/pathplanner/paths/3 to middle depot.path +++ b/src/main/deploy/pathplanner/paths/3 to middle depot.path @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 88.7512237047608 + "rotationDegrees": -91.90233699047563 } ], "constraintZones": [], @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 88.62549580029474 + "rotation": -89.22898482159874 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/4 BACK.path b/src/main/deploy/pathplanner/paths/4 BACK.path index 1ad974b..cbb4879 100644 --- a/src/main/deploy/pathplanner/paths/4 BACK.path +++ b/src/main/deploy/pathplanner/paths/4 BACK.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.9645829716435195, - "y": 5.088836371527778 + "x": 7.928731574864237, + "y": 7.929681923972071 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 7.351153139467592 }, "prevControl": { - "x": 7.125949146412037, - "y": 7.907136791087963 + "x": 8.139825446082234, + "y": 7.394910783553141 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -73.10245753971793 + "rotation": 91.0990137586245 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 95.62030751363861 + "rotation": -97.36924253129777 }, "useDefaultConstraints": true } \ No newline at end of file From ee164222d7862e3445d8bae7f2f0ee807233b612 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 10 Apr 2026 23:17:24 -0400 Subject: [PATCH 18/25] Auto Fixes --- ...f Copy of center to back outpost side.auto | 94 ------------------- 1 file changed, 94 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Copy of Copy of center to back outpost side.auto diff --git a/src/main/deploy/pathplanner/autos/Copy of Copy of center to back outpost side.auto b/src/main/deploy/pathplanner/autos/Copy of Copy of center to back outpost side.auto deleted file mode 100644 index 633ebd4..0000000 --- a/src/main/deploy/pathplanner/autos/Copy of Copy of center to back outpost side.auto +++ /dev/null @@ -1,94 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1 Depot side center" - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "hopper deploy" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "2 Center back depot" - } - }, - { - "type": "named", - "data": { - "name": "rev shooter" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "hopper retract" - } - }, - { - "type": "named", - "data": { - "name": "shoot long" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "path", - "data": { - "pathName": "3 to middle depot" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file From 1cdbc5b39ff8bf30ee245432cf7e18a1ab9cbbf7 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 10 Apr 2026 23:18:13 -0400 Subject: [PATCH 19/25] Flywheel Configs Day 1 --- src/main/java/frc/robot/config/FlywheelConfig.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/config/FlywheelConfig.java b/src/main/java/frc/robot/config/FlywheelConfig.java index de2fe3d..3cdd4e8 100644 --- a/src/main/java/frc/robot/config/FlywheelConfig.java +++ b/src/main/java/frc/robot/config/FlywheelConfig.java @@ -1,12 +1,12 @@ package frc.robot.config; public class FlywheelConfig { - public static final double FLYWHEEL_RIGHT_SUPPLY_CURRENT_LIMIT = 50; - public static final double FLYWHEEL_RIGHT_STATOR_CURRENT_LIMIT = 60; + public static final double FLYWHEEL_RIGHT_SUPPLY_CURRENT_LIMIT = 35; + public static final double FLYWHEEL_RIGHT_STATOR_CURRENT_LIMIT = 45; public static final double FLYWHEEL_RIGHT_GEAR_RATIO = 1; - public static final double FLYWHEEL_LEFT_SUPPLY_CURRENT_LIMIT = 50; - public static final double FLYWHEEL_LEFT_STATOR_CURRENT_LIMIT = 60; + public static final double FLYWHEEL_LEFT_SUPPLY_CURRENT_LIMIT = 35; + public static final double FLYWHEEL_LEFT_STATOR_CURRENT_LIMIT = 45; public static final double FLYWHEEL_LEFT_GEAR_RATIO = 1; // Set @@ -32,7 +32,7 @@ public class FlywheelConfig { // Set public static final double FLYWHEEL_HUB_SHOT_SPEED = 30; public static final double FLYWHEEL_TOWER_SHOT_SPEED = 20; - public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 20; + public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 25; public static final double FLYWHEEL_PASS_SPEED = 0; public static final double FLYWHEEL_OUTTAKE_SPEED = 0; From 6025ead32097f1dacfbeca491b600f5db9195a14 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 10 Apr 2026 23:18:36 -0400 Subject: [PATCH 20/25] Current Limits Set --- src/main/java/frc/robot/config/TunerConstants.java | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 3c37a44..9feb12e 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -59,7 +59,15 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(55)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(55)) + .withSupplyCurrentLimitEnable(true)); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( @@ -68,7 +76,9 @@ public class TunerConstants { // low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(20)) // from 60 - .withStatorCurrentLimitEnable(true)); + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(55)) + .withSupplyCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; From cc36d3d0f30a5ca3f343ca9c0302236ae42bb379 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 10 Apr 2026 23:19:20 -0400 Subject: [PATCH 21/25] (Commented out) Emergency Swerve Coast --- src/main/java/frc/robot/subsystems/Intake.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 293c6fb..7b52d42 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -13,12 +13,21 @@ @Logged public class Intake extends SubsystemBase { protected TalonFX intake; +// protected TalonFX swerveDrive; +// protected TalonFX swerveSteer; public static String intakeState = "Stopped"; public Intake() { intake = new TalonFX(CANMappings.INTAKE_MOTOR_ID); TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); +// swerveDrive = new TalonFX(31); +// swerveSteer = new TalonFX(30); +// TalonFXConfiguration swerveConfig = new TalonFXConfiguration(); +// swerveConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; +// swerveDrive.getConfigurator().apply(swerveConfig); +// swerveSteer.getConfigurator().apply(swerveConfig); + intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true; intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true; From f0da3f3e5f0bea2370901f100015a10ed0dfe5e1 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 10 Apr 2026 23:19:40 -0400 Subject: [PATCH 22/25] Auto Change --- src/main/java/frc/robot/RobotContainer.java | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 27ca569..288c765 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -222,14 +222,7 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return - new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain) - .withDeadline(Commands.waitSeconds(3)).andThen( - Commands.parallel( - new KickerCommand(kicker, KickerCommand.Position.INTAKE), - new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), - new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain)) - .withDeadline(Commands.waitSeconds(8))); + return autoChooser.getSelected(); } public CommandSwerveDrivetrain getDrivetrain() { From c7285213f0cd50606393c37020a12f192bd73d83 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Sat, 11 Apr 2026 09:01:21 -0400 Subject: [PATCH 23/25] Changes --- src/main/java/frc/robot/RobotContainer.java | 9 +++++++-- .../java/frc/robot/config/FlywheelConfig.java | 4 ++-- .../java/frc/robot/helpers/ShotCalculator.java | 17 +++++++++++++++++ src/main/java/frc/robot/subsystems/Intake.java | 16 ++++++++-------- 4 files changed, 34 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 288c765..16ba63f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -191,10 +191,10 @@ private void configureBindings() { .andThen(Commands.runOnce(() -> isExtended = !isExtended))); // Back button = Zero Pigeon controller.back().onTrue(Commands.runOnce(drivetrain::zeroPigeon)); - // Right Bumper = Flywheel Toggle for hub shot speeds + // Right Bumper = Flywheel Toggle for pass controller .rightBumper() - .toggleOnTrue(new FlywheelCommand(flywheel, FlywheelCommand.Position.HUB_SHOT, drivetrain)); + .toggleOnTrue(new FlywheelCommand(flywheel, FlywheelCommand.Position.PASS, drivetrain)); // Left Bumper = Flywheel Toggle for trench shot speeds controller .leftBumper() @@ -219,6 +219,11 @@ private void configureBindings() { .onTrue( Commands.runOnce( () -> drivetrain.setPose(ShotCalculator.calculateRightCornerRobotPosition()))); + operator + .y() + .onTrue( + Commands.runOnce( + () -> drivetrain.setPose(ShotCalculator.calculateRightCornerRobotPosition()))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/config/FlywheelConfig.java b/src/main/java/frc/robot/config/FlywheelConfig.java index 3cdd4e8..3032e97 100644 --- a/src/main/java/frc/robot/config/FlywheelConfig.java +++ b/src/main/java/frc/robot/config/FlywheelConfig.java @@ -26,14 +26,14 @@ public class FlywheelConfig { public static final double FLYWHEEL_LEFT_I = 0; public static final double FLYWHEEL_LEFT_D = 0; public static final double FLYWHEEL_LEFT_S = 0.3; - public static final double FLYWHEEL_LEFT_V = 123; + public static final double FLYWHEEL_LEFT_V = 0.123; public static final double FLYWHEEL_LEFT_A = 0; // Set public static final double FLYWHEEL_HUB_SHOT_SPEED = 30; public static final double FLYWHEEL_TOWER_SHOT_SPEED = 20; public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 25; - public static final double FLYWHEEL_PASS_SPEED = 0; + public static final double FLYWHEEL_PASS_SPEED = 50; public static final double FLYWHEEL_OUTTAKE_SPEED = 0; public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 40; diff --git a/src/main/java/frc/robot/helpers/ShotCalculator.java b/src/main/java/frc/robot/helpers/ShotCalculator.java index c2a970e..ee2a2f7 100644 --- a/src/main/java/frc/robot/helpers/ShotCalculator.java +++ b/src/main/java/frc/robot/helpers/ShotCalculator.java @@ -73,6 +73,23 @@ public static Pose2d calculateRightCornerRobotPosition() { return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.kZero); } + public static Pose2d calculateHubRobotPosition() { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + if (alliance.get() == DriverStation.Alliance.Blue) { + return new Pose2d( + new Translation2d(Units.inchesToMeters(189.36), (0.4826 + 7.5692) / 2), + Rotation2d.kZero); + } else if (alliance.get() == DriverStation.Alliance.Red) { + return new Pose2d( + new Translation2d(Units.inchesToMeters(461.84), (0.4826 + 7.5692) / 2), + Rotation2d.k180deg); + } + } + return new Pose2d( + new Translation2d(Units.inchesToMeters(189.36), (0.4826 + 7.5692) / 2), Rotation2d.kZero); + } + public static Rotation2d calculatePigeonZero() { Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 7b52d42..8f043e9 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -13,20 +13,20 @@ @Logged public class Intake extends SubsystemBase { protected TalonFX intake; -// protected TalonFX swerveDrive; -// protected TalonFX swerveSteer; + // protected TalonFX swerveDrive; + // protected TalonFX swerveSteer; public static String intakeState = "Stopped"; public Intake() { intake = new TalonFX(CANMappings.INTAKE_MOTOR_ID); TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); -// swerveDrive = new TalonFX(31); -// swerveSteer = new TalonFX(30); -// TalonFXConfiguration swerveConfig = new TalonFXConfiguration(); -// swerveConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; -// swerveDrive.getConfigurator().apply(swerveConfig); -// swerveSteer.getConfigurator().apply(swerveConfig); + // swerveDrive = new TalonFX(31); + // swerveSteer = new TalonFX(30); + // TalonFXConfiguration swerveConfig = new TalonFXConfiguration(); + // swerveConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; + // swerveDrive.getConfigurator().apply(swerveConfig); + // swerveSteer.getConfigurator().apply(swerveConfig); intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true; intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true; From 6ea91a9c13ba052ae3d53f6798d944f3dda8c183 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 29 Apr 2026 19:33:44 -0400 Subject: [PATCH 24/25] DCMP Changes --- .../Copy of center to back outpost side.auto | 94 --------- .../autos/center to back depot side.auto | 189 ------------------ .../paths/1 Depot side center.path | 8 +- .../paths/2 Center back depot.path | 24 ++- .../pathplanner/paths/3 to middle depot.path | 18 +- src/main/java/frc/robot/Robot.java | 5 + src/main/java/frc/robot/RobotContainer.java | 40 +++- .../frc/robot/commands/DrivetrainCommand.java | 2 +- .../frc/robot/commands/FlywheelCommand.java | 6 + .../java/frc/robot/config/FlywheelConfig.java | 9 +- .../java/frc/robot/config/KickerConfig.java | 4 +- .../frc/robot/helpers/ShotCalculator.java | 20 +- 12 files changed, 85 insertions(+), 334 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto delete mode 100644 src/main/deploy/pathplanner/autos/center to back depot side.auto diff --git a/src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto b/src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto deleted file mode 100644 index 633ebd4..0000000 --- a/src/main/deploy/pathplanner/autos/Copy of center to back outpost side.auto +++ /dev/null @@ -1,94 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1 Depot side center" - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "hopper deploy" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "2 Center back depot" - } - }, - { - "type": "named", - "data": { - "name": "rev shooter" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "hopper retract" - } - }, - { - "type": "named", - "data": { - "name": "shoot long" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "path", - "data": { - "pathName": "3 to middle depot" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/center to back depot side.auto b/src/main/deploy/pathplanner/autos/center to back depot side.auto deleted file mode 100644 index 34bc2df..0000000 --- a/src/main/deploy/pathplanner/autos/center to back depot side.auto +++ /dev/null @@ -1,189 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "1 Depot side center" - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "hopper deploy" - } - } - ] - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "2 Center back depot" - } - }, - { - "type": "named", - "data": { - "name": "rev shooter" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "hopper retract" - } - }, - { - "type": "named", - "data": { - "name": "shoot long" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "idle" - } - }, - { - "type": "named", - "data": { - "name": "hopper deploy" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "3 to middle depot" - } - }, - { - "type": "named", - "data": { - "name": "intake" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "hopper deploy" - } - } - ] - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "4 BACK" - } - }, - { - "type": "named", - "data": { - "name": "rev shooter" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "hopper retract" - } - }, - { - "type": "named", - "data": { - "name": "shoot long" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "idle" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1 Depot side center.path b/src/main/deploy/pathplanner/paths/1 Depot side center.path index 7e1947a..08c5412 100644 --- a/src/main/deploy/pathplanner/paths/1 Depot side center.path +++ b/src/main/deploy/pathplanner/paths/1 Depot side center.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.489717954282408, - "y": 7.530401114004629 + "x": 4.139596586501166, + "y": 7.549712955779675 }, "prevControl": null, "nextControl": { - "x": 6.6936413483796295, - "y": 7.439924189814815 + "x": 6.343519980598391, + "y": 7.459236031589861 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/2 Center back depot.path b/src/main/deploy/pathplanner/paths/2 Center back depot.path index dc6af98..0b764c1 100644 --- a/src/main/deploy/pathplanner/paths/2 Center back depot.path +++ b/src/main/deploy/pathplanner/paths/2 Center back depot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.9850232738557025, - "y": 4.650690457719163 + "x": 8.012834586466164, + "y": 4.447124060150376 }, "prevControl": null, "nextControl": { - "x": 8.006132660977503, - "y": 7.394910783553141 + "x": 8.09999176373828, + "y": 7.520768188897378 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.9102779260481375, - "y": 7.456715595885093 + "x": 3.3376992481203005, + "y": 7.341883458646617 }, "prevControl": { - "x": 7.54876260667184, - "y": 7.5989681923972086 + "x": 7.9468947368421095, + "y": 7.678176691729323 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,11 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 87.27578727667913 + "rotationDegrees": -5.220589140086717 + }, + { + "waypointRelativePos": 0.9506024096385572, + "rotationDegrees": 0.0 } ], "constraintZones": [], @@ -47,7 +51,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 94.47880231914529 + "rotation": 122.76203131319906 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/3 to middle depot.path b/src/main/deploy/pathplanner/paths/3 to middle depot.path index 19f0e70..ea4ce7e 100644 --- a/src/main/deploy/pathplanner/paths/3 to middle depot.path +++ b/src/main/deploy/pathplanner/paths/3 to middle depot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.027098090277778, - "y": 7.411908492476852 + "x": 3.3640751879699247, + "y": 7.308913533834587 }, "prevControl": null, "nextControl": { - "x": 8.846519748263889, - "y": 7.56884888599537 + "x": 8.507383458646615, + "y": 7.658394736842105 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.944440538194444, - "y": 4.273100622106481 + "x": 8.296375939849625, + "y": 5.060364661654136 }, "prevControl": { - "x": 7.885784722222223, - "y": 6.953094039351852 + "x": 8.42166165413534, + "y": 7.618830827067669 }, "nextControl": null, "isLocked": false, @@ -53,7 +53,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -69.91006596725617 + "rotation": 114.14063152070928 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6eec04a..501dfd9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,9 +9,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -30,6 +32,7 @@ public class Robot extends TimedRobot { private final CommandSwerveDrivetrain drivetrain; StructPublisher publisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish(); + Field2d field = new Field2d(); public Robot() { m_robotContainer = new RobotContainer(); @@ -54,6 +57,8 @@ public void robotPeriodic() { || FlywheelCommand.flywheelState.equals("Stopped"))); robotPose = drivetrain.getState().Pose; publisher.set(robotPose); + field.setRobotPose(robotPose); + SmartDashboard.putData("Field", field); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16ba63f..7a410ba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,8 +96,8 @@ public RobotContainer() { NamedCommands.registerCommand( "rev shooter", - new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain) - .withDeadline(Commands.waitSeconds(1))); + new FlywheelCommand(flywheel, FlywheelCommand.Position.DEPOT_SHOT, drivetrain) + .withDeadline(Commands.waitSeconds(3))); NamedCommands.registerCommand( "shoot long", @@ -105,7 +105,15 @@ public RobotContainer() { new KickerCommand(kicker, KickerCommand.Position.INTAKE), new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), new FlywheelCommand(flywheel, FlywheelCommand.Position.TRENCH_SHOT, drivetrain)) - .withDeadline(Commands.waitSeconds(8))); + .withDeadline(Commands.waitSeconds(5))); + + NamedCommands.registerCommand( + "shoot depot", + Commands.parallel( + new KickerCommand(kicker, KickerCommand.Position.INTAKE), + new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), + new FlywheelCommand(flywheel, FlywheelCommand.Position.DEPOT_SHOT, drivetrain)) + .withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "short shoot", @@ -180,15 +188,13 @@ private void configureBindings() { .povRight() .and(() -> !shootGroup.isScheduled()) .toggleOnTrue(new IntakeCommand(intake, IntakeCommand.Position.OUTTAKE)); - // Right Stick Down = Extend/Retract Hopper + // Right Stick Down = Retract Hopper controller .rightStick() .and(() -> !shootGroup.isScheduled()) .onTrue( - Commands.runEnd( - () -> hopper.move(Hopper.getRotation(isExtended)), () -> hopper.stop(), hopper) - .withDeadline(Commands.waitSeconds(2)) - .andThen(Commands.runOnce(() -> isExtended = !isExtended))); + Commands.run(() -> hopper.move(HopperConfig.HOPPER_RETRACT_ROTATION), hopper) + .withDeadline(Commands.waitSeconds(2))); // Back button = Zero Pigeon controller.back().onTrue(Commands.runOnce(drivetrain::zeroPigeon)); // Right Bumper = Flywheel Toggle for pass @@ -207,7 +213,20 @@ private void configureBindings() { new FlywheelCommand(flywheel, FlywheelCommand.Position.CALCULATED_SHOT, drivetrain)); // Down Plus = Zero hopper controller.povDown().onTrue(Commands.runOnce(() -> hopper.zero(), hopper)); - + // Intake Extend Shoot + controller + .b() + .toggleOnTrue( + Commands.parallel( + new KickerCommand(kicker, KickerCommand.Position.INTAKE), + new IntakeCommand(intake, IntakeCommand.Position.SLOW_INTAKE), + Commands.run(() -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION)))); + // Hopper Extend + controller + .a() + .onTrue( + Commands.run(() -> hopper.move(HopperConfig.HOPPER_EXTEND_ROTATION), hopper) + .withDeadline(Commands.waitSeconds(2))); /* Operator */ operator .leftBumper() @@ -222,8 +241,7 @@ private void configureBindings() { operator .y() .onTrue( - Commands.runOnce( - () -> drivetrain.setPose(ShotCalculator.calculateRightCornerRobotPosition()))); + Commands.runOnce(() -> drivetrain.setPose(ShotCalculator.calculateHubRobotPosition()))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index e1ac82c..2b49c5a 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -70,7 +70,7 @@ public DrivetrainCommand( this.leftX = leftX; this.leftY = leftY; this.rightX = rightX; - double alignP = 75; + double alignP = 60; double alignI = 0; double alignD = 10; this.autoAlignPidController = diff --git a/src/main/java/frc/robot/commands/FlywheelCommand.java b/src/main/java/frc/robot/commands/FlywheelCommand.java index fd07815..dd0dfe1 100644 --- a/src/main/java/frc/robot/commands/FlywheelCommand.java +++ b/src/main/java/frc/robot/commands/FlywheelCommand.java @@ -13,6 +13,7 @@ public static enum Position { HUB_SHOT, TOWER_SHOT, TRENCH_SHOT, + DEPOT_SHOT, CALCULATED_SHOT, DEFAULT_SHOT, OUTTAKE, @@ -57,6 +58,11 @@ public void execute() { flywheelState = "Trench Shot"; break; + case DEPOT_SHOT: + subsystem.shoot(FlywheelConfig.FLYWHEEL_DEPOT_SHOT_SPEED); + flywheelState = "Trench Shot"; + break; + // Spins flywheels at estimated speed given distance from hub case CALCULATED_SHOT: flywheelState = "Calculated Shot"; diff --git a/src/main/java/frc/robot/config/FlywheelConfig.java b/src/main/java/frc/robot/config/FlywheelConfig.java index 3032e97..6b81429 100644 --- a/src/main/java/frc/robot/config/FlywheelConfig.java +++ b/src/main/java/frc/robot/config/FlywheelConfig.java @@ -15,7 +15,7 @@ public class FlywheelConfig { public static final double FLYWHEEL_RIGHT_P = 0.25; public static final double FLYWHEEL_RIGHT_I = 0; public static final double FLYWHEEL_RIGHT_D = 0; - public static final double FLYWHEEL_RIGHT_S = 0.3; + public static final double FLYWHEEL_RIGHT_S = 0.4; public static final double FLYWHEEL_RIGHT_V = 0.123; public static final double FLYWHEEL_RIGHT_A = 0; @@ -25,17 +25,18 @@ public class FlywheelConfig { public static final double FLYWHEEL_LEFT_P = 0.25; public static final double FLYWHEEL_LEFT_I = 0; public static final double FLYWHEEL_LEFT_D = 0; - public static final double FLYWHEEL_LEFT_S = 0.3; + public static final double FLYWHEEL_LEFT_S = 0.4; public static final double FLYWHEEL_LEFT_V = 0.123; public static final double FLYWHEEL_LEFT_A = 0; // Set public static final double FLYWHEEL_HUB_SHOT_SPEED = 30; public static final double FLYWHEEL_TOWER_SHOT_SPEED = 20; - public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 25; - public static final double FLYWHEEL_PASS_SPEED = 50; + public static final double FLYWHEEL_TRENCH_SHOT_SPEED = 30; + public static final double FLYWHEEL_PASS_SPEED = 60; public static final double FLYWHEEL_OUTTAKE_SPEED = 0; public static final double FLYWHEEL_DEFAULT_SHOT_SPEED = 40; public static final double FLYWHEEL_TOLERANCE = 5; + public static final double FLYWHEEL_DEPOT_SHOT_SPEED = 37; } diff --git a/src/main/java/frc/robot/config/KickerConfig.java b/src/main/java/frc/robot/config/KickerConfig.java index b3aa279..47725c4 100644 --- a/src/main/java/frc/robot/config/KickerConfig.java +++ b/src/main/java/frc/robot/config/KickerConfig.java @@ -16,8 +16,8 @@ public class KickerConfig { public static final double KICKER_V = 0; public static final double KICKER_A = 0; - public static final double KICKER_INTAKE_SPEED = 11; - public static final double KICKER_OUTTAKE_SPEED = 11; + public static final double KICKER_INTAKE_SPEED = 6; + public static final double KICKER_OUTTAKE_SPEED = 6; public static final double KICKER_RIGHT_INTAKE_SPEED = 0; public static final double KICKER_RIGHT_OUTTAKE_SPEED = 0; diff --git a/src/main/java/frc/robot/helpers/ShotCalculator.java b/src/main/java/frc/robot/helpers/ShotCalculator.java index ee2a2f7..73de801 100644 --- a/src/main/java/frc/robot/helpers/ShotCalculator.java +++ b/src/main/java/frc/robot/helpers/ShotCalculator.java @@ -53,24 +53,24 @@ public static Pose2d calculateLeftCornerRobotPosition() { Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { if (alliance.get() == DriverStation.Alliance.Blue) { - return new Pose2d(new Translation2d(0.6096, 7.5692), Rotation2d.kZero); + return new Pose2d(new Translation2d(0.6096, 7.5692), Rotation2d.k180deg); } else if (alliance.get() == DriverStation.Alliance.Red) { - return new Pose2d(new Translation2d(15.9258, 0.4826), Rotation2d.k180deg); + return new Pose2d(new Translation2d(15.9258, 0.4826), Rotation2d.kZero); } } - return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.kZero); + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.k180deg); } public static Pose2d calculateRightCornerRobotPosition() { Optional alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { if (alliance.get() == DriverStation.Alliance.Blue) { - return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.kZero); + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.k180deg); } else if (alliance.get() == DriverStation.Alliance.Red) { - return new Pose2d(new Translation2d(15.9258, 7.5692), Rotation2d.k180deg); + return new Pose2d(new Translation2d(15.9258, 7.5692), Rotation2d.kZero); } } - return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.kZero); + return new Pose2d(new Translation2d(0.6096, 0.4826), Rotation2d.k180deg); } public static Pose2d calculateHubRobotPosition() { @@ -79,15 +79,15 @@ public static Pose2d calculateHubRobotPosition() { if (alliance.get() == DriverStation.Alliance.Blue) { return new Pose2d( new Translation2d(Units.inchesToMeters(189.36), (0.4826 + 7.5692) / 2), - Rotation2d.kZero); + Rotation2d.k180deg); } else if (alliance.get() == DriverStation.Alliance.Red) { return new Pose2d( new Translation2d(Units.inchesToMeters(461.84), (0.4826 + 7.5692) / 2), - Rotation2d.k180deg); + Rotation2d.kZero); } } return new Pose2d( - new Translation2d(Units.inchesToMeters(189.36), (0.4826 + 7.5692) / 2), Rotation2d.kZero); + new Translation2d(Units.inchesToMeters(189.36), (0.4826 + 7.5692) / 2), Rotation2d.k180deg); } public static Rotation2d calculatePigeonZero() { @@ -108,7 +108,7 @@ public static Rotation2d getRotationTowardsHub(Translation2d hubpose, Translatio double angleRadians = Math.atan2(deltaY, deltaX); - return new Rotation2d(angleRadians); + return new Rotation2d(angleRadians + Math.PI); } // Determines active based on current match time From 11f0faff434548d31fbcb69cb4e79294b547ed4b Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 29 Apr 2026 19:34:12 -0400 Subject: [PATCH 25/25] DCMP Changes (spotless) --- src/main/java/frc/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 501dfd9..c5c65fd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot;