unfinished sim stuff

Author: SSBASLD

src/main/java/frc/robot/Subsystems/Climber/ClimberIOReal.java

@@ -6,6 +6,8 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.units.measure.Angle;
 import org.littletonrobotics.junction.Logger;
 

src/main/java/frc/robot/Subsystems/Climber/ClimberIOSim.java

@@ -5,6 +5,8 @@
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 import com.ctre.phoenix6.sim.TalonFXSimState;
+
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.units.measure.Angle;
@@ -52,6 +54,7 @@ public void logOutputs(ClimberIOOutputs outputs) {
 		Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/SimLeftRot", outputs.leftPosition.in(Rotations));
 		Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/SimRightRot", outputs.rightPosition.in(Rotations));
 		Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/SimSetpointRot", outputs.setpoint.in(Rotations));
+		Logger.recordOutput(ClimberConstants.SUBSYSTEM_NAME + "/Pose", new Pose3d());
 	}
 
 	@Override

src/main/java/frc/robot/Subsystems/Shooter/Shooter.java

@@ -43,6 +43,7 @@ public void runState() {
 		Logger.recordOutput(SUBSYSTEM_NAME + "/WheelSetpoint", outputs.wheelSetpoint.in(RotationsPerSecond));
 		Logger.recordOutput(SUBSYSTEM_NAME + "/HoodAngle", outputs.hoodAngle.in(Degrees));
 		Logger.recordOutput(SUBSYSTEM_NAME + "/HoodSetpoint", outputs.hoodSetpoint.in(Degrees));
+		Logger.recordOutput(SUBSYSTEM_NAME + "/HoodPose", outputs.hoodPose);
 		Logger.recordOutput(SUBSYSTEM_NAME + "/state", getState().getStateString());
 		Logger.recordOutput(SUBSYSTEM_NAME + "/ReadyToShoot", readyToShoot());
 	}

src/main/java/frc/robot/Subsystems/Shooter/ShooterIO.java

@@ -1,5 +1,6 @@
 package frc.robot.Subsystems.Shooter;
 
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 
@@ -10,7 +11,8 @@ class ShooterIOOutputs {
 		public AngularVelocity rightWheelVelocity;
 		public AngularVelocity wheelSetpoint;
 		public Angle hoodAngle;
-		public Angle hoodSetpoint;
+		public Angle hoodSetpoint;	
+		public Pose3d hoodPose;
 	}
 
 	public abstract void logOutputs(ShooterIOOutputs outputs);

src/main/java/frc/robot/Subsystems/Shooter/ShooterIOSim.java

@@ -11,6 +11,9 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 import com.ctre.phoenix6.sim.TalonFXSimState;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.util.Units;
@@ -78,6 +81,8 @@ public void logOutputs(ShooterIOOutputs outputs) {
 		outputs.hoodAngle = Radians.of(hoodSim.getAngleRads());
 		outputs.hoodSetpoint = hoodSetpoint;
 		// Throw some stuff here for 3D sim later
+
+		outputs.hoodPose = new Pose3d(0, 0, 0, new Rotation3d(Radians.of(hoodSim.getAngleRads()), Radians.of(0), Radians.of(0)));
 	}
 
 	@Override

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

@@ -3,9 +3,14 @@
 import static edu.wpi.first.units.Units.*;
 import static frc.robot.Subsystems.Intake.IntakeConstants.*;
 
+import org.littletonrobotics.junction.Logger;
+
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.sim.TalonFXSimState;
 import edu.wpi.first.math.controller.PIDController;
+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.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -59,6 +64,8 @@ public void logOutputs(IntakeIOOutputs outputs) {
 		outputs.linearPosition = Meters.of(linearSim.getAngularPositionRotations() * LINEAR_METERS_PER_ROTATION);
 		outputs.linearSetpoint = setpoint;
 		outputs.linearVelocity = MetersPerSecond.of(linearSim.getAngularVelocity().in(RotationsPerSecond) * LINEAR_METERS_PER_ROTATION);
+
+		Logger.recordOutput("Intake/Intake Position", new Pose3d(0.296694, 0.0, 0.223, new Rotation3d(Radians.of(0), Radians.of(linearSim.getAngularPositionRotations()), Radians.of(0))));
 	}
 
 	@Override