From d3d84adb42d96036661b7a4fdb5cfa9a88690a56 Mon Sep 17 00:00:00 2001 From: Jacob Hotz Date: Tue, 2 Jan 2024 11:15:11 -0800 Subject: [PATCH] add pose logging for sim --- src/main/java/frc/robot/RobotContainer.java | 1 + src/main/java/frc/robot/subsystems/Claw.java | 4 ++- .../java/frc/robot/subsystems/Swerve.java | 19 ++++++++++++ src/main/java/frc/robot/util/Constants.java | 4 +++ .../java/frc/robot/util/Pose3dLogger.java | 31 +++++++++---------- 5 files changed, 41 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 90dcaa3..791b439 100644 --- 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 index ed82804..7956113 100644 --- 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 index 1a061c1..6fc0082 100644 --- 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 index 1c4b62b..ae39de6 100644 --- 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. * *

@@ -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 index 0502e3b..c1f40a7 100644 --- 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; } -} \ No newline at end of file +}