add pose logging for sim

Patribots4738 · Jan 2, 2024 · d3d84ad · d3d84ad
1 parent 3929014
commit d3d84ad
Show file tree

Hide file tree

Showing 5 changed files with 41 additions and 18 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -84,6 +84,7 @@ private void configureButtonBindings() {
                                                 : 180))),
                         swerve));
 
+
         driver.x().onTrue(
             swerve.getSetWheelsXCommand()
         );

diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
@@ -11,7 +11,8 @@ public class Claw extends SubsystemBase {
 
     public Claw() {
 
-        claw = new Neo(1);
+        claw = new Neo(10);
+
         claw.restoreFactoryDefaults();
 
         claw.setSmartCurrentLimit(30);
@@ -21,6 +22,7 @@ public Claw() {
         claw.setInverted(false);
 
         setBrakeMode();
+
 
     }
 

diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -8,14 +8,19 @@
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 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.Translation3d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.util.ADIS16470_IMU;
+import frc.robot.util.Pose3dLogger;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Robot;
@@ -99,6 +104,20 @@ public Swerve() {
     @Override
     public void periodic() {
 
+      SmartDashboard.putNumberArray("RobotPose3d",
+                Pose3dLogger.composePose3ds(
+                        new Pose3d(
+                                new Translation3d(
+                                        getPose().getX(),
+                                        getPose().getY(),
+                                        Math.hypot(
+                                                getRoll().getSin()
+                                                        * DriveConstants.ROBOT_LENGTH_METERS / 2.0,
+                                                getPitch().getSin() *
+                                                        DriveConstants.ROBOT_LENGTH_METERS / 2.0)),
+                                new Rotation3d(getRoll().getRadians(), getPitch().getRadians(),
+                                        getYaw().getRadians()))));
+
         //Update the poseEstimator using the current timestamp (from DriverUI.java), the gyro angle, and the current module states
 
         poseEstimator.updateWithTime(Timer.getFPGATimestamp(), getGyroAngle(), getModulePositions());

diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java
@@ -23,6 +23,7 @@
  * numerical or boolean
  * constants. This class should not be used for any other purpose. All constants
  * should be declared
+ * 
  * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>
@@ -32,6 +33,9 @@
  */
 public final class Constants {
     public static final class DriveConstants {
+        public static final double ROBOT_LENGTH_METERS = Units.inchesToMeters(25);
+
+
         // Driving Parameters - Note that these are not the maximum capable speeds of
         // the robot, rather the allowed maximum speeds
         public static double MAX_SPEED_METERS_PER_SECOND = AutoConstants.MAX_SPEED_METERS_PER_SECOND;

diff --git a/src/main/java/frc/robot/util/Pose3dLogger.java b/src/main/java/frc/robot/util/Pose3dLogger.java
@@ -1,22 +1,19 @@
 package frc.robot.util;
 
 import edu.wpi.first.math.geometry.Pose3d;
-import io.github.oblarg.oblog.Logger;
-import io.github.oblarg.oblog.annotations.Log;
 
-public class Pose3dLogger extends Logger{
-    @Log
-    private Pose3d pose = new Pose3d();
-
-    public Pose3dLogger(Pose3d pose) {
-        this.pose = pose;
-    }
-
-    public Pose3d getPose() {
-        return this.pose;
-    }
-
-    public void setPose(Pose3d pose) {
-        this.pose = pose;
+public class Pose3dLogger {
+    public static synchronized double[] composePose3ds(Pose3d... value) {
+        double[] data = new double[value.length * 7];
+        for (int i = 0; i < value.length; i++) {
+            data[i * 7] = value[i].getX();
+            data[i * 7 + 1] = value[i].getY();
+            data[i * 7 + 2] = value[i].getZ();
+            data[i * 7 + 3] = value[i].getRotation().getQuaternion().getW();
+            data[i * 7 + 4] = value[i].getRotation().getQuaternion().getX();
+            data[i * 7 + 5] = value[i].getRotation().getQuaternion().getY();
+            data[i * 7 + 6] = value[i].getRotation().getQuaternion().getZ();
+        }
+        return data;
     }
-}   
+}