[sysid] Add SysId (#5672)

The source is copied from this commit:
625ff04784.

sysid/src/main/native/cpp/analysis/JSONConverter.cpp

@@ -0,0 +1,165 @@
+// 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.
+
+#include "sysid/analysis/JSONConverter.h"
+
+#include <stdexcept>
+#include <string>
+
+#include <fmt/core.h>
+#include <fmt/format.h>
+#include <wpi/Logger.h>
+#include <wpi/fmt/raw_ostream.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+#include "sysid/Util.h"
+#include "sysid/analysis/AnalysisManager.h"
+#include "sysid/analysis/AnalysisType.h"
+
+// Sizes of the arrays for new sysid data.
+static constexpr size_t kDrivetrainSize = 9;
+static constexpr size_t kGeneralSize = 4;
+
+// Indices for the old data.
+static constexpr size_t kTimestampCol = 0;
+static constexpr size_t kLVoltsCol = 3;
+static constexpr size_t kRVoltsCol = 4;
+static constexpr size_t kLPosCol = 5;
+static constexpr size_t kRPosCol = 6;
+static constexpr size_t kLVelCol = 7;
+static constexpr size_t kRVelCol = 8;
+
+static wpi::json GetJSON(std::string_view path, wpi::Logger& logger) {
+  std::error_code ec;
+  wpi::raw_fd_istream input{path, ec};
+
+  if (ec) {
+    throw std::runtime_error(fmt::format("Unable to read: {}", path));
+  }
+
+  wpi::json json;
+  input >> json;
+  WPI_INFO(logger, "Read frc-characterization JSON from {}", path);
+  return json;
+}
+
+std::string sysid::ConvertJSON(std::string_view path, wpi::Logger& logger) {
+  wpi::json ojson = GetJSON(path, logger);
+
+  auto type = sysid::analysis::FromName(ojson.at("test").get<std::string>());
+  auto factor = ojson.at("unitsPerRotation").get<double>();
+  auto unit = ojson.at("units").get<std::string>();
+
+  wpi::json json;
+  for (auto&& key : AnalysisManager::kJsonDataKeys) {
+    if (type == analysis::kDrivetrain) {
+      // Get the old data; create a vector for the new data; reserve the
+      // appropriate size for the new data.
+      auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
+      std::vector<std::array<double, kDrivetrainSize>> data;
+      data.reserve(odata.size());
+
+      // Transfer the data.
+      for (auto&& pt : odata) {
+        data.push_back(std::array<double, kDrivetrainSize>{
+            pt[kTimestampCol], pt[kLVoltsCol], pt[kRVoltsCol], pt[kLPosCol],
+            pt[kRPosCol], pt[kLVelCol], pt[kRVelCol], 0.0, 0.0});
+      }
+      json[key] = data;
+    } else {
+      // Get the old data; create a vector for the new data; reserve the
+      // appropriate size for the new data.
+      auto odata = ojson.at(key).get<std::vector<std::array<double, 10>>>();
+      std::vector<std::array<double, kGeneralSize>> data;
+      data.reserve(odata.size());
+
+      // Transfer the data.
+      for (auto&& pt : odata) {
+        data.push_back(std::array<double, kGeneralSize>{
+            pt[kTimestampCol], pt[kLVoltsCol], pt[kLPosCol], pt[kLVelCol]});
+      }
+      json[key] = data;
+    }
+  }
+  json["units"] = unit;
+  json["unitsPerRotation"] = factor;
+  json["test"] = type.name;
+  json["sysid"] = true;
+
+  // Write the new file with "_new" appended to it.
+  path.remove_suffix(std::string_view{".json"}.size());
+  std::string loc = fmt::format("{}_new.json", path);
+
+  sysid::SaveFile(json.dump(2), std::filesystem::path{loc});
+
+  WPI_INFO(logger, "Wrote new JSON to: {}", loc);
+  return loc;
+}
+
+std::string sysid::ToCSV(std::string_view path, wpi::Logger& logger) {
+  wpi::json json = GetJSON(path, logger);
+
+  auto type = sysid::analysis::FromName(json.at("test").get<std::string>());
+  auto factor = json.at("unitsPerRotation").get<double>();
+  auto unit = json.at("units").get<std::string>();
+  std::string_view abbreviation = GetAbbreviation(unit);
+
+  std::error_code ec;
+  // Naming: {sysid-json-name}(Test, Units).csv
+  path.remove_suffix(std::string_view{".json"}.size());
+  std::string loc = fmt::format("{} ({}, {}).csv", path, type.name, unit);
+  wpi::raw_fd_ostream outputFile{loc, ec};
+
+  if (ec) {
+    throw std::runtime_error("Unable to write to: " + loc);
+  }
+
+  fmt::print(outputFile, "Timestamp (s),Test,");
+  if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
+    fmt::print(
+        outputFile,
+        "Left Volts (V),Right Volts (V),Left Position ({0}),Right "
+        "Position ({0}),Left Velocity ({0}/s),Right Velocity ({0}/s),Gyro "
+        "Position (deg),Gyro Rate (deg/s)\n",
+        abbreviation);
+  } else {
+    fmt::print(outputFile, "Volts (V),Position({0}),Velocity ({0}/s)\n",
+               abbreviation);
+  }
+  outputFile << "\n";
+
+  for (auto&& key : AnalysisManager::kJsonDataKeys) {
+    if (type == analysis::kDrivetrain || type == analysis::kDrivetrainAngular) {
+      auto tempData =
+          json.at(key).get<std::vector<std::array<double, kDrivetrainSize>>>();
+      for (auto&& pt : tempData) {
+        fmt::print(outputFile, "{},{},{},{},{},{},{},{},{},{}\n",
+                   pt[0],                           // Timestamp
+                   key,                             // Test
+                   pt[1], pt[2],                    // Left and Right Voltages
+                   pt[3] * factor, pt[4] * factor,  // Left and Right Positions
+                   pt[5] * factor, pt[6] * factor,  // Left and Right Velocity
+                   pt[7], pt[8]  // Gyro Position and Velocity
+        );
+      }
+    } else {
+      auto tempData =
+          json.at(key).get<std::vector<std::array<double, kGeneralSize>>>();
+      for (auto&& pt : tempData) {
+        fmt::print(outputFile, "{},{},{},{},{}\n",
+                   pt[0],           // Timestamp,
+                   key,             // Test
+                   pt[1],           // Voltage
+                   pt[2] * factor,  // Position
+                   pt[3] * factor   // Velocity
+        );
+      }
+    }
+  }
+  outputFile.flush();
+  WPI_INFO(logger, "Wrote CSV to: {}", loc);
+  return loc;
+}