Skip to content

Commit d3d84ad

Browse files
committed
add pose logging for sim
1 parent 3929014 commit d3d84ad

File tree

5 files changed

+41
-18
lines changed

5 files changed

+41
-18
lines changed

src/main/java/frc/robot/RobotContainer.java

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,7 @@ private void configureButtonBindings() {
8484
: 180))),
8585
swerve));
8686

87+
8788
driver.x().onTrue(
8889
swerve.getSetWheelsXCommand()
8990
);

src/main/java/frc/robot/subsystems/Claw.java

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,8 @@ public class Claw extends SubsystemBase {
1111

1212
public Claw() {
1313

14-
claw = new Neo(1);
14+
claw = new Neo(10);
15+
1516
claw.restoreFactoryDefaults();
1617

1718
claw.setSmartCurrentLimit(30);
@@ -21,6 +22,7 @@ public Claw() {
2122
claw.setInverted(false);
2223

2324
setBrakeMode();
25+
2426

2527
}
2628

src/main/java/frc/robot/subsystems/Swerve.java

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,14 +8,19 @@
88
import edu.wpi.first.math.Nat;
99
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
1010
import edu.wpi.first.math.geometry.Pose2d;
11+
import edu.wpi.first.math.geometry.Pose3d;
1112
import edu.wpi.first.math.geometry.Rotation2d;
13+
import edu.wpi.first.math.geometry.Rotation3d;
14+
import edu.wpi.first.math.geometry.Translation3d;
1215
import edu.wpi.first.math.geometry.Twist2d;
1316
import edu.wpi.first.math.kinematics.ChassisSpeeds;
1417
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
1518
import edu.wpi.first.math.kinematics.SwerveModulePosition;
1619
import edu.wpi.first.math.kinematics.SwerveModuleState;
1720
import edu.wpi.first.wpilibj.Timer;
21+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1822
import frc.robot.util.ADIS16470_IMU;
23+
import frc.robot.util.Pose3dLogger;
1924
import edu.wpi.first.wpilibj2.command.Command;
2025
import edu.wpi.first.wpilibj2.command.SubsystemBase;
2126
import frc.robot.Robot;
@@ -99,6 +104,20 @@ public Swerve() {
99104
@Override
100105
public void periodic() {
101106

107+
SmartDashboard.putNumberArray("RobotPose3d",
108+
Pose3dLogger.composePose3ds(
109+
new Pose3d(
110+
new Translation3d(
111+
getPose().getX(),
112+
getPose().getY(),
113+
Math.hypot(
114+
getRoll().getSin()
115+
* DriveConstants.ROBOT_LENGTH_METERS / 2.0,
116+
getPitch().getSin() *
117+
DriveConstants.ROBOT_LENGTH_METERS / 2.0)),
118+
new Rotation3d(getRoll().getRadians(), getPitch().getRadians(),
119+
getYaw().getRadians()))));
120+
102121
//Update the poseEstimator using the current timestamp (from DriverUI.java), the gyro angle, and the current module states
103122

104123
poseEstimator.updateWithTime(Timer.getFPGATimestamp(), getGyroAngle(), getModulePositions());

src/main/java/frc/robot/util/Constants.java

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323
* numerical or boolean
2424
* constants. This class should not be used for any other purpose. All constants
2525
* should be declared
26+
*
2627
* globally (i.e. public static). Do not put anything functional in this class.
2728
*
2829
* <p>
@@ -32,6 +33,9 @@
3233
*/
3334
public final class Constants {
3435
public static final class DriveConstants {
36+
public static final double ROBOT_LENGTH_METERS = Units.inchesToMeters(25);
37+
38+
3539
// Driving Parameters - Note that these are not the maximum capable speeds of
3640
// the robot, rather the allowed maximum speeds
3741
public static double MAX_SPEED_METERS_PER_SECOND = AutoConstants.MAX_SPEED_METERS_PER_SECOND;
Lines changed: 14 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,22 +1,19 @@
11
package frc.robot.util;
22

33
import edu.wpi.first.math.geometry.Pose3d;
4-
import io.github.oblarg.oblog.Logger;
5-
import io.github.oblarg.oblog.annotations.Log;
64

7-
public class Pose3dLogger extends Logger{
8-
@Log
9-
private Pose3d pose = new Pose3d();
10-
11-
public Pose3dLogger(Pose3d pose) {
12-
this.pose = pose;
13-
}
14-
15-
public Pose3d getPose() {
16-
return this.pose;
17-
}
18-
19-
public void setPose(Pose3d pose) {
20-
this.pose = pose;
5+
public class Pose3dLogger {
6+
public static synchronized double[] composePose3ds(Pose3d... value) {
7+
double[] data = new double[value.length * 7];
8+
for (int i = 0; i < value.length; i++) {
9+
data[i * 7] = value[i].getX();
10+
data[i * 7 + 1] = value[i].getY();
11+
data[i * 7 + 2] = value[i].getZ();
12+
data[i * 7 + 3] = value[i].getRotation().getQuaternion().getW();
13+
data[i * 7 + 4] = value[i].getRotation().getQuaternion().getX();
14+
data[i * 7 + 5] = value[i].getRotation().getQuaternion().getY();
15+
data[i * 7 + 6] = value[i].getRotation().getQuaternion().getZ();
16+
}
17+
return data;
2118
}
22-
}
19+
}

0 commit comments

Comments
 (0)