bug fixes

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 93fb78bc82718bcede281ba1a3314af4891fa8fc..5559db23c29b91d5ab2b570cb10e908653d12775 100644 (file)
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -37,8 +37,8 @@ public class Robot extends LoggedRobot {
 
     public Robot(){
         CanBridge.runTCP();
-        PortForwarder.add(5800,"10.9.72.12",5800);
-        PortForwarder.add(1182,"10.9.72.12",1182);
+        PortForwarder.add(5800, Constants.VISION_CAMERA_HOST, 5800);
+        PortForwarder.add(1182, Constants.VISION_CAMERA_HOST, 1182);
 
         // Set up data receivers & replay source
         switch (Constants.CURRENT_MODE) {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f9bf6fabded592ac658156eb518a5e5ece16bbe7..2318df8f9e8a5d294a251debbdfd9671e7680ceb 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -155,9 +155,6 @@ public class RobotContainer {
         initializeAutoBuilder();
         autoChooserInit();
 
-        // put the Chooser on the SmartDashboard
-        SmartDashboard.putData("Auto chooser", autoChooser);
-
         if (turret != null && drive != null && hood != null && shooter != null) {
           SmartDashboard.putData("Lock Shooting", new LockedShoot(turret, drive, hood, shooter));
         }
@@ -424,7 +421,7 @@ public class RobotContainer {
       }
     }
     if (!Constants.DISABLE_SMART_DASHBOARD) {
-      SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString());
+      SmartDashboard.putString("Alliance", DriverStation.getAlliance().map(a -> a.name()).orElse("Unknown"));
     }
   }
 }

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 02abe4c9714351c1df97d313fb3203931ba52c50..67031465787d6ebd92a7cafbbd91bedbf8bee25d 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -169,7 +169,7 @@ public class AutoShootCommand extends Command {
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
 
         // Add a phase delay extrapolation component for latency delay
-        drivepose.exp(
+        drivepose = drivepose.exp(
             new Twist2d(
                 robotRelVel.vxMetersPerSecond * phaseDelay,
                 robotRelVel.vyMetersPerSecond * phaseDelay,

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 092828991719b2e0832e134e0e5767f823512156..f47ca189e37b1fc88954c9a4f14af709f9800000 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -196,7 +196,7 @@ public class Superstructure extends Command {
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
 
         // Add a phase delay extrapolation component for latency delay
-        drivepose.exp(
+        drivepose = drivepose.exp(
             new Twist2d(
                 robotRelVel.vxMetersPerSecond * phaseDelay,
                 robotRelVel.vyMetersPerSecond * phaseDelay,

diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
index 3df7e6f0456bfbc8f9ec5dfa06da48e46d4b5485..f18925a6b686122daf2e604cdb318b33857ef03d 100644 (file)
--- a/src/main/java/frc/robot/constants/Constants.java
+++ b/src/main/java/frc/robot/constants/Constants.java
@@ -83,4 +83,7 @@ public class Constants {
 
     // Enables 3D logs of mechanisms
     public static final boolean LOG_MECHANISMS = true;
+
+    // Network setting for vision
+    public static final String VISION_CAMERA_HOST = "10.9.72.12";
 }

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 6449efe18026e96e5b3d165a21149030d792dbb7..3b67382318aec1e1a1a443604713dc2cfc135d04 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -25,6 +25,7 @@ import lib.controllers.PS5Controller;
 import lib.controllers.PS5Controller.DPad;
 import lib.controllers.PS5Controller.PS5Axis;
 import lib.controllers.PS5Controller.PS5Button;
+import org.littletonrobotics.junction.Logger;
 
 /**
  * Driver controls for the PS5 controller
@@ -39,6 +40,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private Hood hood;
     private Intake intake;
     private Spindexer spindexer;
+    private double originalSpindexerCurrentLimit;
 
     public PS5ControllerDriverConfig(
             Drivetrain drive,
@@ -156,13 +158,14 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
 
     private void shootFocus(boolean turnOn) {
         if (turnOn) {
-            System.out.println("Shooting is now Focused");
+            Logger.recordOutput("ShootFocus", "Focused");
+            originalSpindexerCurrentLimit = spindexer.getCurrentLimit();
             spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
             drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, 
                     DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25);
         } else {
-            System.out.println("Shooting back to normal (From focus)");
-            spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit);
+            Logger.recordOutput("ShootFocus", "Normal");
+            spindexer.setNewCurrentLimit(originalSpindexerCurrentLimit);
             drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, 
                     DriveConstants.DRIVE_PEAK_CURRENT_LIMIT);
         }

diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java
index 56ef81433f465620de3c582da14882395b26f96d..7dff223b5e73767f6789c7864392dfc027f62917 100644 (file)
--- a/src/main/java/frc/robot/subsystems/Intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java
@@ -269,7 +269,7 @@ public class Intake extends SubsystemBase implements IntakeIO{
      */
     public double rotationsToInches(double motorRotations) {
         // circumference of the rack pinion
-        double circ = 2 * Math.PI * 0.5;
+        double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION;
         double pinionRotations = motorRotations / IntakeConstants.GEAR_RATIO;
         double inches = pinionRotations * circ;
         return inches; 
@@ -281,7 +281,7 @@ public class Intake extends SubsystemBase implements IntakeIO{
      * @return motor rotations
      */
     public double inchesToRotations(double inches){
-        double circ = 2 * Math.PI * 0.5;
+        double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION;
         double pinionRotations = inches / circ;
         double motorRotations = pinionRotations * IntakeConstants.GEAR_RATIO;
         return motorRotations;

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index b34a4e515368d1b8641138d0bcf445c5af906446..46f39805f22a3450908a112691873ad1c50082f3 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -99,11 +99,11 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS).withEnableFOC(true));   
         
         if (!Constants.DISABLE_LOGGING) {
-            Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER);
+            Logger.recordOutput("Shooter/realVelocity", Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER * shooterMotorLeft.getVelocity().getValueAsDouble());
             Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
         }
 
-        double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER;
+        double actualWheelVelocity = Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER * shooterMotorLeft.getVelocity().getValueAsDouble();
         
         if (!Constants.DISABLE_SMART_DASHBOARD) {
             SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity);

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index b4b2ae65b7825911fcb0f0d489205139f8ca0ba1..625efb84846e7284b266688e2d39de74056dce4a 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -21,6 +21,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     private boolean wasSpindexerSlow = false;
     private SpindexerState state = SpindexerState.STOPPED;
     private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged();
+    private double currentLimit = SpindexerConstants.currentLimit;
 
     public boolean noIndexing = false;
 
@@ -127,6 +128,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     }
 
     public void setNewCurrentLimit(double newCurrentLimit) {
+        currentLimit = newCurrentLimit;
         CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
         limitConfig.StatorCurrentLimit = newCurrentLimit;
         limitConfig.StatorCurrentLimitEnable = true;
@@ -135,6 +137,10 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
         motor.getConfigurator().apply(limitConfig);
     }
 
+    public double getCurrentLimit() {
+        return currentLimit;
+    }
+
     @Override
     public void updateInputs() {
         inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio;