From 8e3ee46b4c64c96799e24e5f311faf471d65e98c Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 17 Feb 2026 16:09:58 -0700 Subject: [PATCH] Shooter hub position and distance --- simgui-ds.json | 5 --- .../robot/constants/BuildConstants.java | 12 +++--- .../robot/subsystems/shooter/Shooter.java | 31 +++++++------ .../subsystems/shooter/ShooterConstants.java | 43 +++++-------------- .../utility/compute/FieldPositions.java | 9 ++-- 5 files changed, 38 insertions(+), 62 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 4a63cc1..73cc713 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index f34b88c..369ff3d 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,14 +5,14 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters"; + public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 52; - public static final String GIT_SHA = "736d7ef8233e918aa0cc1cc3f06f2069517bed59"; - public static final String GIT_DATE = "2026-02-14 13:49:33 MST"; + public static final int GIT_REVISION = 53; + public static final String GIT_SHA = "da6f9b50b5149ab8b80ed477418f2c5f795feaa4"; + public static final String GIT_DATE = "2026-02-14 15:03:32 MST"; public static final String GIT_BRANCH = "shoot-button"; - public static final String BUILD_DATE = "2026-02-14 14:55:59 MST"; - public static final long BUILD_UNIX_TIME = 1771106159811L; + public static final String BUILD_DATE = "2026-02-17 16:08:46 MST"; + public static final long BUILD_UNIX_TIME = 1771369726522L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index c1ffdf1..de66da0 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -6,6 +6,8 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; @@ -76,7 +78,6 @@ public class Shooter extends SubsystemBase { } - @Override public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter @@ -84,20 +85,18 @@ public class Shooter extends SubsystemBase { Logger.processInputs("Shooter", state); io.updateInputs(state); + + ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; + double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); + double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); + + Pose2d robotPose2d = m_SwerveDrive.getPose2d(); + + double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm(); + Logger.recordOutput("Hub Dist", distanceToHub); + + if(this.mode != ShooterMode.NotReady) { - - ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; - double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); - double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); - - Pose2d robotPose2d = m_SwerveDrive.getPose2d(); - - double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm(); - - - - // this.mode = ShooterMode.Shooting; - // TODO: get if the robot is within the angle of the hub boolean driverError = false; @@ -153,11 +152,11 @@ public class Shooter extends SubsystemBase { switch (mode) { case Shooting: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get())); break; case Ready: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub)); io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); break; case NotReady: diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 1a20a91..a6f6b0f 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,11 +1,14 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; @@ -14,13 +17,6 @@ public class ShooterConstants { public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5; public static final double INDEXER_GEAR_RATIO = 1.; - - // public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30); - // public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15); - // public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - - // public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10); - // public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", -35); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", -10); @@ -29,7 +25,7 @@ public class ShooterConstants { public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10); - // Tolerances + // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0); public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99); @@ -40,8 +36,14 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 5); - + // + public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) { + double speed = SHOOTER_ACTIVE_VELOCITY.get(); + return RotationsPerSecond.of(speed); + } + + // Motor Configuration public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0) @@ -63,29 +65,6 @@ public class ShooterConstants { public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.1); public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); - // Limits - - // 0 is the forward angle on the robot. - // negative is left, positive is right - // public static final Angle ANGLE_LIMIT_LEFT = Degrees.of(-180); - // public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180); - - // 0 is paralell to the ground, 90 is directly up - // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); - // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); - - // Motor configs - // public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); - public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java index 19457d8..b9c8df0 100644 --- a/src/main/java/frc4388/utility/compute/FieldPositions.java +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -3,13 +3,16 @@ package frc4388.utility.compute; import edu.wpi.first.math.geometry.Translation2d; public class FieldPositions { - public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); - public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); + // public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); + // public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); + public static final Translation2d RED_HUB_POSITION = new Translation2d(11.9014494, 4.0213534); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(4.61155415, 4.0213534); + // We set the default position to one just in case it doesn't update // Setting to null could cause a null pointer, and setting to (0,0) could cause problems // I would rather have the 50/50 chance than a code error - public static Translation2d HUB_POSITION = RED_HUB_POSITION; + public static Translation2d HUB_POSITION = BLUE_HUB_POSITION; public static void update() { if(TimesNegativeOne.isRed) {