Comp robot viz

src/main/java/frc/robot/Robot.java

@@ -8,7 +8,12 @@
 import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
 import edu.wpi.first.math.MathUtil;
+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.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
@@ -29,6 +34,7 @@
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Superstructure.SuperState;
+import frc.robot.components.canrange.CANrangeIOReal;
 import frc.robot.components.rollers.RollerIO;
 import frc.robot.components.rollers.RollerIOSim;
 import frc.robot.subsystems.climber.ClimberIO;
@@ -39,6 +45,8 @@
 import frc.robot.subsystems.indexer.SpindexerSubsystem;
 import frc.robot.subsystems.intake.FintakeSubsystem;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.LinearRackIO;
+import frc.robot.subsystems.intake.LinearRackIOSim;
 import frc.robot.subsystems.intake.LintakeSubsystem;
 import frc.robot.subsystems.led.LEDIOReal;
 import frc.robot.subsystems.led.LEDSubsystem;
@@ -52,6 +60,7 @@
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread;
 import frc.robot.utils.CommandXboxControllerSubsystem;
+import frc.robot.utils.LoggedTunableNumber;
 import java.util.Optional;
 import java.util.Set;
 import org.ironmaple.simulation.SimulatedArena;
@@ -171,6 +180,8 @@ public enum RobotEdition {
   private final LEDSubsystem leds;
   private final ClimberSubsystem climber;
 
+  private Intake intake = null;
+
   // climber only exists for the comp bot - this is accounted for later
 
   private final Superstructure superstructure;
@@ -216,7 +227,6 @@ public Robot() {
     // break
     // granted this would never actually happen but
     Indexer indexer = null;
-    Intake intake = null;
     Shooter shooter = null;
 
     // this looks at the ROBOT_EDITION variable and decides which version of each subsystem to
@@ -312,7 +322,27 @@ public Robot() {
                             DCMotor.getKrakenX44Foc(1)),
                         MotorType.KrakenX44,
                         canivore));
-        intake = new LintakeSubsystem();
+        // TODO: FOVs
+        intake =
+            (ROBOT_MODE == RobotMode.REAL)
+                ? new LintakeSubsystem(
+                    new LinearRackIO(14, canivore, LintakeSubsystem.getRackMotorConfig()),
+                    new RollerIO(8, LintakeSubsystem.getRollerMotorConfig(), canivore),
+                    new CANrangeIOReal(0, canivore, 10))
+                : new LintakeSubsystem(
+                    new LinearRackIOSim(14, canivore, LintakeSubsystem.getRackMotorConfig()),
+                    new RollerIOSim(
+                        8,
+                        LintakeSubsystem.getRollerMotorConfig(),
+                        new DCMotorSim(
+                            LinearSystemId.createDCMotorSystem(
+                                DCMotor.getKrakenX44Foc(1),
+                                0.001,
+                                LintakeSubsystem.ROLLER_GEAR_RATIO),
+                            DCMotor.getKrakenX44Foc(1)),
+                        MotorType.KrakenX44,
+                        canivore),
+                    new CANrangeIOReal(0, canivore, 10));
         shooter =
             new TurretSubsystem(
                 ROBOT_MODE == RobotMode.REAL
@@ -564,13 +594,80 @@ private void addAutos() {
   // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid());
   // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid());
 
+  private LoggedTunableNumber turretAngle = new LoggedTunableNumber("Turret Angle Rads", 0);
+  private LoggedTunableNumber hoodAngle = new LoggedTunableNumber("Hood angle rads", 0);
+  private LoggedTunableNumber intakeExtension = new LoggedTunableNumber("Intake extension", 0);
+
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
 
     superstructure.periodic();
 
-    // TODO Log mechanism poses
+    // TODO: YAW VALUE FROM HARDWARE
+    Pose3d turretPose =
+        new Pose3d(
+            new Translation3d(-0.177413, -0.111702, 0.350341),
+            new Rotation3d(0, 0, turretAngle.getAsDouble()));
+
+    // TODO: USE MEASURED EXTENSIONS AND ANGLES
+    Logger.recordOutput(
+        "Robot/Mechanism Poses",
+        new Pose3d[] {
+          // Turret
+          turretPose,
+          // Hood
+          turretPose
+              // First transform the hood out to the hood pivot, and rotate by the amount needed
+              .transformBy(
+                  new Transform3d(
+                      new Translation3d(-0.095638, 0, 0.095123),
+                      new Rotation3d(0, hoodAngle.getAsDouble() * -1, 0)))
+              // Then, transform the hood back to the correct location relative to the turret
+              .transformBy(
+                  new Transform3d(
+                      new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)),
+          // Intake
+          new Pose3d(
+              intake.getExtensionMeters() * LintakeSubsystem.INTAKE_ROTATION.getCos(),
+              0,
+              -(intake.getExtensionMeters() * LintakeSubsystem.INTAKE_ROTATION.getSin()),
+              Rotation3d.kZero),
+          // Climber
+          new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero)
+        });
+
+    // TODO: ACTUAL SETPOINT
+    Pose3d turretSetpoint =
+        new Pose3d(
+            new Translation3d(-0.177413, -0.111702, 0.350341),
+            new Rotation3d(0, 0, turretAngle.getAsDouble()));
+    // TODO: ACTUAL SETPOINTS
+    Logger.recordOutput(
+        "Robot/Mechanism Setpoints",
+        new Pose3d[] {
+          // Turret
+          turretSetpoint,
+          // Hood
+          turretSetpoint
+              // First transform the hood out to the hood pivot, and rotate by the amount needed
+              .transformBy(
+                  new Transform3d(
+                      new Translation3d(-0.095638, 0, 0.095123),
+                      new Rotation3d(0, hoodAngle.getAsDouble() * -1, 0)))
+              // Then, transform the hood back to the correct location relative to the turret
+              .transformBy(
+                  new Transform3d(
+                      new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)),
+          // Intake
+          new Pose3d(
+              intake.getExtensionSetpointMeters() * LintakeSubsystem.INTAKE_ROTATION.getCos(),
+              0,
+              -(intake.getExtensionSetpointMeters() * LintakeSubsystem.INTAKE_ROTATION.getSin()),
+              Rotation3d.kZero),
+          // Climber
+          new Pose3d(0, 0, climber.getClimberSetpointMeters(), Rotation3d.kZero)
+        });
 
     updateAlerts();
   }
@@ -634,7 +731,21 @@ public void simulationInit() {
   }
 
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    // Log zeroed poses for mechs and robot for debugging in sim
+    Logger.recordOutput(
+        "Robot/Zeroed Mechanism Poses",
+        new Pose3d[] {
+          // Turret
+          new Pose3d(),
+          // Hood
+          new Pose3d(),
+          new Pose3d(),
+          new Pose3d()
+        });
+
+    Logger.recordOutput("Robot/Zero Position", new Pose2d());
+  }
 
   @Override
   public void disabledInit() {}

src/main/java/frc/robot/subsystems/climber/ClimberIO.java

@@ -10,19 +10,19 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Temperature;
 import edu.wpi.first.units.measure.Voltage;
 import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.AutoLogOutput;
 
 public class ClimberIO {
 
   @AutoLog
   public static class ClimberIOInputs {
-    public Rotation2d motorPositionRotations = new Rotation2d();
+    public double motorPositionMeters = 0.0;
     public double motorVelocityMetersPerSec = 0.0;
     public double motorStatorCurrentAmps = 0.0;
     public double motorSupplyCurrentAmps = 0.0;
@@ -32,8 +32,9 @@ public static class ClimberIOInputs {
 
   protected final TalonFX climberMotor;
 
-  private final StatusSignal<Angle> motorPositionRotations;
-  private final StatusSignal<AngularVelocity> angularVelocityRotsPerSec;
+  // Rotation -> linear conversion happens in sensor to mech ratio
+  private final StatusSignal<Angle> motorPositionMeters;
+  private final StatusSignal<AngularVelocity> velocityMetersPerSec;
   private final StatusSignal<Current> statorCurrentAmps;
   private final StatusSignal<Current> supplyCurrentAmps;
   private final StatusSignal<Voltage> motorVoltage;
@@ -50,21 +51,21 @@ public ClimberIO(CANBus canBus) {
     climberMotor = new TalonFX(30, canBus);
     climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration());
 
-    angularVelocityRotsPerSec = climberMotor.getVelocity();
+    velocityMetersPerSec = climberMotor.getVelocity();
     supplyCurrentAmps = climberMotor.getSupplyCurrent();
     motorVoltage = climberMotor.getMotorVoltage();
     statorCurrentAmps = climberMotor.getStatorCurrent();
     motorTemp = climberMotor.getDeviceTemp();
-    motorPositionRotations = climberMotor.getPosition();
+    motorPositionMeters = climberMotor.getPosition();
 
     BaseStatusSignal.setUpdateFrequencyForAll(
         50.0,
-        angularVelocityRotsPerSec,
+        velocityMetersPerSec,
         supplyCurrentAmps,
         motorVoltage,
         statorCurrentAmps,
         motorTemp,
-        motorPositionRotations);
+        motorPositionMeters);
     climberMotor.optimizeBusUtilization();
   }
 
@@ -76,7 +77,8 @@ public static TalonFXConfiguration getClimberConfiguration() {
     config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
     // todo: find and make climber gear ratio variable
-    config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO;
+    config.Feedback.SensorToMechanismRatio =
+        ClimberSubsystem.GEAR_RATIO * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS);
 
     // todo: tune
     config.Slot0.kS = 0.0;
@@ -108,19 +110,23 @@ public void setClimberVelocity(double climberVelocity) {
 
   public void updateInputs(ClimberIOInputs inputs) {
     BaseStatusSignal.refreshAll(
-        motorPositionRotations,
-        angularVelocityRotsPerSec,
+        motorPositionMeters,
+        velocityMetersPerSec,
         statorCurrentAmps,
         supplyCurrentAmps,
         motorVoltage,
         motorTemp);
 
-    inputs.motorPositionRotations =
-        Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble());
+    inputs.motorPositionMeters = motorPositionMeters.getValueAsDouble();
     inputs.motorVoltage = motorVoltage.getValueAsDouble();
     inputs.motorTempC = motorTemp.getValueAsDouble();
     inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble();
     inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble();
-    inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble();
+    inputs.motorVelocityMetersPerSec = velocityMetersPerSec.getValueAsDouble();
+  }
+
+  @AutoLogOutput(key = "Climber/Setpoint Meters")
+  public double getClimberSetpointMeters() {
+    return climberSetpoint;
   }
 }

src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems.climber;
 
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
@@ -8,6 +9,7 @@ public class ClimberSubsystem extends SubsystemBase {
   // todo: find actual constants
   public static double GEAR_RATIO = (45.0 / 1.0);
   public static double MAX_EXTENSION_METERS = 0.2413;
+  public static double SPOOL_DIAMETER_METERS = Units.inchesToMeters(0.668898);
   public static double MAX_ACCELERATION = 10.0;
   public static double MAX_VELOCITY = 2.0;
 
@@ -39,4 +41,13 @@ public Command retractClimber() {
           climberIO.setClimberPosition(0.0);
         });
   }
+
+  public double getClimberExtensionMeters() {
+    // Convert rotations into linear motion
+    return climberInputs.motorPositionMeters;
+  }
+
+  public double getClimberSetpointMeters() {
+    return climberIO.getClimberSetpointMeters();
+  }
 }

src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java

@@ -98,4 +98,14 @@ public static TalonFXConfiguration getIntakeConfig() {
   public boolean beambreak() {
     return canrangeInputs.isDetected;
   }
+
+  @Override
+  public double getExtensionMeters() {
+    return 0;
+  }
+
+  @Override
+  public double getExtensionSetpointMeters() {
+    return 0;
+  }
 }

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -19,4 +19,8 @@ public interface Intake {
 
   /** for controller rumble */
   public boolean beambreak();
+
+  public double getExtensionMeters();
+
+  public double getExtensionSetpointMeters();
 }