FRCTeam3255 / SuperCORE

SuperNURD Base classes for FRC Robots
https://frcteam3255.github.io/SuperCORE/
0 stars 0 forks source link

Add AdvantageScopeUtil #84

Closed ACat701 closed 9 months ago

ACat701 commented 9 months ago
// 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);
  }
}
ACat701 commented 9 months ago

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)