Code cleanup and added stuff for cad components in advantagescope

src/main/java/frc/robot/Constants.java

@@ -34,7 +34,7 @@ public static final class ShooterConstants {
 
     // Shooter pivot
     public static final double kPivotGearRatio = 46.722;
-    // TODO: untuned values, fix later
+
     public static final Rotation2d kMaxAngle = Rotation2d.fromDegrees(55);
     public static final Rotation2d kMinAngle = Rotation2d.fromDegrees(15);
     public static final Rotation2d kHomeAngle = Rotation2d.fromDegrees(33);
@@ -104,7 +104,7 @@ public static final class Ports {
     public static final int kIndexerBeamBreakTwo = 3;
 
     public static final int kIntakeRoller = 43;
-    public static final int kIntakePivot = 0;
+    public static final int kIntakePivot = 33;
 
     public static final int kClimbUp = 26;
     public static final int kClimbDown = 25;

src/main/java/frc/robot/RobotContainer.java

@@ -21,6 +21,7 @@
 import frc.robot.commands.DriveCommands;
 import frc.robot.oi.DriverControls;
 import frc.robot.oi.DriverControlsPS5;
+import frc.robot.oi.DriverControlsXbox;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIOSim;
@@ -105,7 +106,11 @@ private void configureSubsystems() {
   }
 
   private void configureControllers() {
-    m_driverControls = new DriverControlsPS5(1);
+    if (RobotBase.isReal()) {
+      m_driverControls = new DriverControlsPS5(1);
+    } else {
+      m_driverControls = new DriverControlsXbox(1);
+    }
   }
 
   private void configureButtonBindings() {
@@ -122,7 +127,7 @@ private void configureButtonBindings() {
             Commands.runOnce(
                 () -> {
                   m_shooter.setFlywheelVelocity(10, 13);
-                  m_shooter.setPivotAngle(Rotation2d.fromDegrees(45));
+                  m_shooter.setPivotAngle(Rotation2d.fromDegrees(50));
                 }))
         .onFalse(
             Commands.runOnce(

src/main/java/frc/robot/RobotState.java

@@ -1,5 +1,8 @@
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
@@ -9,8 +12,10 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.shooter.Shooter;
+import org.littletonrobotics.junction.Logger;
 
 public class RobotState {
+
   private static RobotState instance = null;
 
   // subsystems
@@ -20,21 +25,39 @@ public class RobotState {
 
   // mechanism
   private final boolean kMechanismEnabled = true;
-  MechanismLigament2d m_shooterPivotMech;
+  Mechanism2d m_mechanism;
+
+  MechanismLigament2d m_shooterPivotMechLower;
+  MechanismLigament2d m_shooterPivotMechUpper;
+
+  // advantagescope components
+  private final boolean kComponentsEnabled = true;
+  private final Translation3d kShooterZeroTranslation = new Translation3d(0.017, 0.0, 0.415);
 
   private RobotState(Drive drive, Shooter shooter, Indexer indexer) {
     // subsystems
     m_drive = drive;
     m_shooter = shooter;
 
     // mechanism set up
-    Mechanism2d m_mechanism = new Mechanism2d(3, 3);
-    MechanismRoot2d m_root = m_mechanism.getRoot("shooter", 3, 1);
-    m_shooterPivotMech =
+    m_mechanism = new Mechanism2d(1.5, 1);
+    MechanismRoot2d m_root = m_mechanism.getRoot("shooter", 1.0, 0.5);
+
+    // have as two ligaments so it can rotate around center
+    m_shooterPivotMechLower =
         m_root.append(
             new MechanismLigament2d(
-                "pivot",
-                1,
+                "pivotLower",
+                0.5,
+                m_shooter.getPivotAngle().getDegrees() + 180,
+                6,
+                new Color8Bit(Color.kPurple)));
+
+    m_shooterPivotMechUpper =
+        m_root.append(
+            new MechanismLigament2d(
+                "pivotUpper",
+                0.5,
                 m_shooter.getPivotAngle().getDegrees(),
                 6,
                 new Color8Bit(Color.kPurple)));
@@ -60,12 +83,28 @@ public static RobotState getInstance() {
   }
 
   public void updateMechanism() {
-    m_shooterPivotMech.setAngle(m_shooter.getPivotAngle().getDegrees());
+    m_shooterPivotMechLower.setAngle(m_shooter.getPivotAngle().getDegrees() + 180);
+    m_shooterPivotMechUpper.setAngle(m_shooter.getPivotAngle().getDegrees());
+  }
+
+  public void updateComponents() {
+    Pose3d shooterPose =
+        new Pose3d(
+            kShooterZeroTranslation, new Rotation3d(0, -m_shooter.getPivotAngle().getRadians(), 0));
+
+    Logger.recordOutput("Mechanism", m_mechanism);
+
+    Logger.recordOutput("Components/ShooterPose", shooterPose);
+    Logger.recordOutput("Components/ZeroPose", new Pose3d()); // for tuning config
   }
 
   public void updateRobotState() {
     if (kMechanismEnabled) {
       updateMechanism();
     }
+
+    if (kComponentsEnabled) {
+      updateComponents();
+    }
   }
 }

src/main/java/frc/robot/subsystems/shooter/pivot/ShooterPivotIOSim.java

@@ -15,7 +15,7 @@ public ShooterPivotIOSim() {
             DCMotor.getFalcon500(2),
             46.722,
             10,
-            .13,
+            .24,
             ShooterConstants.kMinAngle.getRadians(),
             ShooterConstants.kMaxAngle.getRadians(),
             false,

src/main/java/frc/robot/utils/shooter/ShooterMath.java → src/main/java/frc/robot/util/shooter/ShooterMath.java

@@ -1,4 +1,4 @@
-package frc.robot.utils.shooter;
+package frc.robot.util.shooter;
 
 import frc.robot.subsystems.shooter.Shooter.ShooterPosition;
 

src/main/java/frc/robot/utils/shooter/ShooterMathTreeMap.java → src/main/java/frc/robot/util/shooter/ShooterMathTreeMap.java

@@ -1,4 +1,4 @@
-package frc.robot.utils.shooter;
+package frc.robot.util.shooter;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
@@ -10,6 +10,9 @@ public class ShooterMathTreeMap implements ShooterMath {
   // in degrees
   private InterpolatingDoubleTreeMap m_shootAngle;
 
+  private final double kMeasurementOffset = 0.0;
+  // when calibrating, this is the x distance from 0.0 on the tape measure to the speaker hole
+
   public ShooterMathTreeMap() {
     m_shootSpeedLeft = new InterpolatingDoubleTreeMap();
     m_shootSpeedRight = new InterpolatingDoubleTreeMap();
@@ -20,10 +23,12 @@ public ShooterMathTreeMap() {
     // Right data
 
     // Angle data
+
   }
 
   @Override
   public ShooterPosition calculateShooterPosition(double distance, double angle) {
+    distance += kMeasurementOffset;
     return new ShooterPosition(
         Rotation2d.fromDegrees(m_shootAngle.get(distance)),
         m_shootSpeedLeft.get(distance),
@@ -32,16 +37,19 @@ public ShooterPosition calculateShooterPosition(double distance, double angle) {
 
   @Override
   public double calculateLeftFlywheelVelocity(double distance) {
+    distance += kMeasurementOffset;
     return m_shootSpeedLeft.get(distance);
   }
 
   @Override
   public double calculateRightFlywheelVelocity(double distance) {
+    distance += kMeasurementOffset;
     return m_shootSpeedRight.get(distance);
   }
 
   @Override
   public double calculatePivotAngle(double distance, double angle) {
+    distance += kMeasurementOffset;
     return m_shootAngle.get(distance);
   }
 }