finish sim viz and add model files

TitaniumTitans · Jan 18, 2025 · 0de21ac · 0de21ac
1 parent a8e32bb
commit 0de21ac
Show file tree

Hide file tree

Showing 7 changed files with 137 additions and 21 deletions.
diff --git a/Robot_Thetys/config.json b/Robot_Thetys/config.json
@@ -0,0 +1,70 @@
+{
+  "name": "4467 Tethys",
+  "sourceUrl": "https://cad.onshape.com/documents/7bc29be4342772fc741ea201/w/72d85fa68f0e9d0ce28c624b/e/033aff10bbfab8525e72a496",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 0
+    },
+    {
+      "axis" : "y",
+      "degrees": 0
+    },
+    {
+      "axis": "z",
+      "degrees": 180
+    }
+  ],
+  "position": [
+    0.3302, 
+    0.3302, 
+    -0.0254
+  ],
+  "cameras": [],
+  "components": [
+	{
+	  "zeroedRotations": [
+		{
+		  "axis": "x",
+		  "degrees": 0
+		},
+		{
+		  "axis": "y",
+		  "degrees": 0
+		},
+		{
+		  "axis": "z",
+		  "degrees": 0
+		}
+	  ],
+
+	  "zeroedPosition": [
+		0.0,
+		0.0,
+		0.0
+	  ]
+	},
+	{
+	  "zeroedRotations": [
+		{
+		  "axis": "y",
+		  "degrees": 0
+		},
+		{
+		  "axis": "x",
+		  "degrees": 0
+		},
+		{
+		  "axis": "z",
+		  "degrees": 0
+		}
+	  ],
+
+	  "zeroedPosition": [
+		0.0,
+		0.0,
+		0.0
+	  ]
+	}
+  ]
+}
diff --git a/Robot_Thetys/model.glb b/Robot_Thetys/model.glb
diff --git a/Robot_Thetys/model_0.glb b/Robot_Thetys/model_0.glb
diff --git a/Robot_Thetys/model_1.glb b/Robot_Thetys/model_1.glb
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -85,31 +85,12 @@ public Robot() {
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
 
-    double xOffset = Units.inchesToMeters(10.75);
-    double yOffset = Units.inchesToMeters(9.75);
+
     double angle = -45;
 
     Logger.recordOutput("Blank Pose 3D", new Pose3d());
     Logger.recordOutput("Blank Pose 3D Array", new Pose3d(), new Pose3d());
-    Logger.recordOutput("90 Intake Pose 3D Array",
-        new Pose3d(
-            new Translation3d(
-                xOffset, //(xOffset * Math.cos(Units.degreesToRadians(angle)) - yOffset * Math.sin(Units.degreesToRadians(angle))),
-                0.3302,
-                yOffset //(yOffset * Math.cos(Units.degreesToRadians(angle)) + xOffset * Math.sin(Units.degreesToRadians(angle)))
-            ),
-            new Rotation3d(0.0, Units.degreesToRadians(angle), Units.degreesToRadians(180.0))
-        ).transformBy(
-            new Transform3d(
-                new Translation3d(
-                    Units.inchesToMeters(2.3),
-                    0.0,
-                    Units.inchesToMeters(9.75)
-                ).unaryMinus(),
-                new Rotation3d()
-            )
-        ),
-        new Pose3d());
+
   }
 
   @Override

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
@@ -7,15 +7,19 @@
 public class IntakeSubsystem extends SubsystemBase {
     private final IntakeIO io;
     private final IntakeIOInputsAutoLogged inputs;
+    private final IntakeVisualizer visualizer;
     public IntakeSubsystem(IntakeIO io) {
         this.io = io;
         inputs = new IntakeIOInputsAutoLogged();
+        visualizer = new IntakeVisualizer("Intake/");
     }
 
     @Override
     public void periodic() {
         io.updateInputs(inputs);
         Logger.processInputs("Intake", inputs);
+
+        visualizer.updateVisualization(0.0, 0.0);
     }
     public void setIntakePower(double power) {
         io.setMotorVoltageIntake(power);

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeVisualizer.java b/src/main/java/frc/robot/subsystems/intake/IntakeVisualizer.java
@@ -0,0 +1,61 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.math.geometry.Pose3d;
+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.util.Units;
+import org.littletonrobotics.junction.Logger;
+
+public class IntakeVisualizer {
+  private final double xOffsetIntake = Units.inchesToMeters(10.75);
+  private final double yOffsetIntake = Units.inchesToMeters(9.75);
+  private final double xOffsetCoral = Units.inchesToMeters(-7.5);
+  private final double yOffsetCoral = Units.inchesToMeters(18.8);
+
+  private final String key;
+
+  public IntakeVisualizer(String key) {
+    this.key = key;
+  }
+
+  public void updateVisualization(double intakeAngle, double coralAngle) {
+    Pose3d intakePose = new Pose3d(
+        new Translation3d(
+            xOffsetIntake,
+            0.3302,
+            yOffsetIntake
+        ),
+        new Rotation3d(0.0, Units.degreesToRadians(45 - intakeAngle), Units.degreesToRadians(180.0))
+    ).transformBy(
+        new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(2.3),
+                0.0,
+                Units.inchesToMeters(9.75)
+            ).unaryMinus(),
+            new Rotation3d()
+        ));
+
+    Pose3d coralPose = new Pose3d(
+        new Translation3d(
+            xOffsetCoral,
+            0.3302,
+            yOffsetCoral
+        ),
+        new Rotation3d(0.0, Units.degreesToRadians(coralAngle - 80), Units.degreesToRadians(180.0))
+    ).transformBy(
+        new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(20.5),
+                0.0,
+                Units.inchesToMeters(18.8)
+            ).unaryMinus(),
+            new Rotation3d()
+        ));
+
+    Logger.recordOutput(key + "3D Pose",
+        intakePose,
+        coralPose);
+  }
+}