// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
public class AdvantageScopeUtil {
public AdvantageScopeUtil() {
}
/**
* <p>
* Converts Pose3ds to double[]s in the order that is needed for AdvantageScope
* logging.
* </p>
* Copied directly from 4738 🦒 who copied from 6328 ðŸ¤
*
* @param value The Pose3ds to convert
* @return The given Pose3ds as double[]s
*/
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;
}
/**
* Starts a WPI data log. All SmartDashboard values will be in the log. In
* simulation, log files will be saved at the given file path. Otherwise, they
* are saved to the RIO.
*
* @param filePath The filepath to save log files to in simulation. For example,
* "src/main"
*/
public static void startDataLogging(String filePath) {
// Set out log file to be in its own folder
if (Robot.isSimulation()) {
DataLogManager.start(filePath);
} else {
DataLogManager.start();
}
// Log data that is being put to shuffleboard
DataLogManager.logNetworkTables(true);
// Log the DS data and joysticks
DriverStation.startDataLog(DataLogManager.getLog(), true);
}
}
Struct support was added in AdvantageScope beta-3 and will be added to WPILib 2024, so this is no longer needed (unless we only want to add startDataLogging(), but that doesn't seem worth it)