From fb1624cc34b1e2ed81e33d1da3afdadeb03b4d07 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 28 Feb 2026 16:49:59 -0600 Subject: [PATCH 01/29] Test shooting works --- src/main/java/frc/robot/Constants.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 11 ++++-- .../frc/robot/commands/AimCommandFactory.java | 39 ++++++++++++------- .../robot/subsystems/ShooterSubsystem.java | 3 +- .../robot/subsystems/StagingSubsystem.java | 7 ++++ 5 files changed, 43 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0cc2936..663d0c3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -286,9 +286,10 @@ public static final class ShooterConstants { public static final double kHoodI = 0.0; public static final double kHoodD = 0.0; - public static final double kShooterP = 0.1; + public static final double kShooterP = 0.0001; public static final double kShooterI = 0.0; public static final double kShooterD = 0.0; + public static final double kShooterFF = 0.0019; // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part // NOTE: Need to use 14D so the result is a double, otherwise you end up with diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6e0b03c..fdb7f42 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 static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; @@ -109,14 +110,18 @@ private void configureBindings() { // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - m_driverController.x().onTrue(DeployIntake()); + // m_driverController.x().onTrue(DeployIntake()); - m_driverController.a().onTrue(retractIntake()); + // m_driverController.a().onTrue(retractIntake()); - m_driverController.b().whileTrue(spinIntake()); + // m_driverController.b().whileTrue(spinIntake()); m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + m_driverController.a().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(15))); + + m_driverController.b().onTrue(m_aimFactory.ShootCommand()).onFalse(m_aimFactory.StopShoot()); + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index d8397e2..58e337f 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -39,10 +39,7 @@ import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.TurretConstants; -import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.*; -import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.ShootingEntry; import frc.robot.utils.TargetSolution; import frc.robot.utils.UtilityFunctions; @@ -55,7 +52,7 @@ public class AimCommandFactory { private boolean m_isAiming = false; - private AngularVelocity m_wheelVelocity = RPM.of(60); + private AngularVelocity m_wheelVelocity = RPM.of(5000); private Translation2d m_lockedTag; @@ -123,17 +120,17 @@ public Command AimTurretRelativeToRobot(Angle angle) { }, m_turret).until(m_turret::atTarget); } - public Command ShootCommand() { - return new ConditionalCommand(new RunCommand(this::Shoot, m_shooter), - AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) - .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) - .andThen(new RunCommand(this::Shoot, m_shooter)), - () -> m_isAiming).alongWith(new RunCommand(() -> { - m_stager.Agitate(); - m_stager.Feed(); - m_stager.Roll(); - }, m_stager)).finallyDo(m_shooter::Stop); - } + // public Command ShootCommand() { + // return new ConditionalCommand(new RunCommand(this::Shoot, m_shooter), + // AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) + // .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) + // .andThen(new RunCommand(this::Shoot, m_shooter)), + // () -> m_isAiming).alongWith(new RunCommand(() -> { + // m_stager.Agitate(); + // m_stager.Feed(); + // m_stager.Roll(); + // }, m_stager)).finallyDo(m_shooter::Stop); + // } public Command RunAllStager() { return new RunCommand(() -> { @@ -151,6 +148,18 @@ private void Shoot() { m_shooter.Spin(m_wheelVelocity); } + public Command ShootCommand() { + return new InstantCommand(() -> { + Shoot(); + }); + } + + public Command StopShoot() { + return new InstantCommand(() -> { + m_shooter.Stop(); + }); + } + public TargetSolution GetHubAimSolution() { Translation2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub : Fixtures.kRedAllianceHub; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 6a5ee2d..32f0bce 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -38,7 +38,8 @@ public ShooterSubsystem() { m_shooterConfig.closedLoop .p(ShooterConstants.kShooterP) .i(ShooterConstants.kShooterI) - .d(ShooterConstants.kShooterD); + .d(ShooterConstants.kShooterD) + .feedForward.kV(ShooterConstants.kShooterFF); m_hoodConfig.closedLoop .p(ShooterConstants.kHoodP) diff --git a/src/main/java/frc/robot/subsystems/StagingSubsystem.java b/src/main/java/frc/robot/subsystems/StagingSubsystem.java index 3087a1f..a9ceab3 100644 --- a/src/main/java/frc/robot/subsystems/StagingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/StagingSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -11,6 +12,12 @@ public class StagingSubsystem extends SubsystemBase { SparkMax m_agitationMotor = new SparkMax(StagingConstants.kAgitationMotorId, MotorType.kBrushless); SparkMax m_rollerMotor = new SparkMax(StagingConstants.kRollerMotorId, MotorType.kBrushless); + SparkMaxConfig m_agitateConfig = new SparkMaxConfig(); + + public StagingSubsystem() { + + } + public void Agitate() { m_agitationMotor.set(StagingConstants.kAgitationSpeed); } From 58628ba895bc749ae7c67a9800ac08f3e079f50f Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 28 Feb 2026 17:41:06 -0600 Subject: [PATCH 02/29] test changes --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/commands/AimCommandFactory.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 663d0c3..c708c1a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -352,7 +352,7 @@ public static final class StagingConstants { public static final double kAgitationSpeed = -0.15; public static final int kRollerMotorId = 12; - public static final double kRollerSpeed = 0.25; + public static final double kRollerSpeed = 0.15; } public static final class Fixtures { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fdb7f42..15e3576 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -110,11 +110,11 @@ private void configureBindings() { // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - // m_driverController.x().onTrue(DeployIntake()); + m_driverController.rightBumper().onTrue(DeployIntake()); - // m_driverController.a().onTrue(retractIntake()); + m_driverController.leftBumper().onTrue(retractIntake()); - // m_driverController.b().whileTrue(spinIntake()); + m_driverController.x().whileTrue(spinIntake()); m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 58e337f..3e7b24a 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -52,7 +52,7 @@ public class AimCommandFactory { private boolean m_isAiming = false; - private AngularVelocity m_wheelVelocity = RPM.of(5000); + private AngularVelocity m_wheelVelocity = RPM.of(4000); private Translation2d m_lockedTag; From 939226a84e1d689e9788b99cf89690d493d7ac55 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Sat, 28 Feb 2026 18:18:12 -0600 Subject: [PATCH 03/29] Update shooting parameters via networktables --- src/main/java/frc/robot/Constants.java | 9 +++++++ src/main/java/frc/robot/RobotContainer.java | 26 ++++++++++++++----- .../frc/robot/commands/AimCommandFactory.java | 22 ++++++++++++---- .../robot/subsystems/ShooterSubsystem.java | 6 ++--- 4 files changed, 48 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 663d0c3..f0afe72 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,6 +24,7 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; @@ -50,6 +51,8 @@ import edu.wpi.first.units.measure.Per; import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.utils.ShootingEntry; /** @@ -342,6 +345,12 @@ public static final class ShooterConstants { public static AngularVelocity kFeedingWheelVelocity = RPM.of(60); public static Angle kHoodFeedingPosition = Degrees.of(25.0); public static Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); + + public static Angle kHoodStartingAngle = Degrees.of(0.0); + public static AngularVelocity kShooterStartVelocity = RPM.of(0.0); + + public static final Time kRampTime = Seconds.of(0.4); + public static final Command kRampWaitCommand = new WaitCommand(kRampTime.in(Seconds)); } public static final class StagingConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fdb7f42..a3b01b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,12 +8,15 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.pathplanner.lib.commands.PathPlannerAuto; +import dev.doglog.DogLog; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -26,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.Fixtures; @@ -60,12 +64,13 @@ public class RobotContainer { Angle commandedShooterAngle; AngularVelocity commandedWheelVelocity; + DoubleSubscriber m_hoodAngleGetter = DogLog.tunable("Hood Angle (in degrees)", ShooterConstants.kHoodStartingAngle); + DoubleSubscriber m_shooterVelocityGetter = DogLog.tunable("Motor Velocity (in RPM)", + ShooterConstants.kShooterStartVelocity); + public RobotContainer() { m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); - - NetworkTableInstance inst = NetworkTableInstance.getDefault(); - // SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); // SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); // SmartDashboard.putNumber("Turret angle in degrees", 0.0); @@ -116,11 +121,20 @@ private void configureBindings() { // m_driverController.b().whileTrue(spinIntake()); - m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + m_driverController.y().onTrue(new InstantCommand(() -> { + double shooterVelocity = m_shooterVelocityGetter.get(); + m_aimFactory.ShootAtVelocity(RPM.of(shooterVelocity)); + System.out.println("Shooting at velocity of " + shooterVelocity + " RPM."); + })); + + m_driverController.x().onTrue(new InstantCommand(() -> { + double hoodAngle = m_hoodAngleGetter.get(); + m_aimFactory.MoveHoodToAngle(Degrees.of(hoodAngle)); + })); - m_driverController.a().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(15))); + // m_driverController.a().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(15))); - m_driverController.b().onTrue(m_aimFactory.ShootCommand()).onFalse(m_aimFactory.StopShoot()); + // m_driverController.b().onTrue(m_aimFactory.ShootCommand()).onFalse(m_aimFactory.StopShoot()); } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 58e337f..b800083 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -154,12 +154,20 @@ public Command ShootCommand() { }); } - public Command StopShoot() { + public void ShootAtVelocity(AngularVelocity velocity) { + m_shooter.Spin(velocity); + } + + public Command stopShootCommand() { return new InstantCommand(() -> { - m_shooter.Stop(); + StopShoot(); }); } + public void StopShoot() { + m_shooter.Stop(); + } + public TargetSolution GetHubAimSolution() { Translation2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub : Fixtures.kRedAllianceHub; @@ -186,13 +194,17 @@ public Command MoveTurretToHeadingCommand(Angle heading) { }, m_turret); } - public Command MoveHoodToAbsoluteCommand(Angle angle) { + public Command moveHoodToAngleCommand(Angle angle) { return new InstantCommand(() -> { - System.out.println("Move Hood to position " + angle); - m_shooter.MoveHoodToPosition(angle); + MoveHoodToAngle(angle); }); } + public void MoveHoodToAngle(Angle angle) { + System.out.println("Move Hood to angle " + angle.in(Degrees) + " degrees."); + m_shooter.MoveHoodToPosition(angle); + } + public Command PointAtHub(boolean isRed) { return new RunCommand(() -> { Translation2d hubPosition = isRed ? Fixtures.kRedAllianceHub : Fixtures.kBlueAllianceHub; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 32f0bce..b6aa12c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -38,8 +38,7 @@ public ShooterSubsystem() { m_shooterConfig.closedLoop .p(ShooterConstants.kShooterP) .i(ShooterConstants.kShooterI) - .d(ShooterConstants.kShooterD) - .feedForward.kV(ShooterConstants.kShooterFF); + .d(ShooterConstants.kShooterD).feedForward.kV(ShooterConstants.kShooterFF); m_hoodConfig.closedLoop .p(ShooterConstants.kHoodP) @@ -61,7 +60,7 @@ public ShooterSubsystem() { // Position between 0 and .55 public void MoveHoodToPosition(Angle angle) { - System.out.println("Move hood to position: " + angle); + // System.out.println("Move hood to position: " + angle); // get target absolute encoder position. 0 starts in hood min, hood max is .55 // (30 degrees of movement) @@ -71,7 +70,6 @@ public void MoveHoodToPosition(Angle angle) { return; } var curentPosition = m_absoluteEncoder.getPosition(); - System.out.println("current hood: " + curentPosition); if (curentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { System.out.println("Hood position icorrect for safe movement. Pos: " + curentPosition); return; From 78f7c380918847020a85a3e19661104eafdad76b Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 28 Feb 2026 18:32:47 -0600 Subject: [PATCH 04/29] vendordeps update --- ...enix6-26.1.0.json => Phoenix6-26.1.1.json} | 62 +++++++++---------- vendordeps/REVLib.json | 18 +++--- vendordeps/photonlib.json | 12 ++-- 3 files changed, 46 insertions(+), 46 deletions(-) rename vendordeps/{Phoenix6-26.1.0.json => Phoenix6-26.1.1.json} (92%) diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.1.json index dc5dc62..7a0eca0 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d35e593..a5af3e9 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", + "version": "2026.0.3", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2026.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.3", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index e1fc8bb..fbb5c80 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1-rc-3", + "version": "v2026.2.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.2.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.2.2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-3", + "version": "v2026.2.2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-3" + "version": "v2026.2.2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-3" + "version": "v2026.2.2" } ] } \ No newline at end of file From 34432b90ec005c7937c8aa4b17d0c000f1a0b159 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Sat, 28 Feb 2026 18:48:30 -0600 Subject: [PATCH 05/29] Add some aiming and edits to NT turret functionality --- src/main/java/frc/robot/Constants.java | 7 +----- src/main/java/frc/robot/RobotContainer.java | 5 ++--- .../frc/robot/commands/AimCommandFactory.java | 22 +++++++------------ .../frc/robot/subsystems/DriveSubsystem.java | 3 --- 4 files changed, 11 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2e5eec3..f404636 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -395,12 +395,7 @@ public static enum FieldLocations { } // Placeholders - public static final Angle kBlueLeftSideFeedHeading = Degrees.of(40); - public static final Angle kBlueRightSideFeedHeading = Degrees.of(160); - - // Placeholders - public static final Angle kRedLeftSideFeedHeading = Degrees.of(-40); - public static final Angle kRedRightSideFeedHeading = Degrees.of(-160); + public static final Angle kFeedOffset = Degrees.of(30); public static final Pose2d kRedHubAprilTag = AprilTagFieldLayout .loadField(AprilTagFields.k2026RebuiltAndymark) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9c98a94..ca42b63 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -115,9 +115,8 @@ private void configureBindings() { // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - m_driverController.rightBumper().onTrue(DeployIntake()); - - m_driverController.leftBumper().onTrue(retractIntake()); + // m_driverController.rightBumper().whileTrue(m_aimFactory.AimCommand(false)); + // m_driverController.leftBumper().whileTrue(m_aimFactory.AimCommand(true)); m_driverController.x().whileTrue(spinIntake()); diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 7fce8fb..c777030 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -1,16 +1,11 @@ package frc.robot.commands; -import java.lang.annotation.Target; -import java.lang.reflect.Array; import java.util.ArrayList; import java.util.function.Supplier; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -32,11 +27,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Fixtures; -import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.*; @@ -64,17 +56,17 @@ public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSu m_shooter = shooter; } - public Command AimCommand(Supplier isFeedingLeftSide) { + public Command AimCommand(boolean isFeedingLeftSide) { return new RunCommand(() -> { Aim(isFeedingLeftSide); m_isAiming = true; - }, m_drive, m_turret).finallyDo(() -> { + }, m_turret).finallyDo(() -> { m_isAiming = false; m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; }); } - private void Aim(Supplier isFeedingLeftSide) { + private void Aim(boolean isFeedingLeftSide) { Fixtures.FieldLocations location = m_drive.getRobotLocation(); switch (location) { @@ -91,9 +83,11 @@ private void Aim(Supplier isFeedingLeftSide) { } case NeutralSide: { // Heading changes 180 degrees depending on which alliance you are on - Angle offset = DriverStation.getAlliance().get() == Alliance.Blue ? Degrees.of(0) : Degrees.of(180); - Angle absHeading = isFeedingLeftSide.get() ? offset.minus(Degrees.of(50)) - : offset.plus(Degrees.of(50)); + Angle offset = DriverStation.getAlliance().get() == Alliance.Red ? Degrees.of(0) : Degrees.of(180); + Angle absHeading = isFeedingLeftSide ? offset.minus(Fixtures.kFeedOffset) + : offset.plus(Fixtures.kFeedOffset); + + absHeading = UtilityFunctions.WrapAngle(absHeading); m_shooter.MoveHoodToPosition(ShooterConstants.kHoodFeedingPosition); m_wheelVelocity = ShooterConstants.kFeedingWheelVelocity; diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index bff4d06..f94fae0 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -15,9 +15,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation; From 2b600fd3e469c6f32e039c889a4dc334c0bb1d87 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Sat, 28 Feb 2026 18:51:51 -0600 Subject: [PATCH 06/29] Log distance to april tag --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/subsystems/DriveSubsystem.java | 5 +++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f404636..6e0984f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -397,9 +397,9 @@ public static enum FieldLocations { // Placeholders public static final Angle kFeedOffset = Degrees.of(30); - public static final Pose2d kRedHubAprilTag = AprilTagFieldLayout + public static final Translation2d kRedHubAprilTag = AprilTagFieldLayout .loadField(AprilTagFields.k2026RebuiltAndymark) - .getTagPose(3).get().toPose2d(); + .getTagPose(3).get().toPose2d().getTranslation(); } public static final class IntakeConstants { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index f94fae0..22d47e8 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -45,6 +45,8 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import dev.doglog.DogLog; + import static edu.wpi.first.units.Units.*; import java.util.Optional; @@ -273,6 +275,9 @@ public void periodic() { SmartDashboard.putData(m_field); SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); + + DogLog.log("X dist to april tag in meters", getPose().getTranslation().minus(Fixtures.kRedHubAprilTag).getX()); + DogLog.log("Y dist to april tag in meters", getPose().getTranslation().minus(Fixtures.kRedHubAprilTag).getY()); } /** From 45cc61f90c771c2e57638df7e9b0dcdbcb782a77 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Sat, 28 Feb 2026 22:49:10 -0600 Subject: [PATCH 07/29] Change camera constants and add entries to shooting entry table --- src/main/java/frc/robot/Constants.java | 45 ++++++++++++++------------ 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6e0984f..f8f0b97 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,6 +20,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inch; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; @@ -206,8 +207,8 @@ public static final class VisionConstants { public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); public static final Distance kCameraTwoZ = Inches.of(18.0); - public static final Translation3d kTurretCenterOfRotation = new Translation3d(Meters.of(0.0), - Meters.of(0.0), + public static final Translation3d kTurretCenterOfRotation = new Translation3d(Inches.of(6.25), + Inches.of(-6.151), Inches.of(18)); public static final Angle kCameraTwoPitch = Degrees.of(15.0); @@ -270,7 +271,7 @@ public static final class TurretConstants { public static final Angle kOvershootAmount = Degrees.of(10.0); - public static final Translation2d kTurretOffset = new Translation2d(Meters.of(0.0), Meters.of(0.0)); + public static final Translation2d kTurretOffset = new Translation2d(Inches.of(6.25), Inches.of(-6.151)); public static final Angle kTurretAngleTolerance = Degrees.of(2.0); @@ -309,25 +310,29 @@ public static final class ShooterConstants { public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); + public static final Distance kHubRobotTurretOffset = Inches.of(47); + public static final ShootingEntry[] kShootingEntries = { - new ShootingEntry(Meters.of(0.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(1.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(2.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(3.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(4.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, - Seconds.of(1.0), - kFeedAngle), - new ShootingEntry(Meters.of(5.0), kPlaceholderWheelVelocity, kMaxMuzzleVelocity, null, + new ShootingEntry(Inches.of(30).plus(kHubRobotTurretOffset), RPM.of(3519), null, + Inches.of(101.9), + Seconds.of(0.812), + Degrees.of(0)), + new ShootingEntry(Inches.of(59).plus(kHubRobotTurretOffset), RPM.of(3565), null, + Inches.of(102.335), + Seconds.of(0.822), + Degrees.of(0)), + new ShootingEntry(Inches.of(89).plus(kHubRobotTurretOffset), RPM.of(3975), null, + Inches.of(120.62), Seconds.of(1.0), - kFeedAngle), + Degrees.of(0)), + new ShootingEntry(Inches.of(122).plus(kHubRobotTurretOffset), RPM.of(4375), null, + Inches.of(134.055333), + Seconds.of(1.217), + Degrees.of(0)), + new ShootingEntry(Inches.of(148).plus(kHubRobotTurretOffset), RPM.of(4600), null, + Inches.of(147.348333), + Seconds.of(1.322), + kFeedAngle) }; public static final Angle kHoodTolerence = Degrees.of(2.0); From d55543b91cc7b67ad5a9205f3428e4a305b93a29 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sun, 1 Mar 2026 13:16:10 -0600 Subject: [PATCH 08/29] Working maybe --- src/main/java/frc/robot/Constants.java | 32 ++++++----- src/main/java/frc/robot/RobotContainer.java | 43 +++++++++------ .../frc/robot/commands/AimCommandFactory.java | 53 +++++++++++-------- .../frc/robot/subsystems/DriveSubsystem.java | 2 +- .../frc/robot/subsystems/IntakeSubsystem.java | 4 +- .../robot/subsystems/ShooterSubsystem.java | 27 +++++++--- .../robot/subsystems/StagingSubsystem.java | 2 +- .../frc/robot/subsystems/TurretSubsystem.java | 5 +- .../java/frc/robot/utils/ShootingEntry.java | 17 +++++- 9 files changed, 121 insertions(+), 64 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f8f0b97..ebcd8a5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -276,7 +276,7 @@ public static final class TurretConstants { public static final Angle kTurretAngleTolerance = Degrees.of(2.0); // TODO: Change to real numbers - public static Angle kNonAimTurretAngle = Degrees.of(25.0); + public static Angle kNonAimTurretAngle = Degrees.of(0.0); public static int kTurretMotorAmpLimit = 10; public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.605); @@ -295,6 +295,8 @@ public static final class ShooterConstants { public static final double kShooterD = 0.0; public static final double kShooterFF = 0.0019; + public static final AngularVelocity kShooterVelocityTolerance = RPM.of(500); + // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part // NOTE: Need to use 14D so the result is a double, otherwise you end up with // zero. @@ -303,7 +305,7 @@ public static final class ShooterConstants { // NOTE: gear ration commented out for now is it isn't used // public static final double kHoodGearRatio = (62D / 25) * (14D / 218); public static final int kHoodSmartCurrentLimit = 20; - public static final Angle kFeedAngle = Degrees.of(90.0); + public static final Angle kFeedAngle = Degrees.of(25.0); public static final AngularVelocity kPlaceholderWheelVelocity = RPM.of(2000); public static final LinearVelocity kMuzzleVelocity = MetersPerSecond.of(10); @@ -345,25 +347,31 @@ public static final class ShooterConstants { public static final double kHoodDegreeConversionFactor = kHoodMaxAbsolutePosition / 30; // TODO: Change to real numbers - public static AngularVelocity kNonAimShooterVelocity = RPM.of(500); - public static Angle kNonAimHoodAngle = Degrees.of(15.0); - public static AngularVelocity kFeedingWheelVelocity = RPM.of(60); - public static Angle kHoodFeedingPosition = Degrees.of(25.0); - public static Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); + public static final AngularVelocity kNonAimShooterVelocity = RPM.of(500); + public static final Angle kNonAimHoodAngle = Degrees.of(15.0); + public static final AngularVelocity kFeedingWheelVelocity = RPM.of(4000); + public static final Angle kHoodFeedingPosition = Degrees.of(25.0); + public static final Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); - public static Angle kHoodStartingAngle = Degrees.of(0.0); - public static AngularVelocity kShooterStartVelocity = RPM.of(0.0); + public static final Angle kHoodStartingAngle = Degrees.of(0.0); + public static final AngularVelocity kShooterStartVelocity = RPM.of(0.0); + public static final Angle kDefaultHoodPosition = Degrees.of(4.0); public static final Time kRampTime = Seconds.of(0.4); - public static final Command kRampWaitCommand = new WaitCommand(kRampTime.in(Seconds)); + + // Absolute encoder wraps backwards, so it doesn't read -0.001, it reads 0.999. + // This is the min rotational amount where we can reasonably assume that the + // hood has just gone backwards a little too far, beyond the zero of the encoder + public static final Angle kWrapBackMin = Rotations.of(0.9); } public static final class StagingConstants { public static int kFeedIntoHoodMotor = 16; public static double kFeedIntoHoodSpeed = 0.10; + public static final int kAgitationAmpLimit = 4; public static final int kAgitationMotorId = 9; - public static final double kAgitationSpeed = -0.15; + public static final double kAgitationSpeed = -0.3; public static final int kRollerMotorId = 12; public static final double kRollerSpeed = 0.15; @@ -400,7 +408,7 @@ public static enum FieldLocations { } // Placeholders - public static final Angle kFeedOffset = Degrees.of(30); + public static final Angle kFeedOffset = Degrees.of(12); public static final Translation2d kRedHubAprilTag = AprilTagFieldLayout .loadField(AprilTagFields.k2026RebuiltAndymark) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca42b63..8dff9cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,10 @@ package frc.robot; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.DegreesPerSecond; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -17,7 +16,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -26,13 +24,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.Fixtures; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.OIConstants; import frc.robot.Constants.ShooterConstants; @@ -118,18 +115,31 @@ private void configureBindings() { // m_driverController.rightBumper().whileTrue(m_aimFactory.AimCommand(false)); // m_driverController.leftBumper().whileTrue(m_aimFactory.AimCommand(true)); - m_driverController.x().whileTrue(spinIntake()); + // m_driverController.a().whileTrue(m_aimFactory.RunAllStager()); + + // m_driverController.y().onTrue(new InstantCommand(() -> { + // double shooterVelocity = m_shooterVelocityGetter.get(); + // m_aimFactory.ShootAtVelocity(RPM.of(shooterVelocity)); + // System.out.println("Shooting at velocity of " + shooterVelocity + " RPM."); + // }).andThen(new + // WaitCommand(ShooterConstants.kRampTime)).andThen(m_aimFactory.RunAllStager()) + // .finallyDo(m_aimFactory::StopShoot)); + + m_driverController.leftBumper().whileTrue(m_aimFactory.AimCommand(true)); + m_driverController.rightBumper().whileTrue(m_aimFactory.AimCommand(false)); - m_driverController.y().onTrue(new InstantCommand(() -> { - double shooterVelocity = m_shooterVelocityGetter.get(); - m_aimFactory.ShootAtVelocity(RPM.of(shooterVelocity)); - System.out.println("Shooting at velocity of " + shooterVelocity + " RPM."); - })); + m_driverController.a() + .onTrue(m_aimFactory.ShootCommand().alongWith(Commands.waitUntil(m_shooter::AtWheelVelocityTarget)) + .andThen(m_aimFactory.RunAllStager())) + .onFalse(m_aimFactory.stopShootCommand().andThen(m_aimFactory.stopStaging())); - m_driverController.x().onTrue(new InstantCommand(() -> { - double hoodAngle = m_hoodAngleGetter.get(); - m_aimFactory.MoveHoodToAngle(Degrees.of(hoodAngle)); - })); + m_driverController.x().whileTrue(m_aimFactory.PointAtHub(false)); + m_driverController.y().whileTrue(m_aimFactory.Shoot(RPM.of(4000))); + + // m_driverController.x().onTrue(new InstantCommand(() -> { + // // double hoodAngle = m_hoodAngleGetter.get(); + // // m_aimFactory.MoveHoodToAngle(Degrees.of(hoodAngle)); + // })); // m_driverController.a().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(15))); @@ -203,12 +213,15 @@ public void teleopPeriodic() { Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); m_field.getObject("targetPose").setPose(targetPose); + } public void periodic() { commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); + DogLog.log("At Shooter Velocity Target", m_shooter.AtWheelVelocityTarget()); + // System.out.println(m_drive.getRobotLocation()); } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index c777030..1f43a1c 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -3,6 +3,7 @@ import java.util.ArrayList; import java.util.function.Supplier; +import dev.doglog.DogLog; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; @@ -62,6 +63,7 @@ public Command AimCommand(boolean isFeedingLeftSide) { m_isAiming = true; }, m_turret).finallyDo(() -> { m_isAiming = false; + m_shooter.MoveHoodToPosition(ShooterConstants.kDefaultHoodPosition); m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; }); } @@ -73,12 +75,13 @@ private void Aim(boolean isFeedingLeftSide) { case AllianceSide: { TargetSolution solution = GetHubAimSolution(); - MoveTurretToHeading(solution.hubAngle()); - m_shooter.MoveHoodToPosition(solution.hoodAngle()); - - m_drive.moveByAngle(solution.phi()); + MoveTurretToHeading(solution.hubAngle().minus(solution.phi())); + DogLog.log("Range from hub (meters)", solution.distance().in(Meters)); + // System.out.println(solution.phi()); + // m_shooter.MoveHoodToPosition(solution.hoodAngle()); m_wheelVelocity = solution.wheelSpeed(); + System.out.println(m_wheelVelocity.in(RPM) + " is RPM target."); break; } case NeutralSide: { @@ -105,7 +108,7 @@ private void Aim(boolean isFeedingLeftSide) { public Command AimHoodToPositionCommand(Angle angle) { return new RunCommand(() -> { m_shooter.MoveHoodToPosition(angle); - }).until(m_shooter::AtTarget); + }).until(m_shooter::AtHoodTarget); } public Command AimTurretRelativeToRobot(Angle angle) { @@ -114,24 +117,28 @@ public Command AimTurretRelativeToRobot(Angle angle) { }, m_turret).until(m_turret::atTarget); } - // public Command ShootCommand() { - // return new ConditionalCommand(new RunCommand(this::Shoot, m_shooter), - // AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) - // .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) - // .andThen(new RunCommand(this::Shoot, m_shooter)), - // () -> m_isAiming).alongWith(new RunCommand(() -> { - // m_stager.Agitate(); - // m_stager.Feed(); - // m_stager.Roll(); - // }, m_stager)).finallyDo(m_shooter::Stop); - // } + public Command ShootWhileAimedCommand() { + + DogLog.log("Aiming", m_isAiming); + + return new ConditionalCommand(ShootCommand(), + AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) + .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) + .andThen(new RunCommand(this::Shoot, m_shooter)), + () -> m_isAiming).until(m_shooter::AtWheelVelocityTarget).andThen(RunAllStager()) + .finallyDo(m_shooter::Stop); + } public Command RunAllStager() { - return new RunCommand(() -> { + return new InstantCommand(() -> { m_stager.Agitate(); m_stager.Feed(); m_stager.Roll(); - }, m_stager).finallyDo(() -> { + }, m_stager); + } + + public Command stopStaging() { + return new InstantCommand(() -> { m_stager.StopAgitate(); m_stager.StopFeed(); m_stager.StopRoll(); @@ -139,6 +146,7 @@ public Command RunAllStager() { } private void Shoot() { + // System.out.println(m_wheelVelocity + " is wheel velocity"); m_shooter.Spin(m_wheelVelocity); } @@ -195,7 +203,7 @@ public Command moveHoodToAngleCommand(Angle angle) { } public void MoveHoodToAngle(Angle angle) { - System.out.println("Move Hood to angle " + angle.in(Degrees) + " degrees."); + // System.out.println("Move Hood to angle " + angle.in(Degrees) + " degrees."); m_shooter.MoveHoodToPosition(angle); } @@ -459,6 +467,9 @@ private static TargetSolution getInterpolatedShootingParameters(Distance distanc secondEntry.distance().in(Meters), firstEntry.shooterAngle().in(Radians), secondEntry.shooterAngle().in(Radians), distance.in(Meters))); + DogLog.log("Last entry", firstEntry.toString()); + DogLog.log("Next entry ", secondEntry.toString()); + return new TargetSolution(hoodAngle, wheelSpeed, phi, distance, turretAngle); } @@ -470,9 +481,9 @@ public Command Aim(Angle turretAngle, Angle hoodAngle) { } public Command Shoot(AngularVelocity shooterWheelVelocity) { - return new InstantCommand(() -> { + return new RunCommand(() -> { m_shooter.Spin(shooterWheelVelocity); - }); + }).finallyDo(m_shooter::Stop); } // TODO: Make this better diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 22d47e8..0e98136 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -421,7 +421,7 @@ public void zeroHeading() { */ public Angle getHeading() { return pidgey.getYaw().getValue(); - // TODO: Don't use this code + // Don't use this code // return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ec9385c..dddb629 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -59,8 +59,8 @@ public IntakeSubsystem() { .kV(IntakeConstants.kFFIn); // Apparently there is no absolute encoder :( // m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - m_deploy1Config.idleMode(IdleMode.kCoast); - m_deploy2Config.idleMode(IdleMode.kCoast); // TODO: change to brake + m_deploy1Config.idleMode(IdleMode.kBrake); + m_deploy2Config.idleMode(IdleMode.kBrake); // TODO: change to brake m_intakeConfig.idleMode(IdleMode.kBrake); m_deploy1Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b6aa12c..62600ed 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -4,11 +4,13 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; @@ -24,6 +26,7 @@ public class ShooterSubsystem extends SubsystemBase { SparkMax m_hoodMotor = new SparkMax(ShooterConstants.kHoodMotorId, MotorType.kBrushless); AbsoluteEncoder m_absoluteEncoder = m_hoodMotor.getAbsoluteEncoder(); + RelativeEncoder m_shooterRelativeEncoder = m_shooterMotor.getEncoder(); private final SparkClosedLoopController m_shooterClosedLoopController = m_shooterMotor.getClosedLoopController(); private final SparkClosedLoopController m_hoodClosedLoopController = m_hoodMotor.getClosedLoopController(); @@ -32,6 +35,7 @@ public class ShooterSubsystem extends SubsystemBase { private final SparkMaxConfig m_hoodConfig = new SparkMaxConfig(); Angle m_targetAngle = Degrees.of(0.0); + AngularVelocity m_targetVelocity = RPM.of(0); public ShooterSubsystem() { @@ -40,6 +44,7 @@ public ShooterSubsystem() { .i(ShooterConstants.kShooterI) .d(ShooterConstants.kShooterD).feedForward.kV(ShooterConstants.kShooterFF); + m_shooterConfig.idleMode(IdleMode.kCoast); m_hoodConfig.closedLoop .p(ShooterConstants.kHoodP) .i(ShooterConstants.kHoodI) @@ -69,9 +74,12 @@ public void MoveHoodToPosition(Angle angle) { System.out.println("Hood target position out of bounds. Target: " + targetPosition); return; } - var curentPosition = m_absoluteEncoder.getPosition(); - if (curentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { - System.out.println("Hood position icorrect for safe movement. Pos: " + curentPosition); + + var currentPosition = m_absoluteEncoder.getPosition(); + + if (currentPosition > ShooterConstants.kHoodMaxAbsolutePosition + && currentPosition < ShooterConstants.kWrapBackMin.in(Rotations)) { + System.out.println("Hood position incorrect for safe movement. Pos: " + currentPosition); return; } @@ -79,20 +87,25 @@ public void MoveHoodToPosition(Angle angle) { } public void Spin(AngularVelocity shootSpeedVelocity) { + m_targetVelocity = shootSpeedVelocity; m_shooterClosedLoopController.setSetpoint(shootSpeedVelocity.in(RPM), ControlType.kVelocity); } public void Stop() { - Spin(RPM.of(0)); + m_shooterMotor.stopMotor(); } public Angle GetHoodAngle() { return Degrees.of(m_absoluteEncoder.getPosition() / ShooterConstants.kHoodDegreeConversionFactor); } - public boolean AtTarget() { - return UtilityFunctions.angleDiff(GetHoodAngle(), m_targetAngle).abs(Degrees) < ShooterConstants.kHoodTolerence - .in(Degrees); + public boolean AtHoodTarget() { + return m_hoodClosedLoopController.isAtSetpoint(); + } + + public boolean AtWheelVelocityTarget() { + return RPM.of(m_shooterClosedLoopController.getSetpoint()).minus(RPM.of(m_shooterRelativeEncoder.getVelocity())) + .abs(RPM) < ShooterConstants.kShooterVelocityTolerance.in(RPM); } @Override diff --git a/src/main/java/frc/robot/subsystems/StagingSubsystem.java b/src/main/java/frc/robot/subsystems/StagingSubsystem.java index a9ceab3..3db5f09 100644 --- a/src/main/java/frc/robot/subsystems/StagingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/StagingSubsystem.java @@ -15,7 +15,7 @@ public class StagingSubsystem extends SubsystemBase { SparkMaxConfig m_agitateConfig = new SparkMaxConfig(); public StagingSubsystem() { - + m_agitateConfig.smartCurrentLimit(StagingConstants.kAgitationAmpLimit); } public void Agitate() { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index e7d8101..7892354 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -101,7 +101,7 @@ public void moveToAngle(Angle angle) { angle = angle.plus(TurretConstants.kAngularDistanceToFrontOfRobot); angle = UtilityFunctions.WrapAngle(angle); - System.out.println(angle + "is commanded angle for turret"); + // System.out.println(angle + "is commanded angle for turret"); if (angle.gt(TurretConstants.kMaxAngle)) { System.out @@ -175,7 +175,6 @@ public void simulationPeriodic() { } public boolean atTarget() { - return UtilityFunctions.angleDiff(m_targetAngle, getRotation()) - .abs(Degrees) < TurretConstants.kTurretAngleTolerance.in(Degrees); + return m_turretClosedLoopController.isAtSetpoint(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/ShootingEntry.java b/src/main/java/frc/robot/utils/ShootingEntry.java index 14b19e1..df4fa97 100644 --- a/src/main/java/frc/robot/utils/ShootingEntry.java +++ b/src/main/java/frc/robot/utils/ShootingEntry.java @@ -1,5 +1,11 @@ package frc.robot.utils; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; @@ -7,7 +13,14 @@ import edu.wpi.first.units.measure.Time; public record ShootingEntry(Distance distance, AngularVelocity wheelVelocity, LinearVelocity muzzleVelocity, - Distance maxHeight, - Time timeOfFlight, Angle shooterAngle) { + Distance maxHeight, + Time timeOfFlight, Angle shooterAngle) { + @Override + public String toString() { + return "Distance (meters): " + distance.in(Meters) + ", Angular Wheel Velocity (RPM): " + wheelVelocity.in(RPM) + + ", Muzzle Velocity (MPS): " + muzzleVelocity + ", Max height (Meters): " + + maxHeight.in(Meters) + "Time of Flight (seconds): " + timeOfFlight.in(Seconds) + + ", Shooter Angle (Degrees): " + shooterAngle.in(Degrees); + } } From ca5b5c0db57f60d8175cf7efd87fc70458e8bf86 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 2 Mar 2026 16:05:44 -0600 Subject: [PATCH 09/29] Arc field changes maybe work idk --- src/main/java/frc/robot/Constants.java | 26 +++++++----- src/main/java/frc/robot/RobotContainer.java | 16 +++++-- .../frc/robot/commands/AimCommandFactory.java | 42 +++++++++++++++---- .../frc/robot/subsystems/DriveSubsystem.java | 8 ++-- .../frc/robot/subsystems/IntakeSubsystem.java | 24 ++++++----- .../robot/subsystems/StagingSubsystem.java | 15 +++++-- .../frc/robot/subsystems/TurretSubsystem.java | 7 ++-- .../frc/robot/utils/UtilityFunctions.java | 2 +- 8 files changed, 97 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ebcd8a5..c4dac11 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -192,8 +192,9 @@ public static final class VisionConstants { // Confidence of encoder readings for vision; should be tuned public static final double kEncoderConfidence = 0.15; - public static final Transform3d kRobotToCamOne = new Transform3d(new Translation3d(0.5, 0.0, 0.5), - new Rotation3d(0, 0, 0)); + public static final Transform3d kRobotToCamOne = new Transform3d( + new Translation3d(Inches.of(-3.854), Inches.of(-4.358), Inches.of(20.585)), + new Rotation3d(0, 23, 0)); // These are not final numbers public static final Transform3d kRobotToCamTwo = new Transform3d( @@ -207,8 +208,8 @@ public static final class VisionConstants { public static final Distance kTurretCameraDistanceToCenter = Meters.of(0.13); public static final Distance kCameraTwoZ = Inches.of(18.0); - public static final Translation3d kTurretCenterOfRotation = new Translation3d(Inches.of(6.25), - Inches.of(-6.151), + public static final Translation3d kTurretCenterOfRotation = new Translation3d(Inches.of(-6.25), + Inches.of(6.151), Inches.of(18)); public static final Angle kCameraTwoPitch = Degrees.of(15.0); @@ -271,7 +272,8 @@ public static final class TurretConstants { public static final Angle kOvershootAmount = Degrees.of(10.0); - public static final Translation2d kTurretOffset = new Translation2d(Inches.of(6.25), Inches.of(-6.151)); + public static final Translation2d kTurretOffset = new Translation2d(Inches.of(-6.25), + Inches.of(6.151)); public static final Angle kTurretAngleTolerance = Degrees.of(2.0); @@ -355,7 +357,7 @@ public static final class ShooterConstants { public static final Angle kHoodStartingAngle = Degrees.of(0.0); public static final AngularVelocity kShooterStartVelocity = RPM.of(0.0); - public static final Angle kDefaultHoodPosition = Degrees.of(4.0); + public static final Angle kDefaultHoodPosition = Degrees.of(0.0); public static final Time kRampTime = Seconds.of(0.4); @@ -366,15 +368,19 @@ public static final class ShooterConstants { } public static final class StagingConstants { + public static final int kAgitationAmpLimit = 4; + public static int kFeedIntoHoodMotor = 16; public static double kFeedIntoHoodSpeed = 0.10; - public static final int kAgitationAmpLimit = 4; + public static double kReverseFeedSpeed = -0.1; public static final int kAgitationMotorId = 9; - public static final double kAgitationSpeed = -0.3; + public static final double kAgitationSpeed = -0.45; + public static double kReverseAgitationSpeed = 0.1; public static final int kRollerMotorId = 12; - public static final double kRollerSpeed = 0.15; + public static final double kRollerSpeed = 0.4; + public static double kReverseRollingSpeed = -0.1; } public static final class Fixtures { @@ -443,6 +449,6 @@ public static final class IntakeConstants { public static final int kDeployMotorCurrentLimit = 40; public static final int kIntakeMotorCurrentLimit = 80; - public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2000); + public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-3200); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8dff9cb..8e620f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,7 +134,10 @@ private void configureBindings() { .onFalse(m_aimFactory.stopShootCommand().andThen(m_aimFactory.stopStaging())); m_driverController.x().whileTrue(m_aimFactory.PointAtHub(false)); - m_driverController.y().whileTrue(m_aimFactory.Shoot(RPM.of(4000))); + m_driverController.y().onTrue(m_aimFactory.ReverseStager()).onFalse(m_aimFactory.stopStaging()); + + m_driverController.leftTrigger().onTrue(DeployIntake().alongWith(SpinIntake())) + .onFalse(retractIntake().alongWith(StopIntake())); // m_driverController.x().onTrue(new InstantCommand(() -> { // // double hoodAngle = m_hoodAngleGetter.get(); @@ -170,7 +173,13 @@ public Command feedPosition(Alliance alliance) { public Command spinIntake() { return new RunCommand(() -> { m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); - }).finallyDo(m_intake::stopIntake); + }); + } + + public Command StopIntake() { + return new InstantCommand(() -> { + m_intake.stopIntake(); + }); } public Command retractIntake() { @@ -214,13 +223,14 @@ public void teleopPeriodic() { m_field.getObject("targetPose").setPose(targetPose); + m_aimFactory.periodic(); } public void periodic() { commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); - DogLog.log("At Shooter Velocity Target", m_shooter.AtWheelVelocityTarget()); + // DogLog.log("At Shooter Velocity Target", m_shooter.AtWheelVelocityTarget()); // System.out.println(m_drive.getRobotLocation()); } diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/AimCommandFactory.java index 1f43a1c..1d5f285 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/AimCommandFactory.java @@ -51,6 +51,8 @@ public class AimCommandFactory { private StagingSubsystem m_stager = new StagingSubsystem(); + TargetSolution m_solution; + public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { m_drive = drive; m_turret = turret; @@ -61,27 +63,38 @@ public Command AimCommand(boolean isFeedingLeftSide) { return new RunCommand(() -> { Aim(isFeedingLeftSide); m_isAiming = true; - }, m_turret).finallyDo(() -> { + }, m_turret, m_shooter).finallyDo(() -> { m_isAiming = false; m_shooter.MoveHoodToPosition(ShooterConstants.kDefaultHoodPosition); m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; }); } + public void periodic() { + m_solution = GetHubAimSolution(); + m_wheelVelocity = m_solution.wheelSpeed(); + DogLog.log("RPM target", m_wheelVelocity.in(RPM)); + } + private void Aim(boolean isFeedingLeftSide) { Fixtures.FieldLocations location = m_drive.getRobotLocation(); switch (location) { case AllianceSide: { - TargetSolution solution = GetHubAimSolution(); + + TargetSolution solution; + + if (m_solution == null) { + solution = GetHubAimSolution(); + } else { + solution = m_solution; + } MoveTurretToHeading(solution.hubAngle().minus(solution.phi())); - DogLog.log("Range from hub (meters)", solution.distance().in(Meters)); + // DogLog.log("Range from hub (meters)", solution.distance().in(Meters)); // System.out.println(solution.phi()); - // m_shooter.MoveHoodToPosition(solution.hoodAngle()); - + m_shooter.MoveHoodToPosition(solution.hoodAngle()); m_wheelVelocity = solution.wheelSpeed(); - System.out.println(m_wheelVelocity.in(RPM) + " is RPM target."); break; } case NeutralSide: { @@ -119,7 +132,7 @@ public Command AimTurretRelativeToRobot(Angle angle) { public Command ShootWhileAimedCommand() { - DogLog.log("Aiming", m_isAiming); + // DogLog.log("Aiming", m_isAiming); return new ConditionalCommand(ShootCommand(), AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) @@ -265,6 +278,14 @@ public Command PointTurretToFixture(Pose2d fixture) { }, m_turret); } + public Command ReverseStager() { + return new InstantCommand(() -> { + m_stager.reverseAgitater(); + m_stager.reverseRoller(); + m_stager.reverseFeeder(); + }); + } + // Aims the camera at april tags within range public Command IdleCameraAim() { @@ -467,8 +488,11 @@ private static TargetSolution getInterpolatedShootingParameters(Distance distanc secondEntry.distance().in(Meters), firstEntry.shooterAngle().in(Radians), secondEntry.shooterAngle().in(Radians), distance.in(Meters))); - DogLog.log("Last entry", firstEntry.toString()); - DogLog.log("Next entry ", secondEntry.toString()); + DogLog.log("First Entry", firstEntry.toString()); + DogLog.log("Second Entry", secondEntry.toString()); + + // DogLog.log("Last entry", firstEntry.toString()); + // DogLog.log("Next entry ", secondEntry.toString()); return new TargetSolution(hoodAngle, wheelSpeed, phi, distance, turretAngle); } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 0e98136..c8e4e95 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -274,10 +274,12 @@ public void periodic() { SmartDashboard.putData(m_field); - SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); + // SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); - DogLog.log("X dist to april tag in meters", getPose().getTranslation().minus(Fixtures.kRedHubAprilTag).getX()); - DogLog.log("Y dist to april tag in meters", getPose().getTranslation().minus(Fixtures.kRedHubAprilTag).getY()); + // DogLog.log("X dist to april tag in meters", + // getPose().getTranslation().minus(Fixtures.kRedHubAprilTag).getX()); + // DogLog.log("Y dist to april tag in meters", + // getPose().getTranslation().minus(Fixtures.kRedHubAprilTag).getY()); } /** diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index dddb629..f8ec577 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -61,7 +61,7 @@ public IntakeSubsystem() { // m_deployConfig.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); m_deploy1Config.idleMode(IdleMode.kBrake); m_deploy2Config.idleMode(IdleMode.kBrake); // TODO: change to brake - m_intakeConfig.idleMode(IdleMode.kBrake); + m_intakeConfig.idleMode(IdleMode.kCoast); m_deploy1Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); m_deploy2Config.smartCurrentLimit(IntakeConstants.kDeployMotorCurrentLimit); @@ -157,18 +157,20 @@ public void periodic() { // if (m_maxLimitSwitch2.isPressed()) // System.out.println("Max Two"); - DogLog.log("Deploy motor 1: canID 13 position", - m_deploy1RelativeEncoder.getPosition()); - DogLog.log("Deploy motor 2: canID 8 position", - m_deploy2RelativeEncoder.getPosition()); + // DogLog.log("Deploy motor 1: canID 13 position", + // m_deploy1RelativeEncoder.getPosition()); + // DogLog.log("Deploy motor 2: canID 8 position", + // m_deploy2RelativeEncoder.getPosition()); - DogLog.log("Deploy motor can ID 8 setpoint", m_deploy2ClosedLoopController.getSetpoint()); - DogLog.log("Deploy motor can ID 13 setpoint", m_deploy1ClosedLoopController.getSetpoint()); + // DogLog.log("Deploy motor can ID 8 setpoint", + // m_deploy2ClosedLoopController.getSetpoint()); + // DogLog.log("Deploy motor can ID 13 setpoint", + // m_deploy1ClosedLoopController.getSetpoint()); - DogLog.log("Min limit one", m_minLimitSwitch1.isPressed()); - DogLog.log("Min limit two", m_minLimitSwitch2.isPressed()); + // DogLog.log("Min limit one", m_minLimitSwitch1.isPressed()); + // DogLog.log("Min limit two", m_minLimitSwitch2.isPressed()); - DogLog.log("Max limit one", m_maxLimitSwitch1.isPressed()); - DogLog.log("Max limit two", m_maxLimitSwitch2.isPressed()); + // DogLog.log("Max limit one", m_maxLimitSwitch1.isPressed()); + // DogLog.log("Max limit two", m_maxLimitSwitch2.isPressed()); } } diff --git a/src/main/java/frc/robot/subsystems/StagingSubsystem.java b/src/main/java/frc/robot/subsystems/StagingSubsystem.java index 3db5f09..8908a22 100644 --- a/src/main/java/frc/robot/subsystems/StagingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/StagingSubsystem.java @@ -12,10 +12,7 @@ public class StagingSubsystem extends SubsystemBase { SparkMax m_agitationMotor = new SparkMax(StagingConstants.kAgitationMotorId, MotorType.kBrushless); SparkMax m_rollerMotor = new SparkMax(StagingConstants.kRollerMotorId, MotorType.kBrushless); - SparkMaxConfig m_agitateConfig = new SparkMaxConfig(); - public StagingSubsystem() { - m_agitateConfig.smartCurrentLimit(StagingConstants.kAgitationAmpLimit); } public void Agitate() { @@ -44,4 +41,16 @@ public void StopFeed() { public void StopRoll() { m_rollerMotor.stopMotor(); } + + public void reverseAgitater() { + m_agitationMotor.set(StagingConstants.kReverseAgitationSpeed); + } + + public void reverseRoller() { + m_rollerMotor.set(StagingConstants.kReverseRollingSpeed); + } + + public void reverseFeeder() { + m_feedIntoHoodMotor.set(StagingConstants.kReverseFeedSpeed); + } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 7892354..85ad730 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -146,9 +146,10 @@ public void periodic() { RPM.of(m_absoluteEncoder.getVelocity()), TurretConstants.kEncoderReadingDelay.in(Seconds)); - DogLog.log("Turret rotation relative to front of robot", getRotation().in(Degrees)); - DogLog.log("Turret rotation relative to turret zero", UtilityFunctions - .WrapAngle(getRotation().minus(TurretConstants.kAngularDistanceToFrontOfRobot)).in(Degrees)); + // DogLog.log("Turret rotation relative to front of robot", + // getRotation().in(Degrees)); + // DogLog.log("Turret rotation relative to turret zero", UtilityFunctions + // .WrapAngle(getRotation().minus(TurretConstants.kAngularDistanceToFrontOfRobot)).in(Degrees)); } // Connected to another periodic loop that runs quicker than 0.02 seconds diff --git a/src/main/java/frc/robot/utils/UtilityFunctions.java b/src/main/java/frc/robot/utils/UtilityFunctions.java index 1e8c7eb..a133e1b 100644 --- a/src/main/java/frc/robot/utils/UtilityFunctions.java +++ b/src/main/java/frc/robot/utils/UtilityFunctions.java @@ -27,7 +27,7 @@ public static double interpolate(double x1, double x2, double y1, double y2, dou double dy = y2 - y1; double dx = x - x1; - return (dx / Math.abs(dsx) > 0.0 ? dsx : NumericalConstants.kEpsilon) * dy + y1; + return (dx / Math.abs(dsx) > 0.0 ? dx / dsx : NumericalConstants.kEpsilon) * dy + y1; } public static Angle angleDiff(Angle a1, Angle a2) { From cf51c1b52522524587875f13b7b67f7788bb6a39 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 2 Mar 2026 17:26:16 -0600 Subject: [PATCH 10/29] Fix interpolation algo --- .../pathplanner/autos/Example Auto.auto | 6 ++ src/main/java/frc/robot/Constants.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 77 ++++++------------- ...ommandFactory.java => CommandFactory.java} | 69 +++++++++++++++-- 4 files changed, 95 insertions(+), 64 deletions(-) rename src/main/java/frc/robot/commands/{AimCommandFactory.java => CommandFactory.java} (92%) diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto index 268147b..8b6eefc 100644 --- a/src/main/deploy/pathplanner/autos/Example Auto.auto +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -9,6 +9,12 @@ "data": { "pathName": "New Path" } + }, + { + "type": "named", + "data": { + "name": null + } } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c4dac11..1132d11 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -280,6 +280,7 @@ public static final class TurretConstants { // TODO: Change to real numbers public static Angle kNonAimTurretAngle = Degrees.of(0.0); public static int kTurretMotorAmpLimit = 10; + public static final Angle kTurretTorwardsFront = Degrees.of(0.0); public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.605); } @@ -440,15 +441,15 @@ public static final class IntakeConstants { public static final int kIntakeMotorId = 7; // 10 teeth on pinion, 20 teeth on rack - public static final Angle kDeployRotations = Rotations.of(9.5); + public static final Angle kDeployRotations = Rotations.of(9.3); public static final Angle kRetractRotations = Rotations.of(0.0); - public static final Angle kMaxExtension = Rotations.of(9.5); + public static final Angle kMaxExtension = Rotations.of(9.3); public static final Angle kMinExtension = Rotations.of(0.0); public static final int kDeployMotorCurrentLimit = 40; public static final int kIntakeMotorCurrentLimit = 80; - public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-3200); + public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2500); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8e620f2..8baa961 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; import dev.doglog.DogLog; @@ -33,7 +34,7 @@ import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.OIConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.commands.AimCommandFactory; +import frc.robot.commands.CommandFactory; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -41,9 +42,6 @@ import frc.robot.utils.*; public class RobotContainer { - - private final IntakeSubsystem m_intake = new IntakeSubsystem(); - private final CommandXboxController m_driverController = new CommandXboxController( OIConstants.kDriverControllerPort); @@ -54,7 +52,7 @@ public class RobotContainer { private final ShooterSubsystem m_shooter = new ShooterSubsystem(); private final DriveSubsystem m_drive = new DriveSubsystem(m_turret::getRotationAtTime); - AimCommandFactory m_aimFactory = new AimCommandFactory(m_drive, m_turret, m_shooter); + CommandFactory m_commandFactory = new CommandFactory(m_drive, m_turret, m_shooter); Field2d m_field; // For getting data points for the lookup table @@ -66,6 +64,13 @@ public class RobotContainer { ShooterConstants.kShooterStartVelocity); public RobotContainer() { + NamedCommands.registerCommand("Deploy Intake", m_commandFactory.DeployIntake()); + NamedCommands.registerCommand("Retract Intake", m_commandFactory.RetractIntake()); + NamedCommands.registerCommand("Aim", m_commandFactory.AimCommand(false)); + NamedCommands.registerCommand("Stop Aim", m_commandFactory.StopAimCommand()); + NamedCommands.registerCommand("Shoot", m_commandFactory.ShootCommand()); + NamedCommands.registerCommand("Stop Shoot", m_commandFactory.StopShootCommand()); + m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); SmartDashboard.putData("Auto Choices", m_chooser); // SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); @@ -125,19 +130,22 @@ private void configureBindings() { // WaitCommand(ShooterConstants.kRampTime)).andThen(m_aimFactory.RunAllStager()) // .finallyDo(m_aimFactory::StopShoot)); - m_driverController.leftBumper().whileTrue(m_aimFactory.AimCommand(true)); - m_driverController.rightBumper().whileTrue(m_aimFactory.AimCommand(false)); + m_driverController.leftBumper().whileTrue(m_commandFactory.AimCommand(true)) + .onFalse(m_commandFactory.StopAimCommand()); + m_driverController.rightBumper().whileTrue(m_commandFactory.AimCommand(false)) + .onFalse(m_commandFactory.StopAimCommand()); m_driverController.a() - .onTrue(m_aimFactory.ShootCommand().alongWith(Commands.waitUntil(m_shooter::AtWheelVelocityTarget)) - .andThen(m_aimFactory.RunAllStager())) - .onFalse(m_aimFactory.stopShootCommand().andThen(m_aimFactory.stopStaging())); + .onTrue(m_commandFactory.ShootCommand().alongWith(Commands.waitUntil(m_shooter::AtWheelVelocityTarget)) + .andThen(m_commandFactory.RunAllStager())) + .onFalse(m_commandFactory.StopShootCommand().andThen(m_commandFactory.stopStaging())); - m_driverController.x().whileTrue(m_aimFactory.PointAtHub(false)); - m_driverController.y().onTrue(m_aimFactory.ReverseStager()).onFalse(m_aimFactory.stopStaging()); + m_driverController.x().whileTrue(m_commandFactory.PointAtHub(false)); + m_driverController.y().onTrue(m_commandFactory.ReverseStager()).onFalse(m_commandFactory.stopStaging()); - m_driverController.leftTrigger().onTrue(DeployIntake().alongWith(SpinIntake())) - .onFalse(retractIntake().alongWith(StopIntake())); + m_driverController.leftTrigger() + .onTrue(m_commandFactory.DeployIntake().alongWith(m_commandFactory.SpinIntake())) + .onFalse(m_commandFactory.RetractIntake().alongWith(m_commandFactory.StopIntake())); // m_driverController.x().onTrue(new InstantCommand(() -> { // // double hoodAngle = m_hoodAngleGetter.get(); @@ -170,47 +178,10 @@ public Command feedPosition(Alliance alliance) { }, m_drive, m_turret); } - public Command spinIntake() { - return new RunCommand(() -> { - m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); - }); - } - - public Command StopIntake() { - return new InstantCommand(() -> { - m_intake.stopIntake(); - }); - } - - public Command retractIntake() { - return new InstantCommand(() -> { - m_intake.retractIntake(); - }); - } - - public Command outTake() { - return new RunCommand(() -> { - m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1)); - }); - } - - public Command DeployIntake() { - return new InstantCommand(() -> { - m_intake.deployIntake(); - }); - - } - - public Command SpinIntake() { - return new InstantCommand(() -> { - m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); - }); - } - public void teleopPeriodic() { m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); - TargetSolution solution = m_aimFactory.GetHubAimSolution(); + TargetSolution solution = m_commandFactory.GetHubAimSolution(); Pose2d robotPose = m_drive.getPose(); @@ -223,7 +194,7 @@ public void teleopPeriodic() { m_field.getObject("targetPose").setPose(targetPose); - m_aimFactory.periodic(); + m_commandFactory.periodic(); } public void periodic() { diff --git a/src/main/java/frc/robot/commands/AimCommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java similarity index 92% rename from src/main/java/frc/robot/commands/AimCommandFactory.java rename to src/main/java/frc/robot/commands/CommandFactory.java index 1d5f285..570a655 100644 --- a/src/main/java/frc/robot/commands/AimCommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.Constants.Fixtures; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.*; @@ -37,7 +38,7 @@ import frc.robot.utils.TargetSolution; import frc.robot.utils.UtilityFunctions; -public class AimCommandFactory { +public class CommandFactory { private DriveSubsystem m_drive; private TurretSubsystem m_turret; @@ -50,10 +51,11 @@ public class AimCommandFactory { private Translation2d m_lockedTag; private StagingSubsystem m_stager = new StagingSubsystem(); + private IntakeSubsystem m_intake = new IntakeSubsystem(); TargetSolution m_solution; - public AimCommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { + public CommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { m_drive = drive; m_turret = turret; m_shooter = shooter; @@ -63,11 +65,17 @@ public Command AimCommand(boolean isFeedingLeftSide) { return new RunCommand(() -> { Aim(isFeedingLeftSide); m_isAiming = true; - }, m_turret, m_shooter).finallyDo(() -> { - m_isAiming = false; - m_shooter.MoveHoodToPosition(ShooterConstants.kDefaultHoodPosition); - m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; - }); + }, m_turret, m_shooter); + } + + public void StopAim() { + m_isAiming = false; + m_shooter.MoveHoodToPosition(ShooterConstants.kDefaultHoodPosition); + m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; + } + + public Command StopAimCommand() { + return new InstantCommand(this::StopAim); } public void periodic() { @@ -76,6 +84,14 @@ public void periodic() { DogLog.log("RPM target", m_wheelVelocity.in(RPM)); } + public void AimTurretToFront() { + m_turret.moveToAngle(TurretConstants.kTurretTorwardsFront); + } + + public Command AimTurretToFrontCommand() { + return new InstantCommand(this::AimTurretToFrontCommand); + } + private void Aim(boolean isFeedingLeftSide) { Fixtures.FieldLocations location = m_drive.getRobotLocation(); @@ -173,7 +189,7 @@ public void ShootAtVelocity(AngularVelocity velocity) { m_shooter.Spin(velocity); } - public Command stopShootCommand() { + public Command StopShootCommand() { return new InstantCommand(() -> { StopShoot(); }); @@ -183,6 +199,37 @@ public void StopShoot() { m_shooter.Stop(); } + public Command StopIntake() { + return new InstantCommand(() -> { + m_intake.stopIntake(); + }); + } + + public Command RetractIntake() { + return new InstantCommand(() -> { + m_intake.retractIntake(); + }); + } + + public Command OutTake() { + return new InstantCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed.times(-1)); + }); + } + + public Command DeployIntake() { + return new InstantCommand(() -> { + m_intake.deployIntake(); + }); + + } + + public Command SpinIntake() { + return new InstantCommand(() -> { + m_intake.spinIntake(IntakeConstants.kDefaultIntakeSpeed); + }); + } + public TargetSolution GetHubAimSolution() { Translation2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub : Fixtures.kRedAllianceHub; @@ -497,6 +544,12 @@ private static TargetSolution getInterpolatedShootingParameters(Distance distanc return new TargetSolution(hoodAngle, wheelSpeed, phi, distance, turretAngle); } + public Command AutoIntakeOut() { + return new InstantCommand(() -> { + + }); + } + public Command Aim(Angle turretAngle, Angle hoodAngle) { return new InstantCommand(() -> { m_turret.moveToAngle(turretAngle); From 8e83b719e2516660afc59bb1388bacc7d75a5bc6 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 2 Mar 2026 19:14:26 -0600 Subject: [PATCH 11/29] Not working vision --- .../pathplanner/autos/Example Auto.auto | 6 -- .../autos/Left Side Neutral Auto.auto | 55 +++++++++++++ .../paths/Left Side Center Return.path | 59 ++++++++++++++ .../paths/Left Side To Center Path.path | 54 +++++++++++++ src/main/java/frc/robot/Constants.java | 18 ++++- src/main/java/frc/robot/RobotContainer.java | 7 +- .../frc/robot/commands/CommandFactory.java | 49 ++++++++++-- .../robot/subsystems/ClimberSubsystem.java | 77 +++++++++++++++++++ 8 files changed, 310 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Left Side Center Return.path create mode 100644 src/main/deploy/pathplanner/paths/Left Side To Center Path.path create mode 100644 src/main/java/frc/robot/subsystems/ClimberSubsystem.java diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto index 8b6eefc..268147b 100644 --- a/src/main/deploy/pathplanner/autos/Example Auto.auto +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -9,12 +9,6 @@ "data": { "pathName": "New Path" } - }, - { - "type": "named", - "data": { - "name": null - } } ] } diff --git a/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto new file mode 100644 index 0000000..c32e656 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Deploy Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Side To Center Path" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "Retract Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Side Center Return" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Side Center Return.path b/src/main/deploy/pathplanner/paths/Left Side Center Return.path new file mode 100644 index 0000000..8b0747c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Side Center Return.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.428987161198288, + "y": 6.279857346647646 + }, + "prevControl": null, + "nextControl": { + "x": 7.493680456490727, + "y": 7.845435092724678 + }, + "isLocked": false, + "linkedName": "Left Center Intake Location" + }, + { + "anchor": { + "x": 2.0853209700392283, + "y": 5.917574893010523 + }, + "prevControl": { + "x": 1.8006704707524948, + "y": 8.026576319546859 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.15527273515932175 + } + ], + "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": -32.347443499441994 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -35.28674897496953 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Side To Center Path.path b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path new file mode 100644 index 0000000..9afe055 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.793223965763195, + "y": 7.405520684736091 + }, + "prevControl": null, + "nextControl": { + "x": 6.1351212553495005, + "y": 7.7289871611982885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.428987161198288, + "y": 6.279857346647646 + }, + "prevControl": { + "x": 5.941041369472183, + "y": 6.991483594864479 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Center Intake Location" + } + ], + "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": -35.28674897496953 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ 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 1132d11..8b2fe27 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -280,7 +280,7 @@ public static final class TurretConstants { // TODO: Change to real numbers public static Angle kNonAimTurretAngle = Degrees.of(0.0); public static int kTurretMotorAmpLimit = 10; - public static final Angle kTurretTorwardsFront = Degrees.of(0.0); + public static final Angle kTurretTorwardsFront = Degrees.of(350); public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.605); } @@ -358,7 +358,7 @@ public static final class ShooterConstants { public static final Angle kHoodStartingAngle = Degrees.of(0.0); public static final AngularVelocity kShooterStartVelocity = RPM.of(0.0); - public static final Angle kDefaultHoodPosition = Degrees.of(0.0); + public static final Angle kDefaultHoodPosition = Degrees.of(2.0); public static final Time kRampTime = Seconds.of(0.4); @@ -452,4 +452,18 @@ public static final class IntakeConstants { public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2500); } + + public static final class ClimberConstants { + public static final int kMotorCanId = 17; + + public static final double kUpVelocity = 0.2; + public static final double kDownVelocity = -0.1; + + public static final double klimitMaxExtension = 3.0; + public static final double kLimitMinExtension = 0.0; + // TODO : Get the Constants for max and minimum distance for the climber + + public static final double kMaxExtension = klimitMaxExtension - 0.1; + public static final double kMinExtension = kLimitMinExtension + 0.1; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8baa961..6a4d565 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -138,10 +138,13 @@ private void configureBindings() { m_driverController.a() .onTrue(m_commandFactory.ShootCommand().alongWith(Commands.waitUntil(m_shooter::AtWheelVelocityTarget)) .andThen(m_commandFactory.RunAllStager())) - .onFalse(m_commandFactory.StopShootCommand().andThen(m_commandFactory.stopStaging())); + .onFalse(m_commandFactory.StopShootCommand().andThen(m_commandFactory.StopStaging())); m_driverController.x().whileTrue(m_commandFactory.PointAtHub(false)); - m_driverController.y().onTrue(m_commandFactory.ReverseStager()).onFalse(m_commandFactory.stopStaging()); + m_driverController.y().onTrue(m_commandFactory.ReverseStager()).onFalse(m_commandFactory.StopStaging()); + + // m_driverController.povUp().whileTrue(m_commandFactory.ClimbUpCommand()); + // m_driverController.povDown().whileTrue(m_commandFactory.ClimbDownCommand()); m_driverController.leftTrigger() .onTrue(m_commandFactory.DeployIntake().alongWith(m_commandFactory.SpinIntake())) diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 570a655..ccc4690 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -52,6 +53,7 @@ public class CommandFactory { private StagingSubsystem m_stager = new StagingSubsystem(); private IntakeSubsystem m_intake = new IntakeSubsystem(); + private ClimberSubsystem m_climber = new ClimberSubsystem(); TargetSolution m_solution; @@ -166,7 +168,7 @@ public Command RunAllStager() { }, m_stager); } - public Command stopStaging() { + public Command StopStaging() { return new InstantCommand(() -> { m_stager.StopAgitate(); m_stager.StopFeed(); @@ -208,7 +210,7 @@ public Command StopIntake() { public Command RetractIntake() { return new InstantCommand(() -> { m_intake.retractIntake(); - }); + }).andThen(StopIntake()); } public Command OutTake() { @@ -220,8 +222,7 @@ public Command OutTake() { public Command DeployIntake() { return new InstantCommand(() -> { m_intake.deployIntake(); - }); - + }).alongWith(SpinIntake()); } public Command SpinIntake() { @@ -256,7 +257,7 @@ public Command MoveTurretToHeadingCommand(Angle heading) { }, m_turret); } - public Command moveHoodToAngleCommand(Angle angle) { + public Command MoveHoodToAngleCommand(Angle angle) { return new InstantCommand(() -> { MoveHoodToAngle(angle); }); @@ -333,6 +334,44 @@ public Command ReverseStager() { }); } + public void ClimbUp() { + m_climber.climbUp(); + } + + public void ClimbDown() { + m_climber.climbUp(); + } + + public void StopClimb() { + m_climber.Stop(); + } + + public Command MoveTurretToRobotRelativeHeadingCommand(Angle angle) { + return new InstantCommand(() -> { + m_turret.moveToAngle(angle); + }); + } + + public Command ClimbUpCommand() { + // return + // MoveTurretToRobotRelativeHeadingCommand(TurretConstants.kTurretTorwardsFront) + // .alongWith(Commands.waitUntil(m_turret::atTarget)) + + return (new RunCommand(this::ClimbUp)) + .until(m_climber::atMax) + .finallyDo(this::StopClimb); + } + + public Command ClimbDownCommand() { + // return + // MoveTurretToRobotRelativeHeadingCommand(TurretConstants.kTurretTorwardsFront) + // .alongWith(Commands.waitUntil(m_turret::atTarget)) + + return (new RunCommand(this::ClimbDown)) + .until(m_climber::atMin) + .finallyDo(this::StopClimb); + } + // Aims the camera at april tags within range public Command IdleCameraAim() { diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..3e7b799 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -0,0 +1,77 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Rotations; + +import com.revrobotics.spark.SparkLimitSwitch; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.revrobotics.RelativeEncoder; +import frc.robot.Constants.ClimberConstants; + +public class ClimberSubsystem extends SubsystemBase { + SparkMax m_climbMotor = new SparkMax(ClimberConstants.kMotorCanId, MotorType.kBrushless); + + RelativeEncoder m_relativeEncoder = m_climbMotor.getEncoder(); + + private final SparkLimitSwitch m_minLimitSwitch = m_climbMotor.getReverseLimitSwitch(); + private final SparkLimitSwitch m_maxLimitSwitch = m_climbMotor.getForwardLimitSwitch(); + + public double GetPosition() { + return m_relativeEncoder.getPosition(); + } + + public void climbUp() { + + if (!atMax()) { + m_climbMotor.set(ClimberConstants.kUpVelocity); + } else { + Stop(); + } + } + + public void climbDown() { + + if (!atMin()) { + m_climbMotor.set(ClimberConstants.kDownVelocity); + } else { + Stop(); + } + } + + public boolean atMax() { + return GetPosition() >= ClimberConstants.kMaxExtension; + } + + public boolean atMin() { + return GetPosition() <= ClimberConstants.kMinExtension; + } + + public void Stop() { + m_climbMotor.set(0.0); + } + + public Command ZeroCommand() { + return new InstantCommand(() -> { + m_relativeEncoder.setPosition(0); + }); + } + + @Override + public void periodic() { + if (m_minLimitSwitch.isPressed()) { + m_relativeEncoder.setPosition(ClimberConstants.kLimitMinExtension); + } else if (m_maxLimitSwitch.isPressed()) { + m_relativeEncoder.setPosition(ClimberConstants.kLimitMinExtension); + } + + // SmartDashboard.putNumber("Climber Position", GetPosition()); + // SmartDashboard.putData("Zero", ZeroCommand()); + } +} \ No newline at end of file From ca78bfe42315dc903a76bc015ba1f5cc669b889e Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 2 Mar 2026 19:39:41 -0600 Subject: [PATCH 12/29] Turret pointing in wrong direction --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/utils/UtilityFunctions.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8b2fe27..31f77a4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -194,7 +194,7 @@ public static final class VisionConstants { public static final Transform3d kRobotToCamOne = new Transform3d( new Translation3d(Inches.of(-3.854), Inches.of(-4.358), Inches.of(20.585)), - new Rotation3d(0, 23, 0)); + new Rotation3d(0, 180 - 23, 0)); // These are not final numbers public static final Transform3d kRobotToCamTwo = new Transform3d( @@ -358,7 +358,7 @@ public static final class ShooterConstants { public static final Angle kHoodStartingAngle = Degrees.of(0.0); public static final AngularVelocity kShooterStartVelocity = RPM.of(0.0); - public static final Angle kDefaultHoodPosition = Degrees.of(2.0); + public static final Angle kDefaultHoodPosition = Degrees.of(3.0); public static final Time kRampTime = Seconds.of(0.4); diff --git a/src/main/java/frc/robot/utils/UtilityFunctions.java b/src/main/java/frc/robot/utils/UtilityFunctions.java index a133e1b..9b94904 100644 --- a/src/main/java/frc/robot/utils/UtilityFunctions.java +++ b/src/main/java/frc/robot/utils/UtilityFunctions.java @@ -27,7 +27,7 @@ public static double interpolate(double x1, double x2, double y1, double y2, dou double dy = y2 - y1; double dx = x - x1; - return (dx / Math.abs(dsx) > 0.0 ? dx / dsx : NumericalConstants.kEpsilon) * dy + y1; + return (Math.abs(dsx) > 0.0 ? dx / dsx : NumericalConstants.kEpsilon) * dy + y1; } public static Angle angleDiff(Angle a1, Angle a2) { From feba71b7172b5bdc398018455adb3197609b8888 Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Tue, 3 Mar 2026 06:25:50 -0600 Subject: [PATCH 13/29] Maybe 180 offset fix and make shoot command change motor speed on the fly for shooting --- simgui-ds.json | 3 +-- src/main/java/frc/robot/Constants.java | 25 ++++--------------- src/main/java/frc/robot/RobotContainer.java | 7 +++--- .../frc/robot/commands/CommandFactory.java | 9 +++++-- .../robot/subsystems/ShooterSubsystem.java | 17 +++++++------ 5 files changed, 26 insertions(+), 35 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 37ec5eb..c77974a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -100,8 +100,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 31f77a4..3da7e96 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,14 +3,13 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import java.util.HashMap; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; @@ -19,41 +18,26 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.Measure; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inch; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; - -import java.lang.reflect.Field; -import java.util.HashMap; - -import edu.wpi.first.units.AngleUnit; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.AngleUnit; -import edu.wpi.first.units.DistanceUnit; -import edu.wpi.first.units.PerUnit; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Per; import edu.wpi.first.units.measure.Time; -import edu.wpi.first.units.measure.Velocity; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.utils.ShootingEntry; /** @@ -230,6 +214,7 @@ public static final class NumericalConstants { public static final Angle kFullRotation = Radians.of(2.0 * Math.PI); public static final Angle kNoRotation = Radians.of(0.0); public static final LinearAcceleration kGravity = MetersPerSecondPerSecond.of(9.807); + public static final Angle kHalfRotation = Degrees.of(180); } public static final class TurretConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a4d565..67aac7b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -135,10 +135,9 @@ private void configureBindings() { m_driverController.rightBumper().whileTrue(m_commandFactory.AimCommand(false)) .onFalse(m_commandFactory.StopAimCommand()); - m_driverController.a() - .onTrue(m_commandFactory.ShootCommand().alongWith(Commands.waitUntil(m_shooter::AtWheelVelocityTarget)) - .andThen(m_commandFactory.RunAllStager())) - .onFalse(m_commandFactory.StopShootCommand().andThen(m_commandFactory.StopStaging())); + m_driverController.a().whileTrue(m_commandFactory.ShootCommand()) + .onTrue(Commands.waitUntil(m_shooter::AtWheelVelocityTarget).andThen(m_commandFactory.RunAllStager())) + .onFalse(m_commandFactory.StopShootCommand().alongWith(m_commandFactory.StopStaging())); m_driverController.x().whileTrue(m_commandFactory.PointAtHub(false)); m_driverController.y().onTrue(m_commandFactory.ReverseStager()).onFalse(m_commandFactory.StopStaging()); diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index ccc4690..0937e3c 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -30,6 +30,8 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants; +import frc.robot.Robot; import frc.robot.Constants.Fixtures; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; @@ -182,7 +184,7 @@ private void Shoot() { } public Command ShootCommand() { - return new InstantCommand(() -> { + return new RunCommand(() -> { Shoot(); }); } @@ -286,7 +288,10 @@ public Command PointAtHub(boolean isRed) { public void MoveTurretToHeading(Angle heading) { Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); - Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading)); + // Weird bug with red side position data + Angle offset = DriverStation.getAlliance().get() == Alliance.Red && !Robot.isReal() ? Constants.NumericalConstants.kHalfRotation : Constants.NumericalConstants.kNoRotation; + + Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading).plus(offset)); // Angle[] currentRange = getCurrentTurretRange(); Angle[] currentRange = TurretConstants.kUnrestrictedAngles; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 62600ed..2f91e8b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,24 +1,27 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.*; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import dev.doglog.DogLog; +import com.revrobotics.spark.config.SparkMaxConfig; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Rotations; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; -import frc.robot.utils.UtilityFunctions; public class ShooterSubsystem extends SubsystemBase { @@ -110,6 +113,6 @@ public boolean AtWheelVelocityTarget() { @Override public void periodic() { - + DogLog.log("Motor velocity setpoint", m_shooterClosedLoopController.getSetpoint()); } } From 3ef1dde329695005970e4f6d93f99813941c0d93 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 3 Mar 2026 18:14:12 -0600 Subject: [PATCH 14/29] Properly calculate overshoot for aiming turret --- simgui-ds.json | 8 ++- .../autos/Left Backwards Auto.auto | 19 +++++++ .../pathplanner/autos/Left Forward Auto.auto | 19 +++++++ .../autos/Left Side Neutral Auto.auto | 18 +++++++ .../pathplanner/autos/New New Auto.auto | 12 +++++ .../autos/Right Backwards Auto.auto | 19 +++++++ .../pathplanner/autos/Right Forward Auto.auto | 19 +++++++ ...mple Auto.auto => Uh Oh Forward Auto.auto} | 0 .../paths/Left Side Center Return.path | 8 +-- .../paths/Left Side To Center Path.path | 8 +-- .../paths/Left Straight Forward.path | 54 +++++++++++++++++++ .../pathplanner/paths/Left Straight back.path | 54 +++++++++++++++++++ .../paths/Right Straight Back.path | 54 +++++++++++++++++++ .../paths/Right Straight Forward.path | 54 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 22 ++++---- .../frc/robot/commands/CommandFactory.java | 38 ++++++------- 16 files changed, 363 insertions(+), 43 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Left Backwards Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Left Forward Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/New New Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right Backwards Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Right Forward Auto.auto rename src/main/deploy/pathplanner/autos/{Example Auto.auto => Uh Oh Forward Auto.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Left Straight Forward.path create mode 100644 src/main/deploy/pathplanner/paths/Left Straight back.path create mode 100644 src/main/deploy/pathplanner/paths/Right Straight Back.path create mode 100644 src/main/deploy/pathplanner/paths/Right Straight Forward.path diff --git a/simgui-ds.json b/simgui-ds.json index c77974a..fa5e969 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,6 +4,11 @@ "visible": true } }, + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -100,7 +105,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/deploy/pathplanner/autos/Left Backwards Auto.auto b/src/main/deploy/pathplanner/autos/Left Backwards Auto.auto new file mode 100644 index 0000000..e11868c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Backwards Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Straight back" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Forward Auto.auto b/src/main/deploy/pathplanner/autos/Left Forward Auto.auto new file mode 100644 index 0000000..af51163 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Forward Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Straight Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto index c32e656..efc3dea 100644 --- a/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto +++ b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto @@ -45,6 +45,24 @@ "data": { "name": "Shoot" } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Stop Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop Aim" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto new file mode 100644 index 0000000..440a1ea --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New New Auto.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Backwards Auto.auto b/src/main/deploy/pathplanner/autos/Right Backwards Auto.auto new file mode 100644 index 0000000..963159f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Backwards Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Straight Back" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Forward Auto.auto b/src/main/deploy/pathplanner/autos/Right Forward Auto.auto new file mode 100644 index 0000000..8992651 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Forward Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Straight Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Uh Oh Forward Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Example Auto.auto rename to src/main/deploy/pathplanner/autos/Uh Oh Forward Auto.auto diff --git a/src/main/deploy/pathplanner/paths/Left Side Center Return.path b/src/main/deploy/pathplanner/paths/Left Side Center Return.path index 8b0747c..840634f 100644 --- a/src/main/deploy/pathplanner/paths/Left Side Center Return.path +++ b/src/main/deploy/pathplanner/paths/Left Side Center Return.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.428987161198288, - "y": 6.279857346647646 + "x": 7.636611111111112, + "y": 5.917574893010523 }, "prevControl": null, "nextControl": { - "x": 7.493680456490727, - "y": 7.845435092724678 + "x": 7.578144444444446, + "y": 7.87431111111111 }, "isLocked": false, "linkedName": "Left Center Intake Location" diff --git a/src/main/deploy/pathplanner/paths/Left Side To Center Path.path b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path index 9afe055..3c2f2fc 100644 --- a/src/main/deploy/pathplanner/paths/Left Side To Center Path.path +++ b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.428987161198288, - "y": 6.279857346647646 + "x": 7.636611111111112, + "y": 5.917574893010523 }, "prevControl": { - "x": 5.941041369472183, - "y": 6.991483594864479 + "x": 6.148665319385007, + "y": 6.629201141227356 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Straight Forward.path b/src/main/deploy/pathplanner/paths/Left Straight Forward.path new file mode 100644 index 0000000..93894da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Straight Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.338366666666666 + }, + "prevControl": null, + "nextControl": { + "x": 5.0, + "y": 7.338366666666666 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.648744444444444, + "y": 7.338366666666666 + }, + "prevControl": { + "x": 4.648744444444444, + "y": 7.338366666666666 + }, + "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": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Straight back.path b/src/main/deploy/pathplanner/paths/Left Straight back.path new file mode 100644 index 0000000..80a0060 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Straight back.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.348111111111112 + }, + "prevControl": null, + "nextControl": { + "x": 3.5439444444444446, + "y": 7.3675999999999995 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.037233333333333, + "y": 7.348111111111112 + }, + "prevControl": { + "x": 3.592666666666667, + "y": 7.3675999999999995 + }, + "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": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Straight Back.path b/src/main/deploy/pathplanner/paths/Right Straight Back.path new file mode 100644 index 0000000..2b41d0a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Straight Back.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.807044444444445, + "y": 0.6829111111111104 + }, + "prevControl": null, + "nextControl": { + "x": 3.2808444444444445, + "y": 0.6634222222222216 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6474555555555552, + "y": 0.6829111111111104 + }, + "prevControl": { + "x": 3.290588888888889, + "y": 0.6829111111111104 + }, + "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": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Straight Forward.path b/src/main/deploy/pathplanner/paths/Right Straight Forward.path new file mode 100644 index 0000000..bb8b92a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Straight Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.7290888888888887, + "y": 0.6341888888888878 + }, + "prevControl": null, + "nextControl": { + "x": 4.729088888888888, + "y": 0.6341888888888878 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.288200000000001, + "y": 0.6341888888888878 + }, + "prevControl": { + "x": 4.288200000000001, + "y": 0.6341888888888878 + }, + "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": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 67aac7b..3c67fe6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,8 +7,6 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; - import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -26,18 +24,14 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.OIConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.commands.CommandFactory; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.*; @@ -46,7 +40,6 @@ public class RobotContainer { OIConstants.kDriverControllerPort); private String m_autoSelected; - private final SendableChooser m_chooser = new SendableChooser<>(); private final TurretSubsystem m_turret = new TurretSubsystem(); private final ShooterSubsystem m_shooter = new ShooterSubsystem(); @@ -63,6 +56,8 @@ public class RobotContainer { DoubleSubscriber m_shooterVelocityGetter = DogLog.tunable("Motor Velocity (in RPM)", ShooterConstants.kShooterStartVelocity); + SendableChooser m_sendable = new SendableChooser<>(); + public RobotContainer() { NamedCommands.registerCommand("Deploy Intake", m_commandFactory.DeployIntake()); NamedCommands.registerCommand("Retract Intake", m_commandFactory.RetractIntake()); @@ -70,13 +65,18 @@ public RobotContainer() { NamedCommands.registerCommand("Stop Aim", m_commandFactory.StopAimCommand()); NamedCommands.registerCommand("Shoot", m_commandFactory.ShootCommand()); NamedCommands.registerCommand("Stop Shoot", m_commandFactory.StopShootCommand()); - - m_chooser.setDefaultOption("Example Auto", AutoConstants.kExampleAutoName); - SmartDashboard.putData("Auto Choices", m_chooser); // SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); // SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); // SmartDashboard.putNumber("Turret angle in degrees", 0.0); + m_sendable.addOption("Right Forward Auto", "Right Forward Auto"); + m_sendable.addOption("Left Forward Auto", "Left Forward Auto"); + m_sendable.addOption("Right Backwards Auto", "Right Backwards Auto"); + m_sendable.addOption("Left Backwards Auto", "Left Backwards Auto"); + m_sendable.addOption("Left Side Neutral Auto", "Left Side Neutral Auto"); + m_sendable.setDefaultOption("Left Side Neutral Auto", "Left Side Neutral Auto"); + SmartDashboard.putData(m_sendable); + // Configure the button bindings configureBindings(); @@ -161,7 +161,7 @@ private void configureBindings() { } public Command getAutonomousCommand() { - m_autoSelected = m_chooser.getSelected(); + m_autoSelected = m_sendable.getSelected(); System.out.print(m_autoSelected); diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 0937e3c..936c661 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -150,18 +150,6 @@ public Command AimTurretRelativeToRobot(Angle angle) { }, m_turret).until(m_turret::atTarget); } - public Command ShootWhileAimedCommand() { - - // DogLog.log("Aiming", m_isAiming); - - return new ConditionalCommand(ShootCommand(), - AimHoodToPositionCommand(ShooterConstants.kNonAimHoodAngle) - .alongWith(AimTurretRelativeToRobot(TurretConstants.kNonAimTurretAngle)) - .andThen(new RunCommand(this::Shoot, m_shooter)), - () -> m_isAiming).until(m_shooter::AtWheelVelocityTarget).andThen(RunAllStager()) - .finallyDo(m_shooter::Stop); - } - public Command RunAllStager() { return new InstantCommand(() -> { m_stager.Agitate(); @@ -286,15 +274,19 @@ public Command PointAtHub(boolean isRed) { } public void MoveTurretToHeading(Angle heading) { - Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); - // Weird bug with red side position data - Angle offset = DriverStation.getAlliance().get() == Alliance.Red && !Robot.isReal() ? Constants.NumericalConstants.kHalfRotation : Constants.NumericalConstants.kNoRotation; + Angle offset = DriverStation.getAlliance().get() == Alliance.Red && Robot.isReal() + ? Constants.NumericalConstants.kHalfRotation + : Constants.NumericalConstants.kNoRotation; + + heading = heading.plus(offset); - Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading).plus(offset)); + Angle robotHeading = UtilityFunctions.WrapAngle(m_drive.getHeading()); + + Angle robotRelativeTurretAngle = UtilityFunctions.WrapAngle(heading.minus(robotHeading)); // Angle[] currentRange = getCurrentTurretRange(); - Angle[] currentRange = TurretConstants.kUnrestrictedAngles; + Angle[] currentRange = getCurrentTurretRange(); if (withinAngles(currentRange, robotRelativeTurretAngle)) { m_turret.moveToAngle(robotRelativeTurretAngle); @@ -304,7 +296,7 @@ public void MoveTurretToHeading(Angle heading) { // The overshoot is negative if the robot has to move in a negative direction; // same for positive - Angle overshoot = robotRelativeTurretAngle.minus(closest).in(Degrees) < 0.0 + Angle overshoot = UtilityFunctions.angleDiff(robotRelativeTurretAngle, closest).in(Degrees) < 0.0 ? TurretConstants.kOvershootAmount : TurretConstants.kOvershootAmount.times(-1.0); @@ -437,11 +429,11 @@ private static Angle angleFromTranslation(Translation2d reference, Translation2d } private static boolean withinRange(Angle min, Angle max, Angle a) { - a = UtilityFunctions.WrapAngle(a); - min = UtilityFunctions.WrapAngle(min); - max = UtilityFunctions.WrapAngle(max); + Angle a1 = UtilityFunctions.WrapAngle(a); + Angle min1 = UtilityFunctions.WrapAngle(min); + Angle max1 = UtilityFunctions.WrapAngle(max); - return a.gt(min) && a.lt(max); + return a1.gt(min1) && a1.lt(max1); } private static Angle getClosestAngle(Angle a, Angle... others) { @@ -614,7 +606,7 @@ public Command Shoot(AngularVelocity shooterWheelVelocity) { // hitting the side of the robot or other balls currently being held in the // robot private Angle[] getCurrentTurretRange() { - if (m_shooter.GetHoodAngle().lt(ShooterConstants.kTurretAngleRestrictiveShooterAngle)) { + if (m_shooter.GetHoodAngle().gt(ShooterConstants.kTurretAngleRestrictiveShooterAngle)) { return TurretConstants.kRestrictedAngles; } From 2c6ffd60295809bda688138f8e5c74f57268fc48 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 3 Mar 2026 18:27:35 -0600 Subject: [PATCH 15/29] Add zero gyro for drivetrain --- src/main/java/frc/robot/Constants.java | 3 +-- src/main/java/frc/robot/RobotContainer.java | 7 +++++++ src/main/java/frc/robot/subsystems/DriveSubsystem.java | 4 ++++ src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 3 +-- 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3da7e96..5537dd5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -262,7 +262,6 @@ public static final class TurretConstants { public static final Angle kTurretAngleTolerance = Degrees.of(2.0); - // TODO: Change to real numbers public static Angle kNonAimTurretAngle = Degrees.of(0.0); public static int kTurretMotorAmpLimit = 10; public static final Angle kTurretTorwardsFront = Degrees.of(350); @@ -341,7 +340,7 @@ public static final class ShooterConstants { public static final Angle kHoodFeedingPosition = Degrees.of(25.0); public static final Measure kTurretAngleRestrictiveShooterAngle = Degrees.of(10); - public static final Angle kHoodStartingAngle = Degrees.of(0.0); + public static final Angle kHoodStartingAngle = Degrees.of(3.0); public static final AngularVelocity kShooterStartVelocity = RPM.of(0.0); public static final Angle kDefaultHoodPosition = Degrees.of(3.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3c67fe6..71916ad 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -56,6 +57,8 @@ public class RobotContainer { DoubleSubscriber m_shooterVelocityGetter = DogLog.tunable("Motor Velocity (in RPM)", ShooterConstants.kShooterStartVelocity); + BooleanSubscriber m_zeroGyroGetter = DogLog.tunable("Zero Gyro", false); + SendableChooser m_sendable = new SendableChooser<>(); public RobotContainer() { @@ -197,6 +200,10 @@ public void teleopPeriodic() { m_field.getObject("targetPose").setPose(targetPose); m_commandFactory.periodic(); + + if (m_zeroGyroGetter.get()) { + m_drive.ZeroGyro(); + } } public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index c8e4e95..a604ff0 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -299,6 +299,10 @@ public SwerveModulePosition[] getModulePositions() { }; } + public void ZeroGyro() { + pidgey.setYaw(NumericalConstants.kNoRotation); + } + /** * Resets the odometry to the specified pose. * diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 2f91e8b..fa6885c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -80,8 +80,7 @@ public void MoveHoodToPosition(Angle angle) { var currentPosition = m_absoluteEncoder.getPosition(); - if (currentPosition > ShooterConstants.kHoodMaxAbsolutePosition - && currentPosition < ShooterConstants.kWrapBackMin.in(Rotations)) { + if (currentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { System.out.println("Hood position incorrect for safe movement. Pos: " + currentPosition); return; } From 5195b833247a23ab93046323a1b3a35ffeebd062 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 3 Mar 2026 20:43:38 -0600 Subject: [PATCH 16/29] Fix right straight auto, autos don't work, lower shooting threshold --- .../deploy/pathplanner/paths/Right Straight Forward.path | 2 +- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 4 ++++ src/main/java/frc/robot/commands/CommandFactory.java | 8 +++++++- 5 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Right Straight Forward.path b/src/main/deploy/pathplanner/paths/Right Straight Forward.path index bb8b92a..159fa85 100644 --- a/src/main/deploy/pathplanner/paths/Right Straight Forward.path +++ b/src/main/deploy/pathplanner/paths/Right Straight Forward.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 4.288200000000001, - "y": 0.6341888888888878 + "y": 0.6341888888888879 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 8e445b7..e18caf3 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -15,7 +15,7 @@ "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60", + "driveMotorType": "vortex", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, "flModuleX": 0.273, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5537dd5..562b4c7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -282,7 +282,7 @@ public static final class ShooterConstants { public static final double kShooterD = 0.0; public static final double kShooterFF = 0.0019; - public static final AngularVelocity kShooterVelocityTolerance = RPM.of(500); + public static final AngularVelocity kShooterVelocityTolerance = RPM.of(50); // Teeth on encoder gear to teeth on shaft, teeth on shaft to teeth on hood part // NOTE: Need to use 14D so the result is a double, otherwise you end up with @@ -356,7 +356,7 @@ public static final class StagingConstants { public static final int kAgitationAmpLimit = 4; public static int kFeedIntoHoodMotor = 16; - public static double kFeedIntoHoodSpeed = 0.10; + public static double kFeedIntoHoodSpeed = 0.5; public static double kReverseFeedSpeed = -0.1; public static final int kAgitationMotorId = 9; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 71916ad..9eab1f3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; @@ -152,6 +153,9 @@ private void configureBindings() { .onTrue(m_commandFactory.DeployIntake().alongWith(m_commandFactory.SpinIntake())) .onFalse(m_commandFactory.RetractIntake().alongWith(m_commandFactory.StopIntake())); + m_driverController.povUp().onTrue( + m_commandFactory.AimHoodToPositionCommand(commandedShooterAngle).andThen(new PrintCommand("Hehe"))); + // m_driverController.x().onTrue(new InstantCommand(() -> { // // double hoodAngle = m_hoodAngleGetter.get(); // // m_aimFactory.MoveHoodToAngle(Degrees.of(hoodAngle)); diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 936c661..7ca7a64 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -96,6 +96,12 @@ public Command AimTurretToFrontCommand() { return new InstantCommand(this::AimTurretToFrontCommand); } + public Command MoveHoodToDefaultPosition() { + return new InstantCommand(() -> { + m_shooter.MoveHoodToPosition(ShooterConstants.kDefaultHoodPosition); + }); + } + private void Aim(boolean isFeedingLeftSide) { Fixtures.FieldLocations location = m_drive.getRobotLocation(); @@ -110,7 +116,7 @@ private void Aim(boolean isFeedingLeftSide) { solution = m_solution; } - MoveTurretToHeading(solution.hubAngle().minus(solution.phi())); + MoveTurretToHeading(solution.hubAngle().plus(solution.phi())); // DogLog.log("Range from hub (meters)", solution.distance().in(Meters)); // System.out.println(solution.phi()); m_shooter.MoveHoodToPosition(solution.hoodAngle()); From 71874e6550ddfc1d0f56b01ff457144bd08d989f Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Wed, 4 Mar 2026 18:00:49 -0600 Subject: [PATCH 17/29] Working on stage code --- src/main/deploy/pathplanner/navgrid.json | 2 +- src/main/java/frc/robot/Constants.java | 7 ++-- src/main/java/frc/robot/RobotContainer.java | 38 ++++++++++++------- .../frc/robot/commands/CommandFactory.java | 31 +++++++++++---- .../robot/subsystems/ClimberSubsystem.java | 6 ++- .../frc/robot/subsystems/DriveSubsystem.java | 15 +++++--- .../robot/subsystems/ShooterSubsystem.java | 4 +- .../frc/robot/subsystems/TurretSubsystem.java | 6 +++ src/main/java/frc/robot/utils/Vision.java | 3 +- 9 files changed, 77 insertions(+), 35 deletions(-) diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index ac5f521..a6fa85a 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ 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 562b4c7..8741ff6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -215,6 +215,7 @@ public static final class NumericalConstants { public static final Angle kNoRotation = Radians.of(0.0); public static final LinearAcceleration kGravity = MetersPerSecondPerSecond.of(9.807); public static final Angle kHalfRotation = Degrees.of(180); + public static final AngularVelocity kNoRotations = RPM.of(0.0); } public static final class TurretConstants { @@ -264,7 +265,7 @@ public static final class TurretConstants { public static Angle kNonAimTurretAngle = Degrees.of(0.0); public static int kTurretMotorAmpLimit = 10; - public static final Angle kTurretTorwardsFront = Degrees.of(350); + public static final Angle kTurretTorwardsFront = Degrees.of(180); public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.605); } @@ -360,11 +361,11 @@ public static final class StagingConstants { public static double kReverseFeedSpeed = -0.1; public static final int kAgitationMotorId = 9; - public static final double kAgitationSpeed = -0.45; + public static final double kAgitationSpeed = -0.60; public static double kReverseAgitationSpeed = 0.1; public static final int kRollerMotorId = 12; - public static final double kRollerSpeed = 0.4; + public static final double kRollerSpeed = 0.75; public static double kReverseRollingSpeed = -0.1; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9eab1f3..a4a7a47 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,9 +45,9 @@ public class RobotContainer { private final TurretSubsystem m_turret = new TurretSubsystem(); private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - private final DriveSubsystem m_drive = new DriveSubsystem(m_turret::getRotationAtTime); + private final DriveSubsystem m_drive; - CommandFactory m_commandFactory = new CommandFactory(m_drive, m_turret, m_shooter); + CommandFactory m_commandFactory; Field2d m_field; // For getting data points for the lookup table @@ -63,12 +63,15 @@ public class RobotContainer { SendableChooser m_sendable = new SendableChooser<>(); public RobotContainer() { - NamedCommands.registerCommand("Deploy Intake", m_commandFactory.DeployIntake()); - NamedCommands.registerCommand("Retract Intake", m_commandFactory.RetractIntake()); - NamedCommands.registerCommand("Aim", m_commandFactory.AimCommand(false)); - NamedCommands.registerCommand("Stop Aim", m_commandFactory.StopAimCommand()); - NamedCommands.registerCommand("Shoot", m_commandFactory.ShootCommand()); - NamedCommands.registerCommand("Stop Shoot", m_commandFactory.StopShootCommand()); + // NamedCommands.registerCommand("Deploy Intake", + // m_commandFactory.DeployIntake()); + // NamedCommands.registerCommand("Retract Intake", + // m_commandFactory.RetractIntake()); + // NamedCommands.registerCommand("Aim", m_commandFactory.AimCommand(false)); + // NamedCommands.registerCommand("Stop Aim", m_commandFactory.StopAimCommand()); + // NamedCommands.registerCommand("Shoot", m_commandFactory.ShootCommand()); + // NamedCommands.registerCommand("Stop Shoot", + // m_commandFactory.StopShootCommand()); // SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); // SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); // SmartDashboard.putNumber("Turret angle in degrees", 0.0); @@ -81,6 +84,10 @@ public RobotContainer() { m_sendable.setDefaultOption("Left Side Neutral Auto", "Left Side Neutral Auto"); SmartDashboard.putData(m_sendable); + m_drive = new DriveSubsystem(m_turret::getRotationAtTime); + m_commandFactory = new CommandFactory(); + m_commandFactory.SetSubsystems(m_drive, m_turret, m_shooter); + // Configure the button bindings configureBindings(); @@ -139,11 +146,12 @@ private void configureBindings() { m_driverController.rightBumper().whileTrue(m_commandFactory.AimCommand(false)) .onFalse(m_commandFactory.StopAimCommand()); - m_driverController.a().whileTrue(m_commandFactory.ShootCommand()) - .onTrue(Commands.waitUntil(m_shooter::AtWheelVelocityTarget).andThen(m_commandFactory.RunAllStager())) + m_driverController.a().whileTrue(m_commandFactory.RunAllStager()) + .onTrue(Commands.waitUntil(m_shooter::AtWheelVelocityTarget).andThen( + m_commandFactory.ShootCommand().until(() -> m_driverController.a().getAsBoolean() == false))) .onFalse(m_commandFactory.StopShootCommand().alongWith(m_commandFactory.StopStaging())); - m_driverController.x().whileTrue(m_commandFactory.PointAtHub(false)); + m_driverController.x().whileTrue(m_commandFactory.MoveTurretToFront()); m_driverController.y().onTrue(m_commandFactory.ReverseStager()).onFalse(m_commandFactory.StopStaging()); // m_driverController.povUp().whileTrue(m_commandFactory.ClimbUpCommand()); @@ -153,8 +161,10 @@ private void configureBindings() { .onTrue(m_commandFactory.DeployIntake().alongWith(m_commandFactory.SpinIntake())) .onFalse(m_commandFactory.RetractIntake().alongWith(m_commandFactory.StopIntake())); - m_driverController.povUp().onTrue( - m_commandFactory.AimHoodToPositionCommand(commandedShooterAngle).andThen(new PrintCommand("Hehe"))); + m_driverController.povUp() + .whileTrue(m_commandFactory.ClimbDownCommand().finallyDo(m_commandFactory::StopClimb)); + m_driverController.povDown() + .whileTrue(m_commandFactory.ClimbUpCommand().finallyDo(m_commandFactory::StopClimb)); // m_driverController.x().onTrue(new InstantCommand(() -> { // // double hoodAngle = m_hoodAngleGetter.get(); @@ -170,7 +180,7 @@ private void configureBindings() { public Command getAutonomousCommand() { m_autoSelected = m_sendable.getSelected(); - System.out.print(m_autoSelected); + DogLog.log("Auto Selected", m_autoSelected); return new PathPlannerAuto(m_autoSelected); } diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 7ca7a64..b6ce4b0 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -34,6 +34,7 @@ import frc.robot.Robot; import frc.robot.Constants.Fixtures; import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.TurretConstants; import frc.robot.subsystems.*; @@ -59,10 +60,10 @@ public class CommandFactory { TargetSolution m_solution; - public CommandFactory(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { - m_drive = drive; - m_turret = turret; - m_shooter = shooter; + public CommandFactory() { + // m_drive = drive; + // m_turret = turret; + // m_shooter = shooter; } public Command AimCommand(boolean isFeedingLeftSide) { @@ -78,6 +79,12 @@ public void StopAim() { m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; } + public void SetSubsystems(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { + m_drive = drive; + m_turret = turret; + m_shooter = shooter; + } + public Command StopAimCommand() { return new InstantCommand(this::StopAim); } @@ -85,6 +92,7 @@ public Command StopAimCommand() { public void periodic() { m_solution = GetHubAimSolution(); m_wheelVelocity = m_solution.wheelSpeed(); + DogLog.log("Turret Rotation in deg", m_turret.getRotation().in(Degrees)); DogLog.log("RPM target", m_wheelVelocity.in(RPM)); } @@ -102,6 +110,12 @@ public Command MoveHoodToDefaultPosition() { }); } + public Command MoveTurretToFront() { + return new InstantCommand(() -> { + m_turret.moveToAngle(TurretConstants.kTurretTorwardsFront); + }); + } + private void Aim(boolean isFeedingLeftSide) { Fixtures.FieldLocations location = m_drive.getRobotLocation(); @@ -180,7 +194,7 @@ private void Shoot() { public Command ShootCommand() { return new RunCommand(() -> { Shoot(); - }); + }).finallyDo(this::StopShoot); } public void ShootAtVelocity(AngularVelocity velocity) { @@ -190,6 +204,7 @@ public void ShootAtVelocity(AngularVelocity velocity) { public Command StopShootCommand() { return new InstantCommand(() -> { StopShoot(); + m_wheelVelocity = NumericalConstants.kNoRotations; }); } @@ -342,7 +357,7 @@ public void ClimbUp() { } public void ClimbDown() { - m_climber.climbUp(); + m_climber.climbDown(); } public void StopClimb() { @@ -361,7 +376,7 @@ public Command ClimbUpCommand() { // .alongWith(Commands.waitUntil(m_turret::atTarget)) return (new RunCommand(this::ClimbUp)) - .until(m_climber::atMax) + // .until(m_climber::atMax) // TODO: put this back .finallyDo(this::StopClimb); } @@ -371,7 +386,7 @@ public Command ClimbDownCommand() { // .alongWith(Commands.waitUntil(m_turret::atTarget)) return (new RunCommand(this::ClimbDown)) - .until(m_climber::atMin) + // .until(m_climber::atMin) TODO: put this back .finallyDo(this::StopClimb); } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 3e7b799..8c603d7 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -29,7 +29,8 @@ public double GetPosition() { public void climbUp() { - if (!atMax()) { + // if (!atMax()) { + if (true) { m_climbMotor.set(ClimberConstants.kUpVelocity); } else { Stop(); @@ -38,7 +39,8 @@ public void climbUp() { public void climbDown() { - if (!atMin()) { + // if (!atMin()) { // TODO: Put this back + if (true) { m_climbMotor.set(ClimberConstants.kDownVelocity); } else { Stop(); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index a604ff0..a3089aa 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -134,9 +134,10 @@ public DriveSubsystem(Function turretPositionSupplier) { } AutoBuilder.configure( - this::getPose, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotRelativeSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + () -> getPose(), // Robot pose supplier + (Pose2d pose) -> resetOdometry(pose), // Method to reset odometry (will be called if your auto has a + // starting pose) + () -> getRobotRelativeSpeeds(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE (speeds, feedforwards) -> drive(speeds), // Method that will drive the robot given ROBOT RELATIVE // ChassisSpeeds. Also optionally outputs individual module // feedforwards @@ -309,7 +310,7 @@ public void ZeroGyro() { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition( + m_poseEstimator.resetPosition( new Rotation2d(pidgey.getYaw().getValue()), new SwerveModulePosition[] { m_frontLeft.getPosition(), @@ -376,7 +377,11 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ } public void drive(ChassisSpeeds speeds) { - setModuleStates(DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds)); + var states = DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds); + m_frontLeft.setDesiredState(states[0]); + m_frontRight.setDesiredState(states[1]); + m_rearLeft.setDesiredState(states[2]); + m_rearRight.setDesiredState(states[3]); } /** diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index fa6885c..a6a3576 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -94,7 +94,7 @@ public void Spin(AngularVelocity shootSpeedVelocity) { } public void Stop() { - m_shooterMotor.stopMotor(); + m_shooterClosedLoopController.setSetpoint(0, ControlType.kVelocity); } public Angle GetHoodAngle() { @@ -108,6 +108,8 @@ public boolean AtHoodTarget() { public boolean AtWheelVelocityTarget() { return RPM.of(m_shooterClosedLoopController.getSetpoint()).minus(RPM.of(m_shooterRelativeEncoder.getVelocity())) .abs(RPM) < ShooterConstants.kShooterVelocityTolerance.in(RPM); + + // return true; } @Override diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 85ad730..9f153fb 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -24,6 +24,8 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -31,6 +33,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.TurretConstants; import frc.robot.utils.PositionBuffer; import frc.robot.utils.TurretPosition; @@ -137,6 +140,9 @@ public TurretPosition getRotationAtTime(double timestamp) { return m_positionBuffer.getAngleAtTime(timestamp); // return new TurretPosition(getRotation(), RotationsPerSecond.of(0.0), // timestamp); + + // return new TurretPosition(NumericalConstants.kNoRotation, + // NumericalConstants.kNoRotations, timestamp); } @Override diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index 24ec10b..11851b1 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -70,7 +70,8 @@ public void periodic() { cameraTransform = getTurretCameraTransform(result.getTimestampSeconds()); if (cameraTransform == null) { - System.out.println("Turret exceeded max velocity valid for reading april tags."); + // System.out.println("Turret exceeded max velocity valid for reading april + // tags."); break; } From 47acef0115d41534fbac6bd21c04564a6d37eb2f Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 5 Mar 2026 18:12:28 -0600 Subject: [PATCH 18/29] End of thursday code changes --- simgui-ds.json | 8 +- .../pathplanner/autos/Simple Shoot Auto.auto | 19 + src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/Constants.java | 20 +- src/main/java/frc/robot/Robot.java | 7 +- src/main/java/frc/robot/RobotContainer.java | 350 ++++++++++-------- .../frc/robot/commands/CommandFactory.java | 18 +- .../frc/robot/subsystems/DriveSubsystem.java | 33 +- .../frc/robot/subsystems/MAXSwerveModule.java | 6 +- .../robot/subsystems/ShooterSubsystem.java | 35 +- src/main/java/frc/robot/utils/RingBuffer.java | 22 +- src/main/java/frc/robot/utils/Vision.java | 32 +- 12 files changed, 319 insertions(+), 235 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto diff --git a/simgui-ds.json b/simgui-ds.json index fa5e969..1a435f9 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "Keyboard 0 Settings": { - "window": { - "visible": true - } - }, "System Joysticks": { "window": { "enabled": false @@ -108,5 +103,6 @@ "guid": "78696e70757401000000000000000000", "useGamepad": true } - ] + ], + "zeroDisconnectedJoysticks": false } diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto new file mode 100644 index 0000000..72b77e3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index e18caf3..72a5693 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,14 +9,14 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 50.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "vortex", - "driveCurrentLimit": 60.0, + "driveCurrentLimit": 80.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8741ff6..ade55d9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -223,10 +223,10 @@ public static final class TurretConstants { public static final Angle kMinAngle = Rotations.of(0.1); public static final Angle kMaxAngle = Rotations.of(0.854); - public static final int kPositionBufferLength = 4000; + public static final int kPositionBufferLength = 1000; public static final Time kEncoderReadingDelay = Seconds.of(0.005); - public static final Time kEncoderReadInterval = Seconds.of(0.01); + public static final Time kEncoderReadInterval = Seconds.of(0.05); public static final double kP = 1.5; public static final double kI = 0.0; @@ -267,7 +267,7 @@ public static final class TurretConstants { public static int kTurretMotorAmpLimit = 10; public static final Angle kTurretTorwardsFront = Degrees.of(180); - public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.605); + public static final Angle kAngularDistanceToFrontOfRobot = Rotations.of(0.629); } public static final class ShooterConstants { @@ -300,7 +300,7 @@ public static final class ShooterConstants { public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); - public static final Distance kHubRobotTurretOffset = Inches.of(47); + public static final Distance kHubRobotTurretOffset = Inches.of(35); public static final ShootingEntry[] kShootingEntries = { new ShootingEntry(Inches.of(30).plus(kHubRobotTurretOffset), RPM.of(3519), null, @@ -335,7 +335,7 @@ public static final class ShooterConstants { public static final double kHoodDegreeConversionFactor = kHoodMaxAbsolutePosition / 30; // TODO: Change to real numbers - public static final AngularVelocity kNonAimShooterVelocity = RPM.of(500); + public static final AngularVelocity kNonAimShooterVelocity = RPM.of(2000); public static final Angle kNonAimHoodAngle = Degrees.of(15.0); public static final AngularVelocity kFeedingWheelVelocity = RPM.of(4000); public static final Angle kHoodFeedingPosition = Degrees.of(25.0); @@ -351,6 +351,8 @@ public static final class ShooterConstants { // This is the min rotational amount where we can reasonably assume that the // hood has just gone backwards a little too far, beyond the zero of the encoder public static final Angle kWrapBackMin = Rotations.of(0.9); + public static final Time kAutoShootTime = Seconds.of(4.0); + public static final AngularVelocity kDefaultShooterVelocity = RPM.of(4000); } public static final class StagingConstants { @@ -361,11 +363,11 @@ public static final class StagingConstants { public static double kReverseFeedSpeed = -0.1; public static final int kAgitationMotorId = 9; - public static final double kAgitationSpeed = -0.60; + public static final double kAgitationSpeed = -0.75; public static double kReverseAgitationSpeed = 0.1; public static final int kRollerMotorId = 12; - public static final double kRollerSpeed = 0.75; + public static final double kRollerSpeed = 0.95; public static double kReverseRollingSpeed = -0.1; } @@ -403,7 +405,7 @@ public static enum FieldLocations { public static final Angle kFeedOffset = Degrees.of(12); public static final Translation2d kRedHubAprilTag = AprilTagFieldLayout - .loadField(AprilTagFields.k2026RebuiltAndymark) + .loadField(AprilTagFields.k2026RebuiltWelded) // TODO: Change to normal field .getTagPose(3).get().toPose2d().getTranslation(); } @@ -435,7 +437,7 @@ public static final class IntakeConstants { public static final int kDeployMotorCurrentLimit = 40; public static final int kIntakeMotorCurrentLimit = 80; - public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2500); + public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2200); } public static final class ClimberConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b0deed2..c2f1df6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,8 +16,8 @@ public class Robot extends TimedRobot { public Robot() { m_robotContainer = new RobotContainer(); - addPeriodic(m_robotContainer.pushTurretEncoderReading(), - Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); + // addPeriodic(m_robotContainer.pushTurretEncoderReading(), + // Constants.TurretConstants.kEncoderReadInterval.in(Seconds)); } @Override @@ -43,12 +43,13 @@ public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { - CommandScheduler.getInstance().schedule(m_autonomousCommand); + m_autonomousCommand.schedule(); } } @Override public void autonomousPeriodic() { + m_robotContainer.teleopPeriodic(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a4a7a47..0cad895 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,8 +25,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.OIConstants; @@ -38,206 +41,235 @@ import frc.robot.utils.*; public class RobotContainer { - private final CommandXboxController m_driverController = new CommandXboxController( - OIConstants.kDriverControllerPort); - - private String m_autoSelected; - - private final TurretSubsystem m_turret = new TurretSubsystem(); - private final ShooterSubsystem m_shooter = new ShooterSubsystem(); - private final DriveSubsystem m_drive; - - CommandFactory m_commandFactory; - Field2d m_field; - - // For getting data points for the lookup table - Angle commandedShooterAngle; - AngularVelocity commandedWheelVelocity; - - DoubleSubscriber m_hoodAngleGetter = DogLog.tunable("Hood Angle (in degrees)", ShooterConstants.kHoodStartingAngle); - DoubleSubscriber m_shooterVelocityGetter = DogLog.tunable("Motor Velocity (in RPM)", - ShooterConstants.kShooterStartVelocity); - - BooleanSubscriber m_zeroGyroGetter = DogLog.tunable("Zero Gyro", false); - - SendableChooser m_sendable = new SendableChooser<>(); - - public RobotContainer() { - // NamedCommands.registerCommand("Deploy Intake", - // m_commandFactory.DeployIntake()); - // NamedCommands.registerCommand("Retract Intake", - // m_commandFactory.RetractIntake()); - // NamedCommands.registerCommand("Aim", m_commandFactory.AimCommand(false)); - // NamedCommands.registerCommand("Stop Aim", m_commandFactory.StopAimCommand()); - // NamedCommands.registerCommand("Shoot", m_commandFactory.ShootCommand()); - // NamedCommands.registerCommand("Stop Shoot", - // m_commandFactory.StopShootCommand()); - // SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); - // SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); - // SmartDashboard.putNumber("Turret angle in degrees", 0.0); - - m_sendable.addOption("Right Forward Auto", "Right Forward Auto"); - m_sendable.addOption("Left Forward Auto", "Left Forward Auto"); - m_sendable.addOption("Right Backwards Auto", "Right Backwards Auto"); - m_sendable.addOption("Left Backwards Auto", "Left Backwards Auto"); - m_sendable.addOption("Left Side Neutral Auto", "Left Side Neutral Auto"); - m_sendable.setDefaultOption("Left Side Neutral Auto", "Left Side Neutral Auto"); - SmartDashboard.putData(m_sendable); - - m_drive = new DriveSubsystem(m_turret::getRotationAtTime); - m_commandFactory = new CommandFactory(); - m_commandFactory.SetSubsystems(m_drive, m_turret, m_shooter); - - // Configure the button bindings - configureBindings(); + private final CommandXboxController m_driverController = new CommandXboxController( + OIConstants.kDriverControllerPort); + + private String m_autoSelected; + + private final TurretSubsystem m_turret = new TurretSubsystem(); + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + private final DriveSubsystem m_drive; + private boolean m_intakeOut = false; + + CommandFactory m_commandFactory; + Field2d m_field; + + // For getting data points for the lookup table + Angle commandedShooterAngle; + AngularVelocity commandedWheelVelocity; + + DoubleSubscriber m_hoodAngleGetter = DogLog.tunable("Hood Angle (in degrees)", + ShooterConstants.kHoodStartingAngle); + DoubleSubscriber m_shooterVelocityGetter = DogLog.tunable("Motor Velocity (in RPM)", + ShooterConstants.kShooterStartVelocity); + + BooleanSubscriber m_zeroGyroGetter = DogLog.tunable("Zero Gyro", false); + + SendableChooser m_sendable = new SendableChooser<>(); + + public RobotContainer() { + m_drive = new DriveSubsystem(m_turret::getRotationAtTime); + m_commandFactory = new CommandFactory(); + m_commandFactory.SetSubsystems(m_drive, m_turret, m_shooter); + + NamedCommands.registerCommand("Deploy Intake", + m_commandFactory.DeployIntake()); + NamedCommands.registerCommand("Retract Intake", + m_commandFactory.RetractIntake()); + NamedCommands.registerCommand("Aim", m_commandFactory.AimCommand(false)); + NamedCommands.registerCommand("Stop Aim", m_commandFactory.StopAimCommand()); + NamedCommands.registerCommand("Shoot", + m_commandFactory.ShootCommand().alongWith(m_commandFactory.RunAllStager()) + .finallyDo(m_commandFactory::StopStagingCommand)); + NamedCommands.registerCommand("Stop Shoot", + m_commandFactory.StopShootCommand()); + SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); + SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); + SmartDashboard.putNumber("Turret angle in degrees", 0.0); + + m_sendable.addOption("Right Forward Auto", "Right Forward Auto"); + m_sendable.addOption("Left Forward Auto", "Left Forward Auto"); + m_sendable.addOption("Right Backwards Auto", "Right Backwards Auto"); + m_sendable.addOption("Left Backwards Auto", "Left Backwards Auto"); + m_sendable.addOption("Left Side Neutral Auto", "Left Side Neutral Auto"); + m_sendable.setDefaultOption("Right Forward Auto", "Right Forward Auto"); + m_sendable.addOption("Simple Shoot Auto", "Simple Shoot Auto"); + + // Configure the button bindings + configureBindings(); + + // Configure default commands + m_drive.setDefaultCommand( + // The left stick controls translation of the robot. + // Turning is controlled by the X axis of the right stick. + new RunCommand( + () -> m_drive.drive( + -MathUtil.applyDeadband(m_driverController.getLeftY(), + OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(m_driverController.getLeftX(), + OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(m_driverController.getRightX(), + OIConstants.kDriveDeadband), + true), + m_drive)); + + m_field = m_drive.getField(); + SmartDashboard.putData(m_sendable); + } - // Configure default commands - m_drive.setDefaultCommand( - // The left stick controls translation of the robot. - // Turning is controlled by the X axis of the right stick. - new RunCommand( - () -> m_drive.drive( - -MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband), - true), - m_drive)); + private void configureBindings() { + // m_driverController.a() + // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - m_field = m_drive.getField(); - } + // m_driverController.b() + // .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle + // in degrees", 0.0)), + // Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); - private void configureBindings() { - // m_driverController.a() - // .whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + // m_driverController.a().whileTrue(m_aimFactory.ShootCommand()); - // m_driverController.b() - // .whileTrue(m_aimFactory.Aim(Degrees.of(SmartDashboard.getNumber("Turret angle - // in degrees", 0.0)), - // Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)))); + // System.out.println("Bindings configured"); + // m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); - // m_driverController.a().whileTrue(m_aimFactory.ShootCommand()); + // m_driverController.x().whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); - // System.out.println("Bindings configured"); - // m_driverController.x().whileTrue(m_aimFactory.PointAtHub(true)); + // m_driverController.x().whileTrue( + // m_aimFactory.Shoot(ShooterConstants.kFeedingWheelVelocity) + // .finallyDo(() -> m_aimFactory.Shoot(RPM.of(0.0)))); - // m_driverController.x().whileTrue(m_aimFactory.MoveTurretToHeadingCommand(Degrees.of(40))); + // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); - // m_driverController.x().whileTrue( - // m_aimFactory.Shoot(ShooterConstants.kFeedingWheelVelocity) - // .finallyDo(() -> m_aimFactory.Shoot(RPM.of(0.0)))); + // m_driverController.rightBumper().whileTrue(m_aimFactory.AimCommand(false)); + // m_driverController.leftBumper().whileTrue(m_aimFactory.AimCommand(true)); - // m_driverController.y().whileTrue(m_aimFactory.RunAllStager()); + // m_driverController.a().whileTrue(m_aimFactory.RunAllStager()); - // m_driverController.rightBumper().whileTrue(m_aimFactory.AimCommand(false)); - // m_driverController.leftBumper().whileTrue(m_aimFactory.AimCommand(true)); + // m_driverController.y().onTrue(new InstantCommand(() -> { + // double shooterVelocity = m_shooterVelocityGetter.get(); + // m_aimFactory.ShootAtVelocity(RPM.of(shooterVelocity)); + // System.out.println("Shooting at velocity of " + shooterVelocity + " RPM."); + // }).andThen(new + // WaitCommand(ShooterConstants.kRampTime)).andThen(m_aimFactory.RunAllStager()) + // .finallyDo(m_aimFactory::StopShoot)); - // m_driverController.a().whileTrue(m_aimFactory.RunAllStager()); + m_driverController.leftBumper().whileTrue(m_commandFactory.AimCommand(true)) + .onFalse(m_commandFactory.StopAimCommand()); + m_driverController.rightBumper().whileTrue(m_commandFactory.AimCommand(false)) + .onFalse(m_commandFactory.StopAimCommand()); - // m_driverController.y().onTrue(new InstantCommand(() -> { - // double shooterVelocity = m_shooterVelocityGetter.get(); - // m_aimFactory.ShootAtVelocity(RPM.of(shooterVelocity)); - // System.out.println("Shooting at velocity of " + shooterVelocity + " RPM."); - // }).andThen(new - // WaitCommand(ShooterConstants.kRampTime)).andThen(m_aimFactory.RunAllStager()) - // .finallyDo(m_aimFactory::StopShoot)); + m_driverController.rightTrigger().whileTrue(m_commandFactory.RunAllStager()) + .onTrue(Commands.waitUntil(m_shooter::AtWheelVelocityTarget).andThen( + m_commandFactory.ShootCommand().until( + () -> m_driverController.rightTrigger() + .getAsBoolean() == false))) + .onFalse(m_commandFactory.StopShootCommand() + .alongWith(m_commandFactory.StopStagingCommand())); - m_driverController.leftBumper().whileTrue(m_commandFactory.AimCommand(true)) - .onFalse(m_commandFactory.StopAimCommand()); - m_driverController.rightBumper().whileTrue(m_commandFactory.AimCommand(false)) - .onFalse(m_commandFactory.StopAimCommand()); + m_driverController.a().onTrue(new InstantCommand(m_drive::ZeroGyro)); - m_driverController.a().whileTrue(m_commandFactory.RunAllStager()) - .onTrue(Commands.waitUntil(m_shooter::AtWheelVelocityTarget).andThen( - m_commandFactory.ShootCommand().until(() -> m_driverController.a().getAsBoolean() == false))) - .onFalse(m_commandFactory.StopShootCommand().alongWith(m_commandFactory.StopStaging())); + m_driverController.x().whileTrue(m_commandFactory.MoveTurretToFront()); + m_driverController.y().onTrue(m_commandFactory.ReverseStager()) + .onFalse(m_commandFactory.StopStagingCommand()); - m_driverController.x().whileTrue(m_commandFactory.MoveTurretToFront()); - m_driverController.y().onTrue(m_commandFactory.ReverseStager()).onFalse(m_commandFactory.StopStaging()); + // m_driverController.povUp().whileTrue(m_commandFactory.ClimbUpCommand()); + // m_driverController.povDown().whileTrue(m_commandFactory.ClimbDownCommand()); - // m_driverController.povUp().whileTrue(m_commandFactory.ClimbUpCommand()); - // m_driverController.povDown().whileTrue(m_commandFactory.ClimbDownCommand()); + // m_driverController.leftTrigger() + // .onTrue(m_commandFactory.DeployIntake().alongWith(m_commandFactory.SpinIntake())) + // .onFalse(m_commandFactory.RetractIntake().alongWith(m_commandFactory.StopIntake())); - m_driverController.leftTrigger() - .onTrue(m_commandFactory.DeployIntake().alongWith(m_commandFactory.SpinIntake())) - .onFalse(m_commandFactory.RetractIntake().alongWith(m_commandFactory.StopIntake())); + m_driverController.leftTrigger() + .onTrue(new ConditionalCommand( + m_commandFactory.DeployIntake() + .alongWith(m_commandFactory.SpinIntake()), + m_commandFactory.RetractIntake() + .alongWith(m_commandFactory.StopIntake()), + () -> { + m_intakeOut = !m_intakeOut; + return m_intakeOut; + })); - m_driverController.povUp() - .whileTrue(m_commandFactory.ClimbDownCommand().finallyDo(m_commandFactory::StopClimb)); - m_driverController.povDown() - .whileTrue(m_commandFactory.ClimbUpCommand().finallyDo(m_commandFactory::StopClimb)); + m_driverController.povUp() + .whileTrue(m_commandFactory.ClimbDownCommand().finallyDo(m_commandFactory::StopClimb)); + m_driverController.povDown() + .whileTrue(m_commandFactory.ClimbUpCommand().finallyDo(m_commandFactory::StopClimb)); - // m_driverController.x().onTrue(new InstantCommand(() -> { - // // double hoodAngle = m_hoodAngleGetter.get(); - // // m_aimFactory.MoveHoodToAngle(Degrees.of(hoodAngle)); - // })); + // m_driverController.x().onTrue(new InstantCommand(() -> { + // // double hoodAngle = m_hoodAngleGetter.get(); + // // m_aimFactory.MoveHoodToAngle(Degrees.of(hoodAngle)); + // })); - // m_driverController.a().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(15))); + // m_driverController.a().onTrue(m_aimFactory.MoveHoodToAbsoluteCommand(Degrees.of(15))); - // m_driverController.b().onTrue(m_aimFactory.ShootCommand()).onFalse(m_aimFactory.StopShoot()); + // m_driverController.b().onTrue(m_aimFactory.ShootCommand()).onFalse(m_aimFactory.StopShoot()); - } + } - public Command getAutonomousCommand() { - m_autoSelected = m_sendable.getSelected(); + public Command getAutonomousCommand() { + m_autoSelected = m_sendable.getSelected(); - DogLog.log("Auto Selected", m_autoSelected); + DogLog.log("Auto Selected", m_autoSelected); - return new PathPlannerAuto(m_autoSelected); - } + return new PathPlannerAuto(m_autoSelected); + } - public Runnable pushTurretEncoderReading() { - return () -> { - m_turret.pushCurrentEncoderReading(); - }; - } + public Runnable pushTurretEncoderReading() { + return () -> { + m_turret.pushCurrentEncoderReading(); + }; + } - public Command feedPosition(Alliance alliance) { - return new RunCommand(() -> { + public Command feedPosition(Alliance alliance) { + return new RunCommand(() -> { - }, m_drive, m_turret); - } + }, m_drive, m_turret); + } - public void teleopPeriodic() { - m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); + public void teleopPeriodic() { + m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); - TargetSolution solution = m_commandFactory.GetHubAimSolution(); + TargetSolution solution = m_commandFactory.GetHubAimSolution(); - Pose2d robotPose = m_drive.getPose(); + Pose2d robotPose = m_drive.getPose(); - Distance xDist = Meters.of(solution.distance().in(Meters) - * Math.cos(solution.hubAngle().minus(solution.phi()).in(Radians))).plus(robotPose.getMeasureX()); - Distance yDist = Meters.of(solution.distance().in(Meters) - * Math.sin(solution.hubAngle().minus(solution.phi()).in(Radians))).plus(robotPose.getMeasureY()); + Distance xDist = Meters.of(solution.distance().in(Meters) + * Math.cos(solution.hubAngle().minus(solution.phi()).in(Radians))) + .plus(robotPose.getMeasureX()); + Distance yDist = Meters.of(solution.distance().in(Meters) + * Math.sin(solution.hubAngle().minus(solution.phi()).in(Radians))) + .plus(robotPose.getMeasureY()); - Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); + Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); - m_field.getObject("targetPose").setPose(targetPose); + m_field.getObject("targetPose").setPose(targetPose); - m_commandFactory.periodic(); + m_commandFactory.periodic(); - if (m_zeroGyroGetter.get()) { - m_drive.ZeroGyro(); + if (m_zeroGyroGetter.get()) { + m_drive.ZeroGyro(); + } } - } - public void periodic() { - commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); - commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); + public void periodic() { + // commandedWheelVelocity = RPM.of(SmartDashboard.getNumber("Wheelspeed in + // rotations per second", 0.0)); + // commandedShooterAngle = Degrees.of(SmartDashboard.getNumber("Shooter hood + // angle in degrees", 0.0)); - // DogLog.log("At Shooter Velocity Target", m_shooter.AtWheelVelocityTarget()); + // DogLog.log("At Shooter Velocity Target", m_shooter.AtWheelVelocityTarget()); - // System.out.println(m_drive.getRobotLocation()); - } + // System.out.println(m_drive.getRobotLocation()); + } - private Angle getSmartdashBoardRequestedShooterAngle() { - return Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); - } + private Angle getSmartdashBoardRequestedShooterAngle() { + return Degrees.of(SmartDashboard.getNumber("Shooter hood angle in degrees", 0.0)); + } - private AngularVelocity getSmartdashboardRequestedWheelSpeed() { - return RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); - } + private AngularVelocity getSmartdashboardRequestedWheelSpeed() { + return RPM.of(SmartDashboard.getNumber("Wheelspeed in rotations per second", 0.0)); + } - private Angle getSmartdashBoardRequestedTurretAngle() { - return Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)); - } + private Angle getSmartdashBoardRequestedTurretAngle() { + return Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)); + } } diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index b6ce4b0..8673a0b 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -91,7 +91,11 @@ public Command StopAimCommand() { public void periodic() { m_solution = GetHubAimSolution(); - m_wheelVelocity = m_solution.wheelSpeed(); + if (m_isAiming) { + m_wheelVelocity = m_solution.wheelSpeed(); + } else { + m_wheelVelocity = ShooterConstants.kDefaultShooterVelocity; + } DogLog.log("Turret Rotation in deg", m_turret.getRotation().in(Degrees)); DogLog.log("RPM target", m_wheelVelocity.in(RPM)); } @@ -178,14 +182,18 @@ public Command RunAllStager() { }, m_stager); } - public Command StopStaging() { + public Command StopStagingCommand() { return new InstantCommand(() -> { - m_stager.StopAgitate(); - m_stager.StopFeed(); - m_stager.StopRoll(); + StopStaging(); }); } + public void StopStaging() { + m_stager.StopAgitate(); + m_stager.StopFeed(); + m_stager.StopRoll(); + } + private void Shoot() { // System.out.println(m_wheelVelocity + " is wheel velocity"); m_shooter.Spin(m_wheelVelocity); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index a3089aa..a40f34d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -361,23 +361,38 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ // System.out.println("Target " + m_targetAutoAngle + ", Current" + // getHeading()); - final var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(ChassisSpeeds.discretize( + // final var swerveModuleStates = + // DriveConstants.kDriveKinematics.toSwerveModuleStates(ChassisSpeeds.discretize( + // fieldRelative + // ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, + // rotDelivered, + // new Rotation2d(getHeading())) + // : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered), + // timeElapsed)); + // SwerveDriveKinematics.desaturateWheelSpeeds( + // swerveModuleStates, DriveConstants.kMaxSpeed.magnitude()); + + var speeds = ChassisSpeeds.discretize( fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, + rotDelivered, new Rotation2d(getHeading())) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered), - timeElapsed)); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.kMaxSpeed.magnitude()); + timeElapsed); + + drive(speeds); - m_frontLeft.setDesiredState(swerveModuleStates[0]); - m_frontRight.setDesiredState(swerveModuleStates[1]); - m_rearLeft.setDesiredState(swerveModuleStates[2]); - m_rearRight.setDesiredState(swerveModuleStates[3]); + // m_frontLeft.setDesiredState(swerveModuleStates[0]); + // m_frontRight.setDesiredState(swerveModuleStates[1]); + // m_rearLeft.setDesiredState(swerveModuleStates[2]); + // m_rearRight.setDesiredState(swerveModuleStates[3]); } public void drive(ChassisSpeeds speeds) { var states = DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds); + + DogLog.log("First commanded motor speeds", states[0]); + m_frontLeft.setDesiredState(states[0]); m_frontRight.setDesiredState(states[1]); m_rearLeft.setDesiredState(states[2]); diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index c92206f..ca35797 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -9,8 +9,10 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.units.measure.Distance; import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -30,7 +32,7 @@ import frc.robot.Robot; public class MAXSwerveModule { - private final SparkMax m_drivingSpark; + private final SparkFlex m_drivingSpark; private final SparkMax m_turningSpark; private final RelativeEncoder m_drivingEncoder; @@ -53,7 +55,7 @@ public class MAXSwerveModule { * Encoder. */ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { - m_drivingSpark = new SparkMax(drivingCANId, MotorType.kBrushless); + m_drivingSpark = new SparkFlex(drivingCANId, MotorType.kBrushless); m_turningSpark = new SparkMax(turningCANId, MotorType.kBrushless); m_drivingEncoder = m_drivingSpark.getEncoder(); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index a6a3576..883bab5 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -21,18 +21,21 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.NumericalConstants; import frc.robot.Constants.ShooterConstants; public class ShooterSubsystem extends SubsystemBase { SparkMax m_shooterMotor = new SparkMax(ShooterConstants.kShooterMotorId, MotorType.kBrushless); - SparkMax m_hoodMotor = new SparkMax(ShooterConstants.kHoodMotorId, MotorType.kBrushless); + // SparkMax m_hoodMotor = new SparkMax(ShooterConstants.kHoodMotorId, + // MotorType.kBrushless); - AbsoluteEncoder m_absoluteEncoder = m_hoodMotor.getAbsoluteEncoder(); + // AbsoluteEncoder m_absoluteEncoder = m_hoodMotor.getAbsoluteEncoder(); RelativeEncoder m_shooterRelativeEncoder = m_shooterMotor.getEncoder(); private final SparkClosedLoopController m_shooterClosedLoopController = m_shooterMotor.getClosedLoopController(); - private final SparkClosedLoopController m_hoodClosedLoopController = m_hoodMotor.getClosedLoopController(); + // private final SparkClosedLoopController m_hoodClosedLoopController = + // m_hoodMotor.getClosedLoopController(); private final SparkMaxConfig m_shooterConfig = new SparkMaxConfig(); private final SparkMaxConfig m_hoodConfig = new SparkMaxConfig(); @@ -63,10 +66,11 @@ public ShooterSubsystem() { m_hoodConfig.encoder.positionConversionFactor(1); m_shooterMotor.configure(m_shooterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - m_hoodMotor.configure(m_hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // m_hoodMotor.configure(m_hoodConfig, ResetMode.kResetSafeParameters, + // PersistMode.kPersistParameters); } - // Position between 0 and .55 + // Position between 0 and .55. Disabled hood motor public void MoveHoodToPosition(Angle angle) { // System.out.println("Move hood to position: " + angle); @@ -78,14 +82,16 @@ public void MoveHoodToPosition(Angle angle) { return; } - var currentPosition = m_absoluteEncoder.getPosition(); + // var currentPosition = m_absoluteEncoder.getPosition(); - if (currentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { - System.out.println("Hood position incorrect for safe movement. Pos: " + currentPosition); - return; - } + // if (currentPosition > ShooterConstants.kHoodMaxAbsolutePosition) { + // System.out.println("Hood position incorrect for safe movement. Pos: " + + // currentPosition); + // return; + // } - m_hoodClosedLoopController.setSetpoint(targetPosition, ControlType.kPosition); + // m_hoodClosedLoopController.setSetpoint(targetPosition, + // ControlType.kPosition); } public void Spin(AngularVelocity shootSpeedVelocity) { @@ -98,11 +104,14 @@ public void Stop() { } public Angle GetHoodAngle() { - return Degrees.of(m_absoluteEncoder.getPosition() / ShooterConstants.kHoodDegreeConversionFactor); + // return Degrees.of(m_absoluteEncoder.getPosition() / + // ShooterConstants.kHoodDegreeConversionFactor); + return NumericalConstants.kNoRotation; } public boolean AtHoodTarget() { - return m_hoodClosedLoopController.isAtSetpoint(); + // return m_hoodClosedLoopController.isAtSetpoint(); + return true; } public boolean AtWheelVelocityTarget() { diff --git a/src/main/java/frc/robot/utils/RingBuffer.java b/src/main/java/frc/robot/utils/RingBuffer.java index f15f4ac..d683c8f 100644 --- a/src/main/java/frc/robot/utils/RingBuffer.java +++ b/src/main/java/frc/robot/utils/RingBuffer.java @@ -30,9 +30,7 @@ public void push(T element) throws Exception { } } - synchronized (m_elements) { - m_elements[getTrueIndex(m_head++)] = element; - } + m_elements[getTrueIndex(m_head++)] = element; } public T pop() throws Exception { @@ -41,22 +39,18 @@ public T pop() throws Exception { throw new Exception("Cannot pop from empty ring buffer."); } - synchronized (m_elements) { - return (T) m_elements[getTrueIndex(m_tail++)]; - } + return (T) m_elements[getTrueIndex(m_tail++)]; } public T get(int index) throws Exception { - synchronized (m_elements) { - int trueIndex = m_tail + index; - if (trueIndex > m_head) { - System.out.println("Index " + index + " is out of bounds of the ring buffer."); + int trueIndex = m_tail + index; + if (trueIndex > m_head) { + System.out.println("Index " + index + " is out of bounds of the ring buffer."); - throw new Exception("Index " + index + " out of bounds."); - } - - return (T) m_elements[getTrueIndex(trueIndex)]; + throw new Exception("Index " + index + " out of bounds."); } + + return (T) m_elements[getTrueIndex(trueIndex)]; } public int getLength() { diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index 11851b1..b3aaaac 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -24,7 +24,7 @@ public class Vision { - PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); + // PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); Optional> m_turretPositionSupplier; @@ -46,22 +46,28 @@ public Vision(Optional> turretPositionSupplier, } public void periodic() { - Optional visionEstimationCameraOne = Optional.empty(); + // Disabled Camera One - for (var result : m_camera1.getAllUnreadResults()) { - visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + // Optional visionEstimationCameraOne = Optional.empty(); - if (visionEstimationCameraOne.isEmpty()) { - visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); - } + // for (var result : m_camera1.getAllUnreadResults()) { + // visionEstimationCameraOne = + // m_poseEstimatorOne.estimateLowestAmbiguityPose(result); - updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), m_poseEstimatorOne); + // if (visionEstimationCameraOne.isEmpty()) { + // visionEstimationCameraOne = + // m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + // } - visionEstimationCameraOne.ifPresent(estimation -> { - m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), - estimation.timestampSeconds, getCurrentStdDevs())); - }); - } + // updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), + // m_poseEstimatorOne); + + // visionEstimationCameraOne.ifPresent(estimation -> { + // m_visionConsumer.accept(new + // VisionEstimation(estimation.estimatedPose.toPose2d(), + // estimation.timestampSeconds, getCurrentStdDevs())); + // }); + // } Optional visionEstimationCameraTwo = Optional.empty(); for (var result : m_camera2.getAllUnreadResults()) { From 8563fa74342fc770edb4ce68fd511e017ad72416 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 5 Mar 2026 21:57:00 -0600 Subject: [PATCH 19/29] Get correct position of turret when calculating distance to hub --- src/main/java/frc/robot/Constants.java | 9 ++++++++- src/main/java/frc/robot/commands/CommandFactory.java | 12 +++++++++++- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ade55d9..d4249f5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -261,6 +261,13 @@ public static final class TurretConstants { public static final Translation2d kTurretOffset = new Translation2d(Inches.of(-6.25), Inches.of(6.151)); + public static final Angle kTurretAngularOffset = Radians + .of(Math.atan2(kTurretOffset.getY(), kTurretOffset.getX())); + + public static final Distance kTurretCenterDistanceFromRobotCenter = Meters + .of(Math.sqrt(Math.pow(kTurretOffset.getX(), 2.0) + + Math.pow(kTurretOffset.getY(), 2.0))); + public static final Angle kTurretAngleTolerance = Degrees.of(2.0); public static Angle kNonAimTurretAngle = Degrees.of(0.0); @@ -300,7 +307,7 @@ public static final class ShooterConstants { public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); - public static final Distance kHubRobotTurretOffset = Inches.of(35); + public static final Distance kHubRobotTurretOffset = Inches.of(44); public static final ShootingEntry[] kShootingEntries = { new ShootingEntry(Inches.of(30).plus(kHubRobotTurretOffset), RPM.of(3519), null, diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 8673a0b..2169867 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -254,7 +254,17 @@ public TargetSolution GetHubAimSolution() { Translation2d hubPosition = DriverStation.getAlliance().get() == Alliance.Blue ? Fixtures.kBlueAllianceHub : Fixtures.kRedAllianceHub; - Translation2d turretTranslation = m_drive.getPose().getTranslation().plus(TurretConstants.kTurretOffset); + Pose2d robotPose = m_drive.getPose(); + + Distance turretX = TurretConstants.kTurretCenterDistanceFromRobotCenter + .times(Math.cos(robotPose.getRotation().getMeasure() + .plus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Radians))); + + Distance turretY = TurretConstants.kTurretCenterDistanceFromRobotCenter + .times(Math.sin(robotPose.getRotation().getMeasure() + .plus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Radians))); + + Translation2d turretTranslation = new Translation2d(turretX, turretY); Translation2d translationToHub = hubPosition.minus(turretTranslation); From dd9c267c63b595e740f49ad1ce12f2c980fdfa71 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 5 Mar 2026 22:02:13 -0600 Subject: [PATCH 20/29] Add robot pose to turret measures x and y --- src/main/java/frc/robot/commands/CommandFactory.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 2169867..1b64ba3 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -258,11 +258,13 @@ public TargetSolution GetHubAimSolution() { Distance turretX = TurretConstants.kTurretCenterDistanceFromRobotCenter .times(Math.cos(robotPose.getRotation().getMeasure() - .plus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Radians))); + .plus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Radians))) + .plus(robotPose.getTranslation().getMeasureX()); Distance turretY = TurretConstants.kTurretCenterDistanceFromRobotCenter .times(Math.sin(robotPose.getRotation().getMeasure() - .plus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Radians))); + .plus(TurretConstants.kAngularDistanceToFrontOfRobot).in(Radians))) + .plus(robotPose.getTranslation().getMeasureY()); Translation2d turretTranslation = new Translation2d(turretX, turretY); From 8ac67a1824a4b606504fd706e910f4b4f620ccb6 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 6 Mar 2026 11:23:57 -0600 Subject: [PATCH 21/29] Qual 1 and 2 edits. Rio problem first match, no problem second match --- src/main/java/frc/robot/Constants.java | 4 +-- src/main/java/frc/robot/RobotContainer.java | 36 +++++++++++++------ .../frc/robot/commands/CommandFactory.java | 2 ++ .../frc/robot/subsystems/DriveSubsystem.java | 8 +++++ .../frc/robot/subsystems/TurretSubsystem.java | 9 +++++ src/main/java/frc/robot/utils/Vision.java | 11 ++++++ 6 files changed, 58 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d4249f5..6d7ce48 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -435,10 +435,10 @@ public static final class IntakeConstants { public static final int kIntakeMotorId = 7; // 10 teeth on pinion, 20 teeth on rack - public static final Angle kDeployRotations = Rotations.of(9.3); + public static final Angle kDeployRotations = Rotations.of(9.6); public static final Angle kRetractRotations = Rotations.of(0.0); - public static final Angle kMaxExtension = Rotations.of(9.3); + public static final Angle kMaxExtension = Rotations.of(9.6); public static final Angle kMinExtension = Rotations.of(0.0); public static final int kDeployMotorCurrentLimit = 40; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0cad895..62c539e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -226,28 +227,43 @@ public Command feedPosition(Alliance alliance) { } public void teleopPeriodic() { + DogLog.log("In Teleop Periodic Robotcontainer", true); m_turret.addDriveHeading(UtilityFunctions.WrapAngle(m_drive.getHeading())); - TargetSolution solution = m_commandFactory.GetHubAimSolution(); + // double solutionStart = Timer.getFPGATimestamp(); + // TargetSolution solution = m_commandFactory.GetHubAimSolution(); + // double solutionEnd = Timer.getFPGATimestamp(); - Pose2d robotPose = m_drive.getPose(); + // Pose2d robotPose = m_drive.getPose(); - Distance xDist = Meters.of(solution.distance().in(Meters) - * Math.cos(solution.hubAngle().minus(solution.phi()).in(Radians))) - .plus(robotPose.getMeasureX()); - Distance yDist = Meters.of(solution.distance().in(Meters) - * Math.sin(solution.hubAngle().minus(solution.phi()).in(Radians))) - .plus(robotPose.getMeasureY()); + // Distance xDist = Meters.of(solution.distance().in(Meters) + // * Math.cos(solution.hubAngle().minus(solution.phi()).in(Radians))) + // .plus(robotPose.getMeasureX()); + // Distance yDist = Meters.of(solution.distance().in(Meters) + // * Math.sin(solution.hubAngle().minus(solution.phi()).in(Radians))) + // .plus(robotPose.getMeasureY()); - Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); + // Pose2d targetPose = new Pose2d(xDist, yDist, new Rotation2d()); - m_field.getObject("targetPose").setPose(targetPose); + // m_field.getObject("targetPose").setPose(targetPose); + double start = Timer.getFPGATimestamp(); m_commandFactory.periodic(); + double end = Timer.getFPGATimestamp(); if (m_zeroGyroGetter.get()) { m_drive.ZeroGyro(); } + + DogLog.log("Drivetrain command", + m_drive.getCurrentCommand() == null ? "null" : m_drive.getCurrentCommand().toString()); + DogLog.log("Turret Command", + m_turret.getCurrentCommand() == null ? "null" + : m_turret.getCurrentCommand().toString()); + DogLog.log("Shooter command", m_shooter.getCurrentCommand() == null ? "null" + : m_shooter.getCurrentCommand().toString()); + DogLog.log("Time for command factory periodic in ms", (end - start) * 1000.0); + DogLog.log("In Teleop Periodic Robotcontainer", false); } public void periodic() { diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 1b64ba3..1843390 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -90,6 +90,7 @@ public Command StopAimCommand() { } public void periodic() { + DogLog.log("In periodic command factor", true); m_solution = GetHubAimSolution(); if (m_isAiming) { m_wheelVelocity = m_solution.wheelSpeed(); @@ -98,6 +99,7 @@ public void periodic() { } DogLog.log("Turret Rotation in deg", m_turret.getRotation().in(Degrees)); DogLog.log("RPM target", m_wheelVelocity.in(RPM)); + DogLog.log("In periodic command factor", false); } public void AimTurretToFront() { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index a40f34d..14dcbf3 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -231,7 +231,9 @@ public void disableFaceHeading() { @Override public void periodic() { + DogLog.log("In periodic drive subsystem", true); // Update the odometry in the periodic block + double start = Timer.getFPGATimestamp(); if (Robot.isSimulation()) { ChassisSpeeds chassisSpeed = DriveConstants.kDriveKinematics.toChassisSpeeds( @@ -275,6 +277,12 @@ public void periodic() { SmartDashboard.putData(m_field); + double end = Timer.getFPGATimestamp(); + + DogLog.log("Drivetrain periodic time (ms)", (end - start) * 1000.0); + + DogLog.log("In periodic drive subsystem", false); + // SmartDashboard.putBoolean("Is manual rotate", m_isManualRotate); // DogLog.log("X dist to april tag in meters", diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 9f153fb..cd1f4cf 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -25,6 +25,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; @@ -147,11 +148,19 @@ public TurretPosition getRotationAtTime(double timestamp) { @Override public void periodic() { + DogLog.log("In periodic turret subsystem", true); + double start = Timer.getFPGATimestamp(); + m_positionBuffer.pushElement( UtilityFunctions.WrapAngle(getRotation()), RPM.of(m_absoluteEncoder.getVelocity()), TurretConstants.kEncoderReadingDelay.in(Seconds)); + double end = Timer.getFPGATimestamp(); + + DogLog.log("Turret periodic time (ms)", (end - start) * 1000.0); + DogLog.log("In periodic turret subsystem", false); + // DogLog.log("Turret rotation relative to front of robot", // getRotation().in(Degrees)); // DogLog.log("Turret rotation relative to turret zero", UtilityFunctions diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index b3aaaac..ef89f96 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -11,6 +11,7 @@ import org.photonvision.PhotonPoseEstimator; import org.photonvision.targeting.PhotonTrackedTarget; +import dev.doglog.DogLog; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation3d; @@ -20,6 +21,8 @@ import edu.wpi.first.math.numbers.N3; import frc.robot.Constants.VisionConstants; import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.Timer; + import static edu.wpi.first.units.Units.*; public class Vision { @@ -69,6 +72,9 @@ public void periodic() { // }); // } + DogLog.log("In periodic vision subsystem", true); + double start = Timer.getFPGATimestamp(); + Optional visionEstimationCameraTwo = Optional.empty(); for (var result : m_camera2.getAllUnreadResults()) { Transform3d cameraTransform; @@ -98,6 +104,11 @@ public void periodic() { estimation.timestampSeconds, getCurrentStdDevs())); }); } + + double end = Timer.getFPGATimestamp(); + + DogLog.log("Vision periodic loop time (ms)", (end - start) * 1000.0); + DogLog.log("In periodic vision subsystem", false); } private void updateEstimationStdDevs( From ca95f84e04d6fa642b240e7191b404907e09599f Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 6 Mar 2026 15:57:10 -0600 Subject: [PATCH 22/29] Crude fix to thread blocking bug --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 5 ++++- .../frc/robot/commands/CommandFactory.java | 21 +++++++++++++------ .../java/frc/robot/utils/PositionBuffer.java | 12 +++++++++++ 4 files changed, 33 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6d7ce48..c19f243 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -223,7 +223,7 @@ public static final class TurretConstants { public static final Angle kMinAngle = Rotations.of(0.1); public static final Angle kMaxAngle = Rotations.of(0.854); - public static final int kPositionBufferLength = 1000; + public static final int kPositionBufferLength = 300; public static final Time kEncoderReadingDelay = Seconds.of(0.005); public static final Time kEncoderReadInterval = Seconds.of(0.05); @@ -441,7 +441,7 @@ public static final class IntakeConstants { public static final Angle kMaxExtension = Rotations.of(9.6); public static final Angle kMinExtension = Rotations.of(0.0); - public static final int kDeployMotorCurrentLimit = 40; + public static final int kDeployMotorCurrentLimit = 60; public static final int kIntakeMotorCurrentLimit = 80; public static final AngularVelocity kDefaultIntakeSpeed = RPM.of(-2200); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 62c539e..98c6f71 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -81,7 +81,10 @@ public RobotContainer() { NamedCommands.registerCommand("Stop Aim", m_commandFactory.StopAimCommand()); NamedCommands.registerCommand("Shoot", m_commandFactory.ShootCommand().alongWith(m_commandFactory.RunAllStager()) - .finallyDo(m_commandFactory::StopStagingCommand)); + .finallyDo(() -> { + m_commandFactory.StopStaging(); + m_commandFactory.StopShoot(); + })); NamedCommands.registerCommand("Stop Shoot", m_commandFactory.StopShootCommand()); SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 1843390..7254572 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -76,7 +76,6 @@ public Command AimCommand(boolean isFeedingLeftSide) { public void StopAim() { m_isAiming = false; m_shooter.MoveHoodToPosition(ShooterConstants.kDefaultHoodPosition); - m_wheelVelocity = ShooterConstants.kNonAimShooterVelocity; } public void SetSubsystems(DriveSubsystem drive, TurretSubsystem turret, ShooterSubsystem shooter) { @@ -92,11 +91,9 @@ public Command StopAimCommand() { public void periodic() { DogLog.log("In periodic command factor", true); m_solution = GetHubAimSolution(); - if (m_isAiming) { - m_wheelVelocity = m_solution.wheelSpeed(); - } else { - m_wheelVelocity = ShooterConstants.kDefaultShooterVelocity; - } + + m_wheelVelocity = m_solution.wheelSpeed(); + DogLog.log("Turret Rotation in deg", m_turret.getRotation().in(Degrees)); DogLog.log("RPM target", m_wheelVelocity.in(RPM)); DogLog.log("In periodic command factor", false); @@ -534,10 +531,13 @@ private static Translation2d getClosestAngleApriltag(Angle referenceAngle, Trans } private static int getFirstEntryIndex(Distance distance) { + DogLog.log("In shooting entry search function", true); int low = 0; int high = ShooterConstants.kShootingEntries.length; int mid = 0; + int i = 0; + while (low < high) { mid = (low + high) / 2; ShootingEntry midEntry = ShooterConstants.kShootingEntries[mid]; @@ -547,6 +547,13 @@ private static int getFirstEntryIndex(Distance distance) { } else { high = mid; } + + i++; + + if (i > 20) { + System.err.println("Shooting entry loop has exceeded 20 iterations."); + return 1; + } } ShootingEntry closestEntry = ShooterConstants.kShootingEntries[mid]; @@ -567,6 +574,8 @@ private static int getFirstEntryIndex(Distance distance) { } } + DogLog.log("In shooting entry search function", false); + return previousEntryIndex; } diff --git a/src/main/java/frc/robot/utils/PositionBuffer.java b/src/main/java/frc/robot/utils/PositionBuffer.java index 15e291a..c4b1040 100644 --- a/src/main/java/frc/robot/utils/PositionBuffer.java +++ b/src/main/java/frc/robot/utils/PositionBuffer.java @@ -1,5 +1,6 @@ package frc.robot.utils; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; import edu.wpi.first.units.measure.Angle; @@ -35,6 +36,8 @@ public TurretPosition getAngleAtTime(double requestedTime) { TurretPosition midpointTurretPosition = null; double timeAtMidpoint = 0.0; + int i = 0; + while (low < high) { try { midpointTurretPosition = m_positions.get(midpoint); @@ -51,6 +54,15 @@ public TurretPosition getAngleAtTime(double requestedTime) { } midpoint = low + (high - low) / 2; + + i++; + + // System.out.println("Turret position at loop " + i); + + if (i > 20) { + System.err.println("Turret position loop has exceeded 20 iterations."); + return new TurretPosition(Degrees.of(40), RPM.of(3500), requestedTime); + } } // Linearly UtilityFunctions.interpolate velocity if we aren't on the first/last From 095d702d1504558fcc85367ed1691e65ed8d03fb Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 6 Mar 2026 17:12:16 -0600 Subject: [PATCH 23/29] Fix drivetrain gear ratios --- src/main/java/frc/robot/Constants.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c19f243..5a8e293 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -118,15 +118,17 @@ public static final class ModuleConstants { // more teeth will result in a robot that drives faster). public static final int kDrivingMotorPinionTeeth = 14; + public static final int kSpurGearTeeth = 21; // Calculations required for driving motor conversion factors and feed forward public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60; public static final double kWheelDiameterMeters = 0.0762; public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; - // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 + // 45 teeth on the wheel's bevel gear, 21 teeth on the first-stage spur gear, 14 // teeth on the bevel pinion - public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDrivingMotorReduction = (45.0 * kSpurGearTeeth) + / (kDrivingMotorPinionTeeth * 15); public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; From 548f1c844ee7d885939374b5b6e03233588f4cd5 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 6 Mar 2026 18:09:53 -0600 Subject: [PATCH 24/29] Maybe auto fixes (revert back if doesn't work on practice field) --- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 51 ++++++++++++------- .../frc/robot/subsystems/DriveSubsystem.java | 8 +-- 3 files changed, 41 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c2f1df6..5c88499 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -61,6 +61,8 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + + m_robotContainer.teleopInit(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 98c6f71..fddfe7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,19 +103,19 @@ public RobotContainer() { configureBindings(); // Configure default commands - m_drive.setDefaultCommand( - // The left stick controls translation of the robot. - // Turning is controlled by the X axis of the right stick. - new RunCommand( - () -> m_drive.drive( - -MathUtil.applyDeadband(m_driverController.getLeftY(), - OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(m_driverController.getLeftX(), - OIConstants.kDriveDeadband), - -MathUtil.applyDeadband(m_driverController.getRightX(), - OIConstants.kDriveDeadband), - true), - m_drive)); + // m_drive.setDefaultCommand( + // // The left stick controls translation of the robot. + // // Turning is controlled by the X axis of the right stick. + // new RunCommand( + // () -> m_drive.drive( + // -MathUtil.applyDeadband(m_driverController.getLeftY(), + // OIConstants.kDriveDeadband), + // -MathUtil.applyDeadband(m_driverController.getLeftX(), + // OIConstants.kDriveDeadband), + // -MathUtil.applyDeadband(m_driverController.getRightX(), + // OIConstants.kDriveDeadband), + // true), + // m_drive)); m_field = m_drive.getField(); SmartDashboard.putData(m_sendable); @@ -193,10 +193,10 @@ private void configureBindings() { return m_intakeOut; })); - m_driverController.povUp() - .whileTrue(m_commandFactory.ClimbDownCommand().finallyDo(m_commandFactory::StopClimb)); - m_driverController.povDown() - .whileTrue(m_commandFactory.ClimbUpCommand().finallyDo(m_commandFactory::StopClimb)); + // m_driverController.povUp() + // .whileTrue(m_commandFactory.ClimbDownCommand().finallyDo(m_commandFactory::StopClimb)); + // m_driverController.povDown() + // .whileTrue(m_commandFactory.ClimbUpCommand().finallyDo(m_commandFactory::StopClimb)); // m_driverController.x().onTrue(new InstantCommand(() -> { // // double hoodAngle = m_hoodAngleGetter.get(); @@ -291,4 +291,21 @@ private AngularVelocity getSmartdashboardRequestedWheelSpeed() { private Angle getSmartdashBoardRequestedTurretAngle() { return Degrees.of(SmartDashboard.getNumber("Turret angle in degrees", 0.0)); } + + public void teleopInit() { + // Configure default commands + m_drive.setDefaultCommand( + // The left stick controls translation of the robot. + // Turning is controlled by the X axis of the right stick. + new RunCommand( + () -> m_drive.drive( + -MathUtil.applyDeadband(m_driverController.getLeftY(), + OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(m_driverController.getLeftX(), + OIConstants.kDriveDeadband), + -MathUtil.applyDeadband(m_driverController.getRightX(), + OIConstants.kDriveDeadband), + true), + m_drive)); + } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 14dcbf3..65f8746 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -138,7 +138,8 @@ public DriveSubsystem(Function turretPositionSupplier) { (Pose2d pose) -> resetOdometry(pose), // Method to reset odometry (will be called if your auto has a // starting pose) () -> getRobotRelativeSpeeds(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> drive(speeds), // Method that will drive the robot given ROBOT RELATIVE + (speeds, feedforwards) -> drive(speeds, "Path planner"), // Method that will drive the robot given ROBOT + // RELATIVE // ChassisSpeeds. Also optionally outputs individual module // feedforwards new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for @@ -388,7 +389,7 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered), timeElapsed); - drive(speeds); + drive(speeds, "Joystick runner"); // m_frontLeft.setDesiredState(swerveModuleStates[0]); // m_frontRight.setDesiredState(swerveModuleStates[1]); @@ -396,10 +397,11 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ // m_rearRight.setDesiredState(swerveModuleStates[3]); } - public void drive(ChassisSpeeds speeds) { + public void drive(ChassisSpeeds speeds, String caller) { var states = DriveConstants.kDriveKinematics.toSwerveModuleStates(speeds); DogLog.log("First commanded motor speeds", states[0]); + DogLog.log("Caller", caller); m_frontLeft.setDesiredState(states[0]); m_frontRight.setDesiredState(states[1]); From 5352f2681e1f23ff2c8126e34c59c15c2444950e Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 6 Mar 2026 21:59:01 -0600 Subject: [PATCH 25/29] Add second camera back to vision and create some new autos --- .../autos/Left Side Neutral Auto.auto | 75 ++++++++++------ .../autos/Right Side Neutral Auto.auto | 88 +++++++++++++++++++ .../paths/Left Side Center Return.path | 24 ++--- .../paths/Left Side To Center Path.path | 31 ++++--- .../paths/Right Side Center Return.path | 59 +++++++++++++ .../paths/Right Side To Center.path | 63 +++++++++++++ src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 8 +- .../frc/robot/commands/CommandFactory.java | 50 ++++++++--- .../frc/robot/subsystems/DriveSubsystem.java | 11 +-- src/main/java/frc/robot/utils/Vision.java | 80 +++++++++++++---- 11 files changed, 398 insertions(+), 92 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Right Side Center Return.path create mode 100644 src/main/deploy/pathplanner/paths/Right Side To Center.path diff --git a/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto index efc3dea..38d5d37 100644 --- a/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto +++ b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto @@ -7,31 +7,70 @@ { "type": "named", "data": { - "name": "Deploy Intake" + "name": "Aim" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Left Side To Center Path" + "name": "Shoot" } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 0.8 + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Side To Center Path" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.3 + } + }, + { + "type": "named", + "data": { + "name": "Deploy Intake" + } + } + ] + } + } + ] } }, { - "type": "named", + "type": "wait", "data": { - "name": "Retract Intake" + "waitTime": 1.0 } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "Left Side Center Return" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Side Center Return" + } + }, + { + "type": "named", + "data": { + "name": "Retract Intake" + } + } + ] } }, { @@ -45,24 +84,6 @@ "data": { "name": "Shoot" } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "Stop Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop Aim" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto new file mode 100644 index 0000000..bc3b355 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Side To Center" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.3 + } + }, + { + "type": "named", + "data": { + "name": "Deploy Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Side Center Return" + } + }, + { + "type": "named", + "data": { + "name": "Retract Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Side Center Return.path b/src/main/deploy/pathplanner/paths/Left Side Center Return.path index 840634f..71384ad 100644 --- a/src/main/deploy/pathplanner/paths/Left Side Center Return.path +++ b/src/main/deploy/pathplanner/paths/Left Side Center Return.path @@ -3,35 +3,35 @@ "waypoints": [ { "anchor": { - "x": 7.636611111111112, - "y": 5.917574893010523 + "x": 7.4548644793152645, + "y": 6.292796005706134 }, "prevControl": null, "nextControl": { - "x": 7.578144444444446, - "y": 7.87431111111111 + "x": 7.3963978126485985, + "y": 8.249532223806721 }, "isLocked": false, "linkedName": "Left Center Intake Location" }, { "anchor": { - "x": 2.0853209700392283, - "y": 5.917574893010523 + "x": 3.909671897289586, + "y": 7.405520684736091 }, "prevControl": { - "x": 1.8006704707524948, - "y": 8.026576319546859 + "x": 7.15727532097004, + "y": 7.172624821683309 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Left Auto Start Position" } ], "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 0.15527273515932175 + "rotationDegrees": 88.18635612332106 } ], "constraintZones": [], @@ -47,13 +47,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -32.347443499441994 + "rotation": 91.59114027035312 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -35.28674897496953 + "rotation": -62.80891001618222 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Side To Center Path.path b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path index 3c2f2fc..07ca384 100644 --- a/src/main/deploy/pathplanner/paths/Left Side To Center Path.path +++ b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path @@ -3,32 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.793223965763195, + "x": 3.909671897289586, "y": 7.405520684736091 }, "prevControl": null, "nextControl": { - "x": 6.1351212553495005, - "y": 7.7289871611982885 + "x": 5.630513552068472, + "y": 7.444336661911555 }, "isLocked": false, - "linkedName": null + "linkedName": "Left Auto Start Position" }, { "anchor": { - "x": 7.636611111111112, - "y": 5.917574893010523 + "x": 7.4548644793152645, + "y": 6.292796005706134 }, "prevControl": { - "x": 6.148665319385007, - "y": 6.629201141227356 + "x": 6.885563480741797, + "y": 7.6125392296718974 }, "nextControl": null, "isLocked": false, "linkedName": "Left Center Intake Location" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5219917012448098, + "rotationDegrees": 87.03163957112189 + }, + { + "waypointRelativePos": 0.8144989339019161, + "rotationDegrees": -62.3466986889636 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -42,13 +51,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -35.28674897496953 + "rotation": -62.80891001618222 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 91.59114027035312 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Side Center Return.path b/src/main/deploy/pathplanner/paths/Right Side Center Return.path new file mode 100644 index 0000000..e0530bd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Side Center Return.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.334533333333334, + "y": 2.125088888888889 + }, + "prevControl": null, + "nextControl": { + "x": 6.213922222222224, + "y": 0.9362666666666666 + }, + "isLocked": false, + "linkedName": "Right Center Intake Location" + }, + { + "anchor": { + "x": 3.8752555555555555, + "y": 0.6536777777777774 + }, + "prevControl": { + "x": 6.165200000000001, + "y": 0.5659777777777777 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Auto Start Position" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -88.52805548427246 + } + ], + "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": -92.48955292199913 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 61.78264414527067 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Side To Center.path b/src/main/deploy/pathplanner/paths/Right Side To Center.path new file mode 100644 index 0000000..57e71d2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Side To Center.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8752555555555555, + "y": 0.6536777777777774 + }, + "prevControl": null, + "nextControl": { + "x": 7.207855555555556, + "y": 0.6536777777777774 + }, + "isLocked": false, + "linkedName": "Right Auto Start Position" + }, + { + "anchor": { + "x": 7.334533333333334, + "y": 2.125088888888889 + }, + "prevControl": { + "x": 6.6816555555555555, + "y": 0.9265222222222211 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Center Intake Location" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2746887966804946, + "rotationDegrees": -92.14563506328979 + }, + { + "waypointRelativePos": 0.7526970954356819, + "rotationDegrees": 59.821891467790174 + } + ], + "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": 61.78264414527067 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -92.48955292199913 + }, + "useDefaultConstraints": true +} \ 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 5a8e293..dbff3ef 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -160,6 +160,7 @@ public static final class AutoConstants { // Auto Names public static final String kExampleAutoName = "Example Auto"; + public static final Time kShootTime = Seconds.of(3.5); } public static final class NeoMotorConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fddfe7e..3588fb4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -77,16 +77,14 @@ public RobotContainer() { m_commandFactory.DeployIntake()); NamedCommands.registerCommand("Retract Intake", m_commandFactory.RetractIntake()); - NamedCommands.registerCommand("Aim", m_commandFactory.AimCommand(false)); - NamedCommands.registerCommand("Stop Aim", m_commandFactory.StopAimCommand()); + NamedCommands.registerCommand("Aim", m_commandFactory.AutoAimAtHubCommand()); NamedCommands.registerCommand("Shoot", m_commandFactory.ShootCommand().alongWith(m_commandFactory.RunAllStager()) .finallyDo(() -> { m_commandFactory.StopStaging(); m_commandFactory.StopShoot(); - })); - NamedCommands.registerCommand("Stop Shoot", - m_commandFactory.StopShootCommand()); + }).raceWith(new WaitCommand(AutoConstants.kShootTime))); + SmartDashboard.putNumber("Wheelspeed in rotations per second", 0.0); SmartDashboard.putNumber("Shooter hood angle in degrees", 0.0); SmartDashboard.putNumber("Turret angle in degrees", 0.0); diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index 7254572..ecbb76d 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -119,6 +119,23 @@ public Command MoveTurretToFront() { }); } + public void AutoAimAtHub() { + TargetSolution solution; + + if (m_solution == null) { + solution = GetHubAimSolution(); + } else { + solution = m_solution; + } + + m_wheelVelocity = solution.wheelSpeed(); + MoveTurretToHeading(solution.hubAngle(), false); + } + + public Command AutoAimAtHubCommand() { + return new InstantCommand(this::AutoAimAtHub); + } + private void Aim(boolean isFeedingLeftSide) { Fixtures.FieldLocations location = m_drive.getRobotLocation(); @@ -133,7 +150,7 @@ private void Aim(boolean isFeedingLeftSide) { solution = m_solution; } - MoveTurretToHeading(solution.hubAngle().plus(solution.phi())); + MoveTurretToHeading(solution.hubAngle().plus(solution.phi()), true); // DogLog.log("Range from hub (meters)", solution.distance().in(Meters)); // System.out.println(solution.phi()); m_shooter.MoveHoodToPosition(solution.hoodAngle()); @@ -150,7 +167,7 @@ private void Aim(boolean isFeedingLeftSide) { m_shooter.MoveHoodToPosition(ShooterConstants.kHoodFeedingPosition); m_wheelVelocity = ShooterConstants.kFeedingWheelVelocity; - MoveTurretToHeading(absHeading); + MoveTurretToHeading(absHeading, true); break; } case OpponentSide: { @@ -283,7 +300,7 @@ public TargetSolution GetHubAimSolution() { public Command MoveTurretToHeadingCommand(Angle heading) { return new RunCommand(() -> { - MoveTurretToHeading(heading); + MoveTurretToHeading(heading, true); }, m_turret); } @@ -308,12 +325,12 @@ public Command PointAtHub(boolean isRed) { Angle angle = Radians.of(Math.atan2(dy, dx)); - MoveTurretToHeading(angle); + MoveTurretToHeading(angle, true); System.out.println(angle); }).finallyDo(m_drive::disableFaceHeading); } - public void MoveTurretToHeading(Angle heading) { + public void MoveTurretToHeading(Angle heading, boolean moveDrivetrain) { // Weird bug with red side position data Angle offset = DriverStation.getAlliance().get() == Alliance.Red && Robot.isReal() ? Constants.NumericalConstants.kHalfRotation @@ -336,17 +353,22 @@ public void MoveTurretToHeading(Angle heading) { // The overshoot is negative if the robot has to move in a negative direction; // same for positive - Angle overshoot = UtilityFunctions.angleDiff(robotRelativeTurretAngle, closest).in(Degrees) < 0.0 - ? TurretConstants.kOvershootAmount - : TurretConstants.kOvershootAmount.times(-1.0); - closest = closest.plus(overshoot); + if (moveDrivetrain) { + Angle overshoot = UtilityFunctions.angleDiff(robotRelativeTurretAngle, closest).in(Degrees) < 0.0 + ? TurretConstants.kOvershootAmount + : TurretConstants.kOvershootAmount.times(-1.0); + + closest = closest.plus(overshoot); - Angle driveTarget = heading.minus(closest); + Angle driveTarget = heading.minus(closest); - // System.out.println(); - m_drive.moveToAngle(driveTarget); - m_turret.moveToAngle(closest); + // System.out.println(); + m_drive.moveToAngle(driveTarget); + m_turret.moveToAngle(closest); + } else { + m_turret.moveToAngle(closest); + } } } @@ -359,7 +381,7 @@ public Command PointTurretToFixture(Pose2d fixture) { Angle angle = Radians.of(Math.atan2(dy, dx)).minus(Radians.of(robotPose.getRotation().getRadians())); - MoveTurretToHeading(angle); + MoveTurretToHeading(angle, true); }, m_turret); } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 65f8746..d005777 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -245,11 +245,11 @@ public void periodic() { m_simPidgey.setSupplyVoltage(RobotController.getBatteryVoltage()); m_simPidgey.setRawYaw( - getHeading().in(Degrees) + Radians.of(chassisSpeed.omegaRadiansPerSecond).in(Degrees) + getGyroHeading().in(Degrees) + Radians.of(chassisSpeed.omegaRadiansPerSecond).in(Degrees) * DriveConstants.kPeriodicInterval.in(Seconds)); m_odometry.update( - new Rotation2d(getHeading()), + new Rotation2d(getGyroHeading()), new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), @@ -403,6 +403,8 @@ public void drive(ChassisSpeeds speeds, String caller) { DogLog.log("First commanded motor speeds", states[0]); DogLog.log("Caller", caller); + SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeed.magnitude()); + m_frontLeft.setDesiredState(states[0]); m_frontRight.setDesiredState(states[1]); m_rearLeft.setDesiredState(states[2]); @@ -456,9 +458,8 @@ public void zeroHeading() { * @return the robot's heading in degrees, from -180 to 180 */ public Angle getHeading() { - return pidgey.getYaw().getValue(); - // Don't use this code - // return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); + // return pidgey.getYaw().getValue(); + return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); } public Angle getGyroHeading() { diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index ef89f96..3eb1daf 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -27,7 +27,7 @@ public class Vision { - // PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); + PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); Optional> m_turretPositionSupplier; @@ -49,28 +49,25 @@ public Vision(Optional> turretPositionSupplier, } public void periodic() { - // Disabled Camera One + // Enabled Camera One - // Optional visionEstimationCameraOne = Optional.empty(); + Optional visionEstimationCameraOne = Optional.empty(); - // for (var result : m_camera1.getAllUnreadResults()) { - // visionEstimationCameraOne = - // m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + for (var result : m_camera1.getAllUnreadResults()) { + visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); - // if (visionEstimationCameraOne.isEmpty()) { - // visionEstimationCameraOne = - // m_poseEstimatorOne.estimateLowestAmbiguityPose(result); - // } + if (visionEstimationCameraOne.isEmpty()) { + visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + } - // updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), - // m_poseEstimatorOne); + updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), + m_poseEstimatorOne); - // visionEstimationCameraOne.ifPresent(estimation -> { - // m_visionConsumer.accept(new - // VisionEstimation(estimation.estimatedPose.toPose2d(), - // estimation.timestampSeconds, getCurrentStdDevs())); - // }); - // } + visionEstimationCameraOne.ifPresent(estimation -> { + m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), + estimation.timestampSeconds, getCurrentStdDevs())); + }); + } DogLog.log("In periodic vision subsystem", true); double start = Timer.getFPGATimestamp(); @@ -157,6 +154,53 @@ private void updateEstimationStdDevs( } } + private void updateEstimationStdDevsLessStable( + Optional estimatedPose, List targets, + PhotonPoseEstimator poseEstimator) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an + // average-distance metric + for (var tgt : targets) { + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) + continue; + numTags++; + avgDist += tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) + estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else + // Less stable, so simply doubles the standard deviation to trust camera less + estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)).times(2.0); + curStdDevs = estStdDevs; + } + } + } + private Transform3d getTurretCameraTransform(double estimationTime) { if (m_turretPositionSupplier.isEmpty()) return VisionConstants.kRobotToCamTwo; From 3dd7ca4a609da7768d57c1af508a85f9f9d874d7 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 7 Mar 2026 06:52:43 -0600 Subject: [PATCH 26/29] Add right side auto and fix some constants --- .../paths/Right Side Center Return.path | 10 ++++---- .../paths/Right Side To Center.path | 10 ++++---- .../paths/Right Straight Forward.path | 4 ++-- src/main/deploy/pathplanner/settings.json | 24 +++++++++---------- src/main/java/frc/robot/Constants.java | 10 ++++---- src/main/java/frc/robot/RobotContainer.java | 1 + 6 files changed, 30 insertions(+), 29 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Right Side Center Return.path b/src/main/deploy/pathplanner/paths/Right Side Center Return.path index e0530bd..9ad9a8b 100644 --- a/src/main/deploy/pathplanner/paths/Right Side Center Return.path +++ b/src/main/deploy/pathplanner/paths/Right Side Center Return.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.8752555555555555, - "y": 0.6536777777777774 + "x": 3.8579172610556345, + "y": 0.6386019971469341 }, "prevControl": { - "x": 6.165200000000001, - "y": 0.5659777777777777 + "x": 6.14786170550008, + "y": 0.5509019971469344 }, "nextControl": null, "isLocked": false, @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -92.48955292199913 + "rotation": -112.83365417791752 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Right Side To Center.path b/src/main/deploy/pathplanner/paths/Right Side To Center.path index 57e71d2..2479076 100644 --- a/src/main/deploy/pathplanner/paths/Right Side To Center.path +++ b/src/main/deploy/pathplanner/paths/Right Side To Center.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.8752555555555555, - "y": 0.6536777777777774 + "x": 3.8579172610556345, + "y": 0.6386019971469341 }, "prevControl": null, "nextControl": { - "x": 7.207855555555556, - "y": 0.6536777777777774 + "x": 7.190517261055635, + "y": 0.6386019971469341 }, "isLocked": false, "linkedName": "Right Auto Start Position" @@ -57,7 +57,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -92.48955292199913 + "rotation": -112.83365417791752 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Straight Forward.path b/src/main/deploy/pathplanner/paths/Right Straight Forward.path index 159fa85..aa1b78e 100644 --- a/src/main/deploy/pathplanner/paths/Right Straight Forward.path +++ b/src/main/deploy/pathplanner/paths/Right Straight Forward.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.7290888888888887, + "x": 3.663837375178317, "y": 0.6341888888888878 }, "prevControl": null, "nextControl": { - "x": 4.729088888888888, + "x": 4.663837375178316, "y": 0.6341888888888878 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 72a5693..bfb85f8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -12,20 +12,20 @@ "robotMass": 50.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, - "maxDriveSpeed": 5.45, - "driveMotorType": "vortex", + "driveWheelRadius": 0.076, + "driveGearing": 4.5, + "maxDriveSpeed": 4.0, + "driveMotorType": "NEO", "driveCurrentLimit": 80.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.294, + "flModuleY": 0.294, + "frModuleX": 0.294, + "frModuleY": -0.294, + "blModuleX": -0.294, + "blModuleY": 0.294, + "brModuleX": -0.294, + "brModuleY": -0.294, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dbff3ef..6f7fb88 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,7 +56,7 @@ public static final class DriveConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds - public static final LinearVelocity kMaxSpeed = MetersPerSecond.of(2.4); + public static final LinearVelocity kMaxSpeed = MetersPerSecond.of(4.0); public static final LinearAcceleration kMaxAcceleration = MetersPerSecondPerSecond.of(10.0); public static final AngularVelocity kMaxAngularSpeed = RadiansPerSecond.of(2 * Math.PI); @@ -64,9 +64,9 @@ public static final class DriveConstants { .of(4 * Math.PI); // Chassis configuration - public static final double kTrackWidth = Units.inchesToMeters(26.5); + public static final double kTrackWidth = Units.inchesToMeters(23.149606); // Distance between centers of right and left wheels on robot - public static final double kWheelBase = Units.inchesToMeters(26.5); + public static final double kWheelBase = Units.inchesToMeters(23.149606); // Distance between front and back wheels on robot public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( new Translation2d(kWheelBase / 2, kTrackWidth / 2), @@ -144,8 +144,8 @@ public static final class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 3; public static final double kMaxAccelerationMetersPerSecondSquared = 3; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecond = 2 * Math.PI; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = 2 * Math.PI; public static final double kPXController = 1; public static final double kPYController = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3588fb4..7acaedf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -94,6 +94,7 @@ public RobotContainer() { m_sendable.addOption("Right Backwards Auto", "Right Backwards Auto"); m_sendable.addOption("Left Backwards Auto", "Left Backwards Auto"); m_sendable.addOption("Left Side Neutral Auto", "Left Side Neutral Auto"); + m_sendable.addOption("Right Side Neutral Auto", "Right Side Neutral Auto"); m_sendable.setDefaultOption("Right Forward Auto", "Right Forward Auto"); m_sendable.addOption("Simple Shoot Auto", "Simple Shoot Auto"); From 1670b52be9ec154449dc90e4f6257a7a6943fe65 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 7 Mar 2026 08:29:51 -0600 Subject: [PATCH 27/29] WORKING AUTOS --- .../autos/Left Side Neutral Auto.auto | 2 +- .../autos/Right Side Neutral Auto.auto | 2 +- .../pathplanner/autos/Simple Shoot Auto.auto | 6 ++++ .../paths/Left Side Center Return.path | 12 +++---- .../paths/Left Side To Center Path.path | 12 +++---- .../paths/Right Side Center Return.path | 6 ++-- .../paths/Right Side To Center.path | 6 ++-- src/main/java/frc/robot/Constants.java | 2 +- .../frc/robot/subsystems/DriveSubsystem.java | 4 +-- src/main/java/frc/robot/utils/Vision.java | 31 ++++++++++--------- 10 files changed, 46 insertions(+), 37 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto index 38d5d37..50c2c88 100644 --- a/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto +++ b/src/main/deploy/pathplanner/autos/Left Side Neutral Auto.auto @@ -33,7 +33,7 @@ { "type": "wait", "data": { - "waitTime": 1.3 + "waitTime": 2.4 } }, { diff --git a/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto b/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto index bc3b355..95166c8 100644 --- a/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto +++ b/src/main/deploy/pathplanner/autos/Right Side Neutral Auto.auto @@ -33,7 +33,7 @@ { "type": "wait", "data": { - "waitTime": 1.3 + "waitTime": 2.4 } }, { diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto index 72b77e3..18f2ba1 100644 --- a/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Auto.auto @@ -4,6 +4,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Left Side Center Return.path b/src/main/deploy/pathplanner/paths/Left Side Center Return.path index 71384ad..59960ad 100644 --- a/src/main/deploy/pathplanner/paths/Left Side Center Return.path +++ b/src/main/deploy/pathplanner/paths/Left Side Center Return.path @@ -4,24 +4,24 @@ { "anchor": { "x": 7.4548644793152645, - "y": 6.292796005706134 + "y": 5.438844507845934 }, "prevControl": null, "nextControl": { "x": 7.3963978126485985, - "y": 8.249532223806721 + "y": 7.3955807259465205 }, "isLocked": false, "linkedName": "Left Center Intake Location" }, { "anchor": { - "x": 3.909671897289586, - "y": 7.405520684736091 + "x": 3.922610556348074, + "y": 7.340827389443652 }, "prevControl": { - "x": 7.15727532097004, - "y": 7.172624821683309 + "x": 7.170213980028528, + "y": 7.10793152639087 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Side To Center Path.path b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path index 07ca384..f6acac8 100644 --- a/src/main/deploy/pathplanner/paths/Left Side To Center Path.path +++ b/src/main/deploy/pathplanner/paths/Left Side To Center Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.909671897289586, - "y": 7.405520684736091 + "x": 3.922610556348074, + "y": 7.340827389443652 }, "prevControl": null, "nextControl": { - "x": 5.630513552068472, - "y": 7.444336661911555 + "x": 5.64345221112696, + "y": 7.379643366619116 }, "isLocked": false, "linkedName": "Left Auto Start Position" @@ -17,11 +17,11 @@ { "anchor": { "x": 7.4548644793152645, - "y": 6.292796005706134 + "y": 5.438844507845934 }, "prevControl": { "x": 6.885563480741797, - "y": 7.6125392296718974 + "y": 6.758587731811697 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Side Center Return.path b/src/main/deploy/pathplanner/paths/Right Side Center Return.path index 9ad9a8b..178cf03 100644 --- a/src/main/deploy/pathplanner/paths/Right Side Center Return.path +++ b/src/main/deploy/pathplanner/paths/Right Side Center Return.path @@ -17,11 +17,11 @@ { "anchor": { "x": 3.8579172610556345, - "y": 0.6386019971469341 + "y": 0.8197432239657632 }, "prevControl": { "x": 6.14786170550008, - "y": 0.5509019971469344 + "y": 0.7320432239657635 }, "nextControl": null, "isLocked": false, @@ -47,7 +47,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -112.83365417791752 + "rotation": -112.83365417791754 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Right Side To Center.path b/src/main/deploy/pathplanner/paths/Right Side To Center.path index 2479076..d3e35ed 100644 --- a/src/main/deploy/pathplanner/paths/Right Side To Center.path +++ b/src/main/deploy/pathplanner/paths/Right Side To Center.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.8579172610556345, - "y": 0.6386019971469341 + "y": 0.8197432239657632 }, "prevControl": null, "nextControl": { "x": 7.190517261055635, - "y": 0.6386019971469341 + "y": 0.8197432239657632 }, "isLocked": false, "linkedName": "Right Auto Start Position" @@ -57,7 +57,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -112.83365417791752 + "rotation": -112.83365417791754 }, "useDefaultConstraints": true } \ 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 6f7fb88..44cda3e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -310,7 +310,7 @@ public static final class ShooterConstants { public static final LinearVelocity kMaxMuzzleVelocity = MetersPerSecond.of(10.0); - public static final Distance kHubRobotTurretOffset = Inches.of(44); + public static final Distance kHubRobotTurretOffset = Inches.of(47); public static final ShootingEntry[] kShootingEntries = { new ShootingEntry(Inches.of(30).plus(kHubRobotTurretOffset), RPM.of(3519), null, diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index d005777..2198446 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -458,8 +458,8 @@ public void zeroHeading() { * @return the robot's heading in degrees, from -180 to 180 */ public Angle getHeading() { - // return pidgey.getYaw().getValue(); - return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); + return pidgey.getYaw().getValue(); + // return m_poseEstimator.getEstimatedPosition().getRotation().getMeasure(); } public Angle getGyroHeading() { diff --git a/src/main/java/frc/robot/utils/Vision.java b/src/main/java/frc/robot/utils/Vision.java index 3eb1daf..809e5ae 100644 --- a/src/main/java/frc/robot/utils/Vision.java +++ b/src/main/java/frc/robot/utils/Vision.java @@ -27,7 +27,7 @@ public class Vision { - PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); + // PhotonCamera m_camera1 = new PhotonCamera(VisionConstants.kCameraName1); PhotonCamera m_camera2 = new PhotonCamera(VisionConstants.kCameraName2); Optional> m_turretPositionSupplier; @@ -51,23 +51,26 @@ public Vision(Optional> turretPositionSupplier, public void periodic() { // Enabled Camera One - Optional visionEstimationCameraOne = Optional.empty(); + // Optional visionEstimationCameraOne = Optional.empty(); - for (var result : m_camera1.getAllUnreadResults()) { - visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + // for (var result : m_camera1.getAllUnreadResults()) { + // visionEstimationCameraOne = + // m_poseEstimatorOne.estimateLowestAmbiguityPose(result); - if (visionEstimationCameraOne.isEmpty()) { - visionEstimationCameraOne = m_poseEstimatorOne.estimateLowestAmbiguityPose(result); - } + // if (visionEstimationCameraOne.isEmpty()) { + // visionEstimationCameraOne = + // m_poseEstimatorOne.estimateLowestAmbiguityPose(result); + // } - updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), - m_poseEstimatorOne); + // updateEstimationStdDevs(visionEstimationCameraOne, result.getTargets(), + // m_poseEstimatorOne); - visionEstimationCameraOne.ifPresent(estimation -> { - m_visionConsumer.accept(new VisionEstimation(estimation.estimatedPose.toPose2d(), - estimation.timestampSeconds, getCurrentStdDevs())); - }); - } + // visionEstimationCameraOne.ifPresent(estimation -> { + // m_visionConsumer.accept(new + // VisionEstimation(estimation.estimatedPose.toPose2d(), + // estimation.timestampSeconds, getCurrentStdDevs())); + // }); + // } DogLog.log("In periodic vision subsystem", true); double start = Timer.getFPGATimestamp(); From 02e25e620595c4febb8eb3fce86edc37bd803f00 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 7 Mar 2026 20:31:05 -0600 Subject: [PATCH 28/29] Final fixes for LSR --- .../pathplanner/paths/Right Side Center Return.path | 8 ++++---- .../deploy/pathplanner/paths/Right Side To Center.path | 8 ++++---- src/main/java/frc/robot/commands/CommandFactory.java | 2 +- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 1 - 4 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Right Side Center Return.path b/src/main/deploy/pathplanner/paths/Right Side Center Return.path index 178cf03..3988ff2 100644 --- a/src/main/deploy/pathplanner/paths/Right Side Center Return.path +++ b/src/main/deploy/pathplanner/paths/Right Side Center Return.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.334533333333334, - "y": 2.125088888888889 + "x": 7.416048502139798, + "y": 2.4888302425106996 }, "prevControl": null, "nextControl": { - "x": 6.213922222222224, - "y": 0.9362666666666666 + "x": 6.295437391028688, + "y": 1.3000080202884772 }, "isLocked": false, "linkedName": "Right Center Intake Location" diff --git a/src/main/deploy/pathplanner/paths/Right Side To Center.path b/src/main/deploy/pathplanner/paths/Right Side To Center.path index d3e35ed..adbbc32 100644 --- a/src/main/deploy/pathplanner/paths/Right Side To Center.path +++ b/src/main/deploy/pathplanner/paths/Right Side To Center.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.334533333333334, - "y": 2.125088888888889 + "x": 7.416048502139798, + "y": 2.4888302425106996 }, "prevControl": { - "x": 6.6816555555555555, - "y": 0.9265222222222211 + "x": 6.76317072436202, + "y": 1.2902635758440317 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/commands/CommandFactory.java b/src/main/java/frc/robot/commands/CommandFactory.java index ecbb76d..1795cda 100644 --- a/src/main/java/frc/robot/commands/CommandFactory.java +++ b/src/main/java/frc/robot/commands/CommandFactory.java @@ -150,7 +150,7 @@ private void Aim(boolean isFeedingLeftSide) { solution = m_solution; } - MoveTurretToHeading(solution.hubAngle().plus(solution.phi()), true); + MoveTurretToHeading(solution.hubAngle().minus (solution.phi()), true); // DogLog.log("Range from hub (meters)", solution.distance().in(Meters)); // System.out.println(solution.phi()); m_shooter.MoveHoodToPosition(solution.hoodAngle()); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index cd1f4cf..54b6daf 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -70,7 +70,6 @@ public class TurretSubsystem extends SubsystemBase { public TurretSubsystem() { m_config.closedLoop.p(TurretConstants.kP).i(TurretConstants.kI).d(TurretConstants.kD); m_config.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); - m_config.smartCurrentLimit(TurretConstants.kSmartCurrentLimit); m_config.idleMode(IdleMode.kBrake); m_config.absoluteEncoder.inverted(true); m_config.inverted(true); From 8e3c8164991b702908351d7f4d7d6a47bf23e2b1 Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Tue, 10 Mar 2026 18:02:54 -0500 Subject: [PATCH 29/29] Add new motors to README --- README.md | 39 ++++++++++++++++++++++++--------------- 1 file changed, 24 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 919f6da..5e62afa 100644 --- a/README.md +++ b/README.md @@ -27,26 +27,35 @@ This is a guide to using and updating 5690's season code for our 2026 Command Ro All devices connected to the CAN bus along with their corresponding CAN IDs -| Device | CAN ID | -| ----------------------- | ------ | -| Front Right Drive Motor | 1 | -| Front Left Drive Motor | 2 | -| Rear Left Drive Motor | 3 | -| Rear Right Drive Motor | 4 | -| Front Right Turn Motor | 5 | -| Front Left Turn Motor | 6 | -| Rear Left Turn Motor | 7 | -| Rear Right Turn Motor | 8 | -| Pigeon Gyro | 13 | +| Device | CAN ID | +| -------------------------- | ------ | +| Front Right Drive Motor | 1 | +| Front Left Drive Motor | 10 | +| Rear Left Drive Motor | 14 | +| Rear Right Drive Motor | 2 | +| Front Right Turn Motor | 62 | +| Front Left Turn Motor | 11 | +| Rear Left Turn Motor | 15 | +| Rear Right Turn Motor | 3 | +| Pigeon Gyro | 13 | +| Turret Turning Motor | 18 | +| Turret Shooter Motor | 5 | +| Shooter Hood Motor | 4 | +| Agitator Motor | 9 | +| Roller Motor | 12 | +| First Intake Deploy Motor | 13 | +| Second Intake Deploy Motor | 8 | +| Intake Motor | 7 | +| Climber Motor | 17 | ## Network Map All devices connected to the robot's local network along with each device's assigned IP address -| Device | IP | -| ------- | ---------- | -| Gateway | 10.56.90.1 | -| RoboRio | 10.56.90.2 | +| Device | IP | +| ------------------ | ----------- | +| Gateway | 10.56.90.1 | +| RoboRio | 10.56.90.2 | | Vision Coprocessor | 10.56.90.10 | ## Button Bindings