Skip to content

Commit f6d300c

Browse files
FatblabsFatblabs
authored
Fix Issue 166: DataLog overrunning buffers, causing wpilog files to abort mid-match (#167)
* Commented out stuff that needs to be NOT in 250hz periods. Will work on this later * Updated the frequency at which the nt stuff gets published in the telemterize() function in the Telemetry class to 50 hz. Also updated the flush frequency to 60hz * name is wrong * dynamic frequency? I'm not sure if state.OdometryPeriod is always changing or not, but if it is then this would dynamically adapt to changing frequencies * comment for clearness * changed flush frequency to double the default value, and changed variable names. Also added divide by zero check for state.OdometryPeriod * changed flush frequency to 10hz * removed all NT related stuff outside of telemeterize function. Created seperate drivebase simulation class so that i can be updated in robot.periodic() in 20ms intervals, rather than 4ms intervals (nominal 250hz). Added some null checks here and there, and mentor changes * Mentor changes * unstatic'ed thecachedDriveState * Well if it isn't "DrivebaseSim", then it must be not sim related. * Changed class names * Mentor changes * moved to util folder * Removed legacy code, replaced with the solution that was hiding in plain sight 😭 * made the turret nt publisher a pose2d rather than a translation 2d as requested by connor. * seperated NT updating * resume logs periodically every second
1 parent 81683e5 commit f6d300c

5 files changed

Lines changed: 128 additions & 91 deletions

File tree

‎src/main/java/frc/robot/Controls.java‎

Lines changed: 0 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -104,8 +104,6 @@ public class Controls {
104104
.withRotationalDeadband(0.0001)
105105
.withDriveRequestType(DriveRequestType.Velocity);
106106

107-
private final Telemetry logger = new Telemetry(MaxSpeed);
108-
109107
/** The container for the robot. Contains subsystems, OI devices, and commands. */
110108
public Controls(Subsystems subsystems, SimWrapper simWrapper) {
111109
// Configure the trigger bindings
@@ -225,9 +223,6 @@ private void configureDrivebaseBindings() {
225223
s.drivebaseSubsystem.runOnce(() -> m_simWrapper.cycleResetPosition(Pose2d.kZero)));
226224
}
227225

228-
// logging the telemetry
229-
s.drivebaseSubsystem.registerTelemetry(logger::telemeterize);
230-
231226
// reset pose incase vision is bugging
232227
driverController
233228
.rightBumper()

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 24 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
import edu.wpi.first.wpilibj.RobotBase;
1818
import edu.wpi.first.wpilibj.RobotController;
1919
import edu.wpi.first.wpilibj.TimedRobot;
20+
import edu.wpi.first.wpilibj.Timer;
2021
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
2122
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
2223
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -30,9 +31,12 @@
3031
import frc.robot.subsystems.auto.AutonomousField;
3132
import frc.robot.util.AllianceUtils;
3233
import frc.robot.util.BuildInfo;
34+
import frc.robot.util.DriveStateNtLogger;
35+
import frc.robot.util.DriveStateSignalLogger;
3336
import frc.robot.util.HubShiftUtil;
3437
import frc.robot.util.LimelightHelpers;
3538
import frc.robot.util.simulation.RobotSim;
39+
import frc.robot.util.tuning.LauncherConstants;
3640

3741
/**
3842
* The methods in this class are called automatically corresponding to each mode, as described in
@@ -53,6 +57,12 @@ public class Robot extends TimedRobot {
5357
private final Mechanism2d mechanismRobot;
5458
private final SimWrapper m_simWrapper;
5559
private final double BROWNOUT_VOLTAGE = 6.4; // Limelight's minimum operating voltage is 3.3volts
60+
private static final double DATA_LOG_FLUSH_PERIOD_S = 1.0 / 14.0; // 14 Hz flush
61+
private final DriveStateNtLogger driveBaseSim;
62+
private final DriveStateSignalLogger logger;
63+
64+
// Cached time for robot.periodic()
65+
private double LAST_TIME = 0;
5666

5767
/**
5868
* This function is run when the robot is first started up and should be used for any
@@ -65,7 +75,7 @@ protected Robot() {
6575

6676
// logging
6777
if (RobotBase.isReal()) {
68-
DataLogManager.start();
78+
DataLogManager.start("", "", DATA_LOG_FLUSH_PERIOD_S);
6979
DriverStation.startDataLog(DataLogManager.getLog(), true);
7080
}
7181
PDH = new PowerDistribution(Hardware.PDH_ID, PowerDistribution.ModuleType.kRev);
@@ -133,6 +143,10 @@ protected Robot() {
133143
CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand());
134144
}
135145
WebServer.start(5800, Filesystem.getDeployDirectory().getPath());
146+
147+
logger = new DriveStateSignalLogger();
148+
subsystems.drivebaseSubsystem.registerTelemetry(logger::telemeterize);
149+
driveBaseSim = logger.DrivebaseSim(Controls.MaxSpeed);
136150
}
137151

138152
/**
@@ -144,6 +158,13 @@ protected Robot() {
144158
*/
145159
@Override
146160
public void robotPeriodic() {
161+
// Resume logging every X seconds
162+
double time = Timer.getFPGATimestamp();
163+
if (time - LAST_TIME >= 1) {
164+
LAST_TIME = time;
165+
DataLogManager.getLog().resume();
166+
}
167+
147168
// $VISIONSIM - Wrapper for sim features
148169
if (Robot.isSimulation() && m_simWrapper != null) {
149170
// NOTE: We run the vision period FIRST in robotPeriodic, since it updates
@@ -162,6 +183,8 @@ public void robotPeriodic() {
162183
// var robotState = subsystems.drivebaseSubsystem.getState();
163184
// LauncherConstants.update(robotState.Pose, subsystems.drivebaseSubsystem);
164185
CommandScheduler.getInstance().run();
186+
driveBaseSim.update();
187+
LauncherConstants.UpdateNT(subsystems.drivebaseSubsystem.getState().Pose);
165188
}
166189

167190
/** This function is called once each time the robot enters Disabled mode. */

src/main/java/frc/robot/Telemetry.java renamed to src/main/java/frc/robot/util/DriveStateNtLogger.java

Lines changed: 30 additions & 68 deletions
Original file line numberDiff line numberDiff line change
@@ -1,51 +1,33 @@
1-
package frc.robot;
1+
package frc.robot.util;
22

3-
import com.ctre.phoenix6.SignalLogger;
43
import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
54
import edu.wpi.first.math.geometry.Pose2d;
6-
import edu.wpi.first.math.geometry.Translation2d;
75
import edu.wpi.first.math.kinematics.ChassisSpeeds;
86
import edu.wpi.first.math.kinematics.SwerveModulePosition;
97
import edu.wpi.first.math.kinematics.SwerveModuleState;
10-
import edu.wpi.first.networktables.DoubleArrayPublisher;
118
import edu.wpi.first.networktables.DoublePublisher;
129
import edu.wpi.first.networktables.NetworkTable;
1310
import edu.wpi.first.networktables.NetworkTableInstance;
14-
import edu.wpi.first.networktables.StringPublisher;
1511
import edu.wpi.first.networktables.StructArrayPublisher;
1612
import edu.wpi.first.networktables.StructPublisher;
1713
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
1814
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
1915
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2016
import edu.wpi.first.wpilibj.util.Color;
2117
import edu.wpi.first.wpilibj.util.Color8Bit;
22-
import frc.robot.util.AllianceUtils;
23-
import frc.robot.util.tuning.LauncherConstants;
2418

25-
public class Telemetry {
26-
private final double MaxSpeed;
27-
28-
/**
29-
* Construct a telemetry object, with the specified max speed of the robot
30-
*
31-
* @param maxSpeed Maximum speed in meters per second
32-
*/
33-
public Telemetry(double maxSpeed) {
34-
MaxSpeed = maxSpeed;
35-
SignalLogger.start();
36-
}
37-
38-
/* What to publish over networktables for telemetry */
19+
public class DriveStateNtLogger {
3920
private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
21+
private final DriveStateSignalLogger telem;
22+
private final double MaxSpeed;
4023

4124
/* Robot swerve drive state */
4225
private final NetworkTable driveStateTable = inst.getTable("DriveState");
26+
27+
// This correctly publishes as a Struct, completely avoiding the race condition
4328
private final StructPublisher<Pose2d> drivePose =
4429
driveStateTable.getStructTopic("Pose", Pose2d.struct).publish();
45-
private final StructPublisher<Translation2d> turretTranslation =
46-
driveStateTable.getStructTopic("Turret Pose", Translation2d.struct).publish();
47-
private final DoublePublisher turretToHubDistance =
48-
driveStateTable.getDoubleTopic("Turret to hub distance").publish();
30+
4931
private final StructPublisher<ChassisSpeeds> driveSpeeds =
5032
driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish();
5133
private final StructArrayPublisher<SwerveModuleState> driveModuleStates =
@@ -59,11 +41,6 @@ public Telemetry(double maxSpeed) {
5941
private final DoublePublisher driveOdometryFrequency =
6042
driveStateTable.getDoubleTopic("OdometryFrequency").publish();
6143

62-
/* Robot pose for field positioning */
63-
private final NetworkTable table = inst.getTable("Pose");
64-
private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
65-
private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
66-
6744
/* Mechanisms to represent the swerve module states */
6845
private final Mechanism2d[] m_moduleMechanisms =
6946
new Mechanism2d[] {
@@ -102,52 +79,37 @@ public Telemetry(double maxSpeed) {
10279
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
10380
};
10481

105-
private final double[] m_poseArray = new double[3];
106-
private final double[] m_moduleStatesArray = new double[8];
107-
private final double[] m_moduleTargetsArray = new double[8];
108-
109-
/** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
110-
public void telemeterize(SwerveDriveState state) {
111-
/* Telemeterize the swerve drive state */
112-
drivePose.set(state.Pose);
113-
var turret = LauncherConstants.launcherFromRobot(state.Pose);
114-
var robotToHubMeters = AllianceUtils.getHubTranslation2d().minus(turret).getNorm();
115-
turretTranslation.set(turret);
116-
turretToHubDistance.set(robotToHubMeters);
117-
driveSpeeds.set(state.Speeds);
118-
driveModuleStates.set(state.ModuleStates);
119-
driveModuleTargets.set(state.ModuleTargets);
120-
driveModulePositions.set(state.ModulePositions);
121-
driveTimestamp.set(state.Timestamp);
122-
driveOdometryFrequency.set(1.0 / state.OdometryPeriod);
123-
124-
/* Also write to log file */
125-
m_poseArray[0] = state.Pose.getX();
126-
m_poseArray[1] = state.Pose.getY();
127-
m_poseArray[2] = state.Pose.getRotation().getDegrees();
82+
public DriveStateNtLogger(DriveStateSignalLogger telemetry, double MaxSpeed) {
83+
this.MaxSpeed = MaxSpeed;
84+
this.telem = telemetry;
85+
/* Telemeterize the module states to a Mechanism2d */
12886
for (int i = 0; i < 4; ++i) {
129-
m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians();
130-
m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond;
131-
m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians();
132-
m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond;
87+
SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
13388
}
89+
// Legacy double[] publisher has been removed entirely
90+
}
13491

135-
SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray);
136-
SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray);
137-
SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray);
138-
SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
139-
140-
/* Telemeterize the pose to a Field2d */
141-
fieldTypePub.set("Field2d");
142-
fieldPub.set(m_poseArray);
92+
// should be updated periodically in robot.periodic()
93+
public void update() {
94+
SwerveDriveState state = telem.returnDriveState();
95+
// null check
96+
if (state == null) {
97+
return;
98+
}
14399

144-
/* Telemeterize the module states to a Mechanism2d */
145100
for (int i = 0; i < 4; ++i) {
146101
m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
147102
m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
148103
m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));
149-
150-
SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
151104
}
105+
106+
/* Telemeterize the swerve drive state */
107+
drivePose.set(state.Pose);
108+
driveSpeeds.set(state.Speeds);
109+
driveModuleStates.set(state.ModuleStates);
110+
driveModuleTargets.set(state.ModuleTargets);
111+
driveModulePositions.set(state.ModulePositions);
112+
driveTimestamp.set(state.Timestamp);
113+
driveOdometryFrequency.set(state.OdometryPeriod == 0 ? 0 : 1.0 / state.OdometryPeriod);
152114
}
153115
}

‎src/main/java/frc/robot/util/DriveStateSignalLogger.java‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
package frc.robot.util;
2+
3+
import com.ctre.phoenix6.SignalLogger;
4+
import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
5+
6+
public class DriveStateSignalLogger {
7+
/** Construct a telemetry object */
8+
public DriveStateSignalLogger() {
9+
SignalLogger.start();
10+
}
11+
12+
public DriveStateNtLogger DrivebaseSim(double MaxSpeed) {
13+
return new DriveStateNtLogger(this, MaxSpeed);
14+
}
15+
16+
/* What to publish over networktables for telemetry */
17+
private volatile SwerveDriveState cachedDriveState;
18+
19+
private final double[] m_poseArray = new double[3];
20+
private final double[] m_moduleStatesArray = new double[8];
21+
private final double[] m_moduleTargetsArray = new double[8];
22+
23+
/** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
24+
public void telemeterize(SwerveDriveState state) {
25+
cachedDriveState = state;
26+
27+
/* Write to log file (Thread-safe because it happens sequentially in the same thread) */
28+
m_poseArray[0] = state.Pose.getX();
29+
m_poseArray[1] = state.Pose.getY();
30+
m_poseArray[2] = state.Pose.getRotation().getDegrees();
31+
for (int i = 0; i < 4; ++i) {
32+
m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians();
33+
m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond;
34+
m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians();
35+
m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond;
36+
}
37+
38+
SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray);
39+
SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray);
40+
SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray);
41+
SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
42+
}
43+
44+
public SwerveDriveState returnDriveState() {
45+
return cachedDriveState;
46+
}
47+
}

‎src/main/java/frc/robot/util/tuning/LauncherConstants.java‎

Lines changed: 27 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -6,22 +6,25 @@
66
import edu.wpi.first.math.geometry.Translation2d;
77
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
88
import edu.wpi.first.math.kinematics.ChassisSpeeds;
9+
import edu.wpi.first.networktables.DoublePublisher;
10+
import edu.wpi.first.networktables.NetworkTable;
911
import edu.wpi.first.networktables.NetworkTableInstance;
10-
import edu.wpi.first.networktables.StructArrayPublisher;
12+
import edu.wpi.first.networktables.StructPublisher;
13+
import frc.robot.util.AllianceUtils;
1114
import frc.robot.util.robotType.RobotType;
1215

1316
public class LauncherConstants {
14-
private static final Translation2d LAUNCHER_OFFSET =
15-
RobotType.isAlpha() ? new Translation2d(0.2159, -0.1397) : new Translation2d(0.2159, 0.1397);
16-
17-
private static final StructArrayPublisher<Pose2d> turretToTarget =
18-
NetworkTableInstance.getDefault()
19-
.getStructArrayTopic("lines/turretToTarget", Pose2d.struct)
20-
.publish();
21-
private static final StructArrayPublisher<Pose2d> turretRotationalVelocity =
22-
NetworkTableInstance.getDefault()
23-
.getStructArrayTopic("lines/turretRotationalVelocity", Pose2d.struct)
24-
.publish();
17+
private static final Transform2d LAUNCHER_OFFSET =
18+
RobotType.isAlpha()
19+
? new Transform2d(new Translation2d(0.2159, -0.1397), Rotation2d.kZero)
20+
: new Transform2d(new Translation2d(0.2159, 0.1397), Rotation2d.kZero);
21+
22+
private static final NetworkTable table =
23+
NetworkTableInstance.getDefault().getTable("/SmartDashboard/LiveLauncherData");
24+
private static final StructPublisher<Pose2d> turretPose =
25+
table.getStructTopic("Turret Pose", Pose2d.struct).publish();
26+
private static final DoublePublisher turretToHubDistance =
27+
table.getDoubleTopic("Turret to hub distance").publish();
2528

2629
private static double minTime = Double.POSITIVE_INFINITY;
2730
private static double maxTime = Double.NEGATIVE_INFINITY;
@@ -104,8 +107,14 @@ public static double getFlywheelSpeedFromDistance(double distance) {
104107
}
105108

106109
public static Translation2d launcherFromRobot(Pose2d robot) {
107-
Transform2d fieldRelativeLauncherOffset = new Transform2d(LAUNCHER_OFFSET, Rotation2d.kZero);
108-
return robot.plus(fieldRelativeLauncherOffset).getTranslation();
110+
return LAUNCHER_OFFSET.getTranslation();
111+
}
112+
113+
public static void UpdateNT(Pose2d robot) {
114+
Pose2d result = robot.transformBy(LAUNCHER_OFFSET);
115+
turretPose.set(result);
116+
turretToHubDistance.set(
117+
AllianceUtils.getHubTranslation2d().minus(result.getTranslation()).getNorm());
109118
}
110119

111120
public static double getFlywheelSpeedFromPose2d(Translation2d target, Pose2d robot) {
@@ -114,7 +123,7 @@ public static double getFlywheelSpeedFromPose2d(Translation2d target, Pose2d rob
114123
}
115124

116125
public static Transform2d turretTransform() {
117-
return new Transform2d(LAUNCHER_OFFSET, Rotation2d.kZero);
126+
return LAUNCHER_OFFSET;
118127
}
119128

120129
public static double getHoodAngleFromDistance(double distance) {
@@ -148,8 +157,9 @@ public static double maxTimeOfFlight() {
148157
// radius is launcher offset from center of robot
149158
// then converts angular speed into tangent velocity
150159
public static Translation2d angularVelocity(Pose2d robot, ChassisSpeeds fieldSpeeds) {
151-
Translation2d angle = LAUNCHER_OFFSET.rotateBy(robot.getRotation());
152-
double angleVelocitySpeed = (fieldSpeeds.omegaRadiansPerSecond * LAUNCHER_OFFSET.getNorm());
160+
Translation2d angle = LAUNCHER_OFFSET.getTranslation().rotateBy(robot.getRotation());
161+
double angleVelocitySpeed =
162+
(fieldSpeeds.omegaRadiansPerSecond * LAUNCHER_OFFSET.getTranslation().getNorm());
153163
double vx = -angle.getY() * angleVelocitySpeed;
154164
double vy = angle.getX() * angleVelocitySpeed;
155165
return new Translation2d(vx, vy);

0 commit comments

Comments
 (0)