add 3d component poses to arm visualizer

team4909 · Feb 7, 2024 · f78834d · f78834d
1 parent cbaad89
commit f78834d
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 14 deletions.
diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
@@ -1,9 +1,5 @@
 package frc.robot.arm;
 
-import java.util.Optional;
-
-import org.littletonrobotics.junction.Logger;
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
@@ -13,6 +9,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.arm.ArmConfig.JointConfig;
+import java.util.Optional;
+import org.littletonrobotics.junction.Logger;
 
 public class Arm extends SubsystemBase {
 
@@ -64,7 +62,9 @@ public void setSetpoint(Translation2d setpoint) {
   // }
 
   Translation2d folded = new Translation2d(0.0762, 0.4699);
-  Translation2d trapSetpoint = new Translation2d(Units.inchesToMeters(19.8), Units.inchesToMeters(30));
+  Translation2d trapSetpoint =
+      new Translation2d(Units.inchesToMeters(19.8), Units.inchesToMeters(30));
+
   public Command testSetpoint() {
     return this.run(() -> setSetpoint(trapSetpoint)).withName("Test Setpoint");
   }
@@ -144,9 +144,8 @@ public Optional<Vector<N2>> inverse(Translation2d position) {
           || wristAngle > m_wristConfig.maxAngle()) {
         return Optional.empty();
       }
-      // return Optional.of(VecBuilder.fill(Units.degreesToRadians(0),
-      // Units.degreesToRadians(0)));
-      return Optional.of(VecBuilder.fill(elbowAngle, wristAngle));
+      return Optional.of(VecBuilder.fill(Units.degreesToRadians(0), Units.degreesToRadians(0)));
+      // return Optional.of(VecBuilder.fill(elbowAngle, wristAngle));
     }
   }
 }
diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotController;
 import frc.robot.Constants;
 
@@ -176,11 +177,12 @@ public void updateSim() {
     final TalonFXSimState wristSimState = m_wristLeftMotor.getSimState();
     elbowSimState.setSupplyVoltage(RobotController.getBatteryVoltage());
     wristSimState.setSupplyVoltage(RobotController.getBatteryVoltage());
-    elbowWristSimStates =
-        m_armSimModel.simulate(
-            elbowWristSimStates,
-            VecBuilder.fill(elbowSimState.getMotorVoltage(), wristSimState.getMotorVoltage()),
-            0.02);
+    if (!DriverStation.isDisabled())
+      elbowWristSimStates =
+          m_armSimModel.simulate(
+              elbowWristSimStates,
+              VecBuilder.fill(elbowSimState.getMotorVoltage(), wristSimState.getMotorVoltage()),
+              0.02);
     elbowSimState.setRawRotorPosition(Units.radiansToRotations(elbowWristSimStates.get(0, 0)));
     elbowSimState.setRotorVelocity(Units.radiansToRotations(elbowWristSimStates.get(2, 0)));
     wristSimState.setRawRotorPosition(Units.radiansToRotations(elbowWristSimStates.get(1, 0)));

diff --git a/src/main/java/frc/robot/arm/ArmVisualizer.java b/src/main/java/frc/robot/arm/ArmVisualizer.java
@@ -1,5 +1,9 @@
 package frc.robot.arm;
 
+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 edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
@@ -41,8 +45,21 @@ public ArmVisualizer(String logKey) {
   }
 
   public void update(double elbowAngle, double wristAngle) {
-    m_elbowLigament.setAngle(Units.radiansToDegrees(elbowAngle) - 90.0);
+    m_elbowLigament.setAngle(Units.radiansToDegrees(elbowAngle) - kShoulderAngleDegrees);
     m_wristLigament.setAngle(Units.radiansToDegrees(wristAngle));
     Logger.recordOutput("Mechanism2d/" + m_logKey, m_mechanism);
+
+    Pose3d elbowPose =
+        new Pose3d(
+            ArmConfig.kOrigin.getX(),
+            0.0,
+            ArmConfig.kOrigin.getY(),
+            new Rotation3d(0.0, -elbowAngle, 0.0));
+    Pose3d wristPose =
+        elbowPose.transformBy(
+            new Transform3d(
+                new Translation3d(ArmConfig.kElbowConfig.length(), 0.0, 0.0),
+                new Rotation3d(0.0, -wristAngle, 0.0)));
+    Logger.recordOutput("Mechanism3d/" + m_logKey, elbowPose, wristPose);
   }
 }