Shooter hub position and distance

simgui-ds.json

@@ -1,9 +1,4 @@
 {
-  "System Joysticks": {
-    "window": {
-      "enabled": false
-    }
-  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

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 int GIT_REVISION = 53;
-  public static final String GIT_SHA = "736d7ef8233e918aa0cc1cc3f06f2069517bed59";
+  public static final String GIT_SHA = "da6f9b50b5149ab8b80ed477418f2c5f795feaa4";
-  public static final String GIT_DATE = "2026-02-14 13:49:33 MST";
+  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 String BUILD_DATE = "2026-02-17 16:08:46 MST";
-  public static final long BUILD_UNIX_TIME = 1771106159811L;
+  public static final long BUILD_UNIX_TIME = 1771369726522L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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:

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);

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 RED_HUB_POSITION = new Translation2d(0, 0);
-    public static final Translation2d BLUE_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) {