diff --git a/CMakeLists.txt b/CMakeLists.txt index 47e23668005..e617a756a55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -293,6 +293,7 @@ if (WITH_GUI) add_subdirectory(wpigui) add_subdirectory(glass) add_subdirectory(outlineviewer) + add_subdirectory(sysid) if (LIBSSH_FOUND) add_subdirectory(roborioteamnumbersetter) add_subdirectory(datalogtool) diff --git a/settings.gradle b/settings.gradle index 38617022ae8..3e41f162093 100644 --- a/settings.gradle +++ b/settings.gradle @@ -40,6 +40,7 @@ include 'glass' include 'outlineviewer' include 'roborioteamnumbersetter' include 'datalogtool' +include 'sysid' include 'simulation:halsim_ds_socket' include 'simulation:halsim_gui' include 'simulation:halsim_ws_core' diff --git a/sysid/.styleguide b/sysid/.styleguide new file mode 100644 index 00000000000..8506bf193ea --- /dev/null +++ b/sysid/.styleguide @@ -0,0 +1,35 @@ +cppHeaderFileInclude { + \.h$ + \.inc$ + \.inl$ +} + +cppSrcFileInclude { + \.cpp$ +} + +generatedFileExclude { + src/main/native/resources/ + src/main/native/win/sysid.ico + src/main/native/mac/sysid.icns +} + +repoRootNameOverride { + sysid +} + +includeOtherLibs { + ^GLFW + ^fmt/ + ^frc/ + ^glass/ + ^gtest/ + ^imgui + ^implot\.h$ + ^networktables/ + ^portable-file-dialogs\.h$ + ^ntcore + ^units/ + ^wpi/ + ^wpigui +} diff --git a/sysid/CMakeLists.txt b/sysid/CMakeLists.txt new file mode 100644 index 00000000000..31fbe08d0da --- /dev/null +++ b/sysid/CMakeLists.txt @@ -0,0 +1,41 @@ +project(sysid) + +include(CompileWarnings) +include(GenResources) +include(LinkMacOSGUI) +include(AddTest) + +configure_file(src/main/generate/WPILibVersion.cpp.in WPILibVersion.cpp) +generate_resources(src/main/native/resources generated/main/cpp SYSID sysid sysid_resources_src) + +file(GLOB_RECURSE sysid_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp) + +if (WIN32) + set(sysid_rc src/main/native/win/sysid.rc) +elseif(APPLE) + set(MACOSX_BUNDLE_ICON_FILE sysid.icns) + set(APP_ICON_MACOSX src/main/native/mac/sysid.icns) + set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources") +endif() + +add_executable(sysid ${sysid_src} ${sysid_resources_src} ${sysid_rc} ${APP_ICON_MACOSX}) +wpilib_link_macos_gui(sysid) +wpilib_target_warnings(sysid) +target_include_directories(sysid PRIVATE src/main/native/include) +target_link_libraries(sysid wpimath libglassnt libglass) + +if (WIN32) + set_target_properties(sysid PROPERTIES WIN32_EXECUTABLE YES) +elseif(APPLE) + set_target_properties(sysid PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "SysId") +endif() + +if (WITH_TESTS) + wpilib_add_test(sysid src/test/native/cpp) + wpilib_link_macos_gui(sysid_test) + target_sources(sysid_test PRIVATE ${sysid_src}) + target_compile_definitions(sysid_test PRIVATE RUNNING_SYSID_TESTS) + target_include_directories(sysid_test PRIVATE src/main/native/cpp + src/main/native/include) + target_link_libraries(sysid_test wpimath libglassnt libglass gtest) +endif() diff --git a/sysid/Info.plist b/sysid/Info.plist new file mode 100644 index 00000000000..37bb798b4f0 --- /dev/null +++ b/sysid/Info.plist @@ -0,0 +1,32 @@ + + + + + CFBundleName + SysId + CFBundleExecutable + sysid + CFBundleDisplayName + SysId + CFBundleIdentifier + edu.wpi.first.tools.SysId + CFBundleIconFile + sysid.icns + CFBundlePackageType + APPL + CFBundleSupportedPlatforms + + MacOSX + + CFBundleInfoDictionaryVersion + 6.0 + CFBundleShortVersionString + 2021 + CFBundleVersion + 2021 + LSMinimumSystemVersion + 10.14 + NSHighResolutionCapable + + + diff --git a/sysid/build.gradle b/sysid/build.gradle new file mode 100644 index 00000000000..e6709fc8285 --- /dev/null +++ b/sysid/build.gradle @@ -0,0 +1,172 @@ +import org.gradle.internal.os.OperatingSystem + +if (project.hasProperty('onlylinuxathena')) { + return; +} + +description = 'System identification for robot mechanisms' + +apply plugin: 'cpp' +apply plugin: 'google-test-test-suite' +apply plugin: 'visual-studio' +apply plugin: 'edu.wpi.first.NativeUtils' + +if (OperatingSystem.current().isWindows()) { + apply plugin: 'windows-resources' +} + +ext { + nativeName = 'sysid' +} + +apply from: "${rootDir}/shared/resources.gradle" +apply from: "${rootDir}/shared/config.gradle" + +def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in") +def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp") + +apply from: "${rootDir}/shared/imgui.gradle" + +task generateCppVersion() { + description = 'Generates the wpilib version class' + group = 'WPILib' + + outputs.file wpilibVersionFileOutput + inputs.file wpilibVersionFileInput + + if (wpilibVersioning.releaseMode) { + outputs.upToDateWhen { false } + } + + // We follow a simple set of checks to determine whether we should generate a new version file: + // 1. If the release type is not development, we generate a new version file + // 2. If there is no generated version number, we generate a new version file + // 3. If there is a generated build number, and the release type is development, then we will + // only generate if the publish task is run. + doLast { + def version = wpilibVersioning.version.get() + println "Writing version ${version} to $wpilibVersionFileOutput" + + if (wpilibVersionFileOutput.exists()) { + wpilibVersionFileOutput.delete() + } + def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version) + wpilibVersionFileOutput.write(read) + } +} + +gradle.taskGraph.addTaskExecutionGraphListener { graph -> + def willPublish = graph.hasTask(publish) + if (willPublish) { + generateCppVersion.outputs.upToDateWhen { false } + } +} + +def generateTask = createGenerateResourcesTask('main', 'SYSID', 'sysid', project) + +project(':').libraryBuild.dependsOn build +tasks.withType(CppCompile) { + dependsOn generateTask + dependsOn generateCppVersion +} + +model { + components { + // By default, a development executable will be generated. This is to help the case of + // testing specific functionality of the library. + "${nativeName}"(NativeExecutableSpec) { + baseName = 'sysid' + sources { + cpp { + source { + srcDirs 'src/main/native/cpp', "$buildDir/generated/main/cpp" + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/include' + } + } + if (OperatingSystem.current().isWindows()) { + rc.source { + srcDirs 'src/main/native/win' + include '*.rc' + } + } + } + binaries.all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + it.buildable = false + return + } + lib project: ':glass', library: 'glassnt', linkage: 'static' + lib project: ':glass', library: 'glass', linkage: 'static' + project(':ntcore').addNtcoreDependency(it, 'static') + lib project: ':wpinet', library: 'wpinet', linkage: 'static' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' + lib project: ':wpimath', library: 'wpimath', linkage: 'static' + lib project: ':wpigui', library: 'wpigui', linkage: 'static' + nativeUtils.useRequiredLibrary(it, 'imgui') + if (it.targetPlatform.operatingSystem.isWindows()) { + it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' + it.linker.args << '/DELAYLOAD:MF.dll' << '/DELAYLOAD:MFReadWrite.dll' << '/DELAYLOAD:MFPlat.dll' << '/delay:nobind' + } else if (it.targetPlatform.operatingSystem.isMacOsX()) { + it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' + if (it.buildType.getName() == "release") { + it.linker.args << '-s' + } + } else { + it.linker.args << '-lX11' + if (it.targetPlatform.name.startsWith('linuxarm')) { + it.linker.args << '-lGL' + } + } + } + } + } + testSuites { + "${nativeName}Test"(GoogleTestTestSuiteSpec) { + for (NativeComponentSpec c : $.components) { + if (c.name == nativeName) { + testing c + break + } + } + sources.cpp.source { + srcDirs "src/test/native/cpp" + include "**/*.cpp" + } + binaries.all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + it.buildable = false + return + } + lib project: ':glass', library: 'glassnt', linkage: 'static' + lib project: ':glass', library: 'glass', linkage: 'static' + project(':ntcore').addNtcoreDependency(it, 'static') + lib project: ':wpinet', library: 'wpinet', linkage: 'static' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' + lib project: ':wpimath', library: 'wpimath', linkage: 'static' + lib project: ':wpigui', library: 'wpigui', linkage: 'static' + nativeUtils.useRequiredLibrary(it, 'imgui') + if (it.targetPlatform.operatingSystem.isWindows()) { + it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' + it.linker.args << '/DELAYLOAD:MF.dll' << '/DELAYLOAD:MFReadWrite.dll' << '/DELAYLOAD:MFPlat.dll' << '/delay:nobind' + } else if (it.targetPlatform.operatingSystem.isMacOsX()) { + it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' + if (it.buildType.getName() == "release") { + it.linker.args << '-s' + } + } else { + it.linker.args << '-lX11' + if (it.targetPlatform.name.startsWith('linuxarm')) { + it.linker.args << '-lGL' + } + } + nativeUtils.useRequiredLibrary(it, "googletest_static") + it.cppCompiler.define("RUNNING_SYSID_TESTS") + } + } + } +} + +apply from: 'publish.gradle' diff --git a/sysid/docs/arm-ols-with-angle-offset.md b/sysid/docs/arm-ols-with-angle-offset.md new file mode 100644 index 00000000000..ecb5c439820 --- /dev/null +++ b/sysid/docs/arm-ols-with-angle-offset.md @@ -0,0 +1,66 @@ +# Arm OLS with angle offset + +If the arm encoder doesn't read zero degrees when the arm is horizontal, the fit +for `Kg` will be wrong. An angle offset should be added to the model like so. +``` +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle + offset) +``` +Use a trig identity to split the cosine into two terms. +``` +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka (cos(angle) cos(offset) - sin(angle) sin(offset)) +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(angle) cos(offset) + Kg/Ka sin(angle) sin(offset) +``` +Reorder multiplicands so the offset trig is absorbed by the OLS terms. +``` +dx/dt = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(offset) cos(angle) + Kg/Ka sin(offset) sin(angle) +``` + +## OLS + +Let `α = -Kv/Ka`, `β = 1/Ka`, `γ = -Ks/Ka`, `δ = -Kg/Ka cos(offset)`, and `ε = Kg/Ka sin(offset)`. +``` +dx/dt = αx + βu + γ sgn(x) + δ cos(angle) + ε sin(angle) +``` + +### Ks, Kv, Ka + +Divide the OLS terms by each other to obtain `Ks`, `Kv`, and `Ka`. +``` +Ks = -γ/β +Kv = -α/β +Ka = 1/β +``` + +### Kg + +Take the sum of squares of the OLS terms containing the angle offset. The angle +offset trig functions will form a trig identity that cancels out. Then, just +solve for `Kg`. +``` +δ²+ε² = (-Kg/Ka cos(offset))² + (Kg/Ka sin(offset))² +δ²+ε² = (-Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset) +δ²+ε² = (Kg/Ka)² cos²(offset) + (Kg/Ka)² sin²(offset) +δ²+ε² = (Kg/Ka)² (cos²(offset) + sin²(offset)) +δ²+ε² = (Kg/Ka)² (1) +δ²+ε² = (Kg/Ka)² +√(δ²+ε²) = Kg/Ka +√(δ²+ε²) = Kg β +Kg = √(δ²+ε²)/β +``` + +As a sanity check, when the offset is zero, ε is zero and the equation for +`Kg` simplifies to -δ/β, the equation previously used by SysId. + +### Angle offset + +Divide ε by δ, combine the trig functions into `tan(offset)`, then use `atan2()` +to preserve the angle quadrant. Maintaining the proper negative signs in the +numerator and denominator are important for obtaining the correct result. +``` +δ = -Kg/Ka cos(offset) +ε = Kg/Ka sin(offset) +sin(offset)/-cos(offset) = ε/δ +sin(offset)/cos(offset) = ε/-δ +tan(offset) = ε/-δ +offset = atan2(ε, -δ) +``` diff --git a/sysid/docs/data-collection.md b/sysid/docs/data-collection.md new file mode 100644 index 00000000000..9d28a151a0d --- /dev/null +++ b/sysid/docs/data-collection.md @@ -0,0 +1,210 @@ +# Data Collection + +This document details how data must be sent over NetworkTables for accurate data collection. Note that the data format has changed from what the old [frc-characterization](https://github.com/wpilibsuite/frc-characterization) tool used to generate. + +## NetworkTables Data Entries + +Here is a list of the NT entries that are used to send and collect data between sysid and the robot program: + +| NT Entry | Type | Description | +| --------------------------------------| -------- | -------------------------------------------------------------------------------------------------------------------------------------------------- | +| `/SmartDashboard/SysIdTelemetry` | `string` | Used to send telemetry from the robot program. This data is sent after the test completes once the robot enters the disabled state. | +| `/SmartDashboard/SysIdVoltageCommand` | `double` | Used to either send the ramp rate (V/s) for the quasistatic test or the voltage (V) for the dynamic test. | +| `/SmartDashboard/SysIdTestType` | `string` | Used to send the test type ("Quasistatic" or "Dynamic") which helps determine how the `VoltageCommand` entry will be used. | +| `/SmartDashboard/SysIdRotate` | `bool` | Used to receive the rotation bool from the Logger. If this is set to true, the drivetrain will rotate. It is only applicable for drivetrain tests. | + +## Telemetry Format + +There are two formats used to send telemetry from the robot program. One format is for non-drivetrain mechanisms, whereas the other is for all drivetrain tests (linear and angular). + +### Non-Drivetrain Mechanisms + +`timestamp, voltage, position, velocity` + +Example JSON: + +```json +{ +"fast-backward": [ +[ +timestamp 1, +voltage 1, +position 1, +velocity 1 +], +[ +timestamp 2, +voltage 2, +position 2, +velocity 2 +] +], +"fast-forward": [ +[ +timestamp 1, +voltage 1, +position 1, +velocity 1 +], +[ +timestamp 2, +voltage 2, +position 2, +velocity 2 +] +], +"slow-backward": [ +[ +timestamp 1, +voltage 1, +position 1, +velocity 1 +], +[ +timestamp 2, +voltage 2, +position 2, +velocity 2 +] +], +"slow-forward": [ +[ +timestamp 1, +voltage 1, +position 1, +velocity 1 +], +[ +timestamp 2, +voltage 2, +position 2, +velocity 2 +] +], +"sysid": true, +"test": "Simple", +"units": "Rotations", +"unitsPerRotation": 1.0 +} +``` + +Supported test types for the "test" field in this data format include "Arm", +"Elevator", and "Simple". Supported unit types include "Meters", "Feet", +"Inches", "Radians", "Rotations", and "Degrees". + +### Drivetrain + +`timestamp, l voltage, r voltage, l position, r position, l velocity, r velocity, angle, angular rate` + +Note that all positions and velocities should be in rotations of the output and rotations/sec of the output respectively. If there is a gearing between the encoder and the output, that should be taken into account. + +Example JSON: + +```json +{ +"fast-backward": [ +[ +timestamp 1, +l voltage 1, +r voltage 1, +l position 1, +r position 1, +l velocity 1, +r velocity 1, +angle 1, +angular rate 1 +], +[ +timestamp 2, +l voltage 2, +r voltage 2, +l position 2, +r position 2, +l velocity 2, +r velocity 2, +angle 2, +angular rate 2 +] +], +"fast-forward": [ +[ +timestamp 1, +l voltage 1, +r voltage 1, +l position 1, +r position 1, +l velocity 1, +r velocity 1, +angle 1, +angular rate 1 +], +[ +timestamp 2, +l voltage 2, +r voltage 2, +l position 2, +r position 2, +l velocity 2, +r velocity 2, +angle 2, +angular rate 2 +] +], +"slow-backward": [ +[ +timestamp 1, +l voltage 1, +r voltage 1, +l position 1, +r position 1, +l velocity 1, +r velocity 1, +angle 1, +angular rate 1 +], +[ +timestamp 2, +l voltage 2, +r voltage 2, +l position 2, +r position 2, +l velocity 2, +r velocity 2, +angle 2, +angular rate 2 +] +], +"slow-forward": [ +[ +timestamp 1, +l voltage 1, +r voltage 1, +l position 1, +r position 1, +l velocity 1, +r velocity 1, +angle 1, +angular rate 1 +], +[ +timestamp 2, +l voltage 2, +r voltage 2, +l position 2, +r position 2, +l velocity 2, +r velocity 2, +angle 2, +angular rate 2 +] +], +"sysid": true, +"test": "Drivetrain", +"units": "Rotations", +"unitsPerRotation": 1.0 +} +``` + +Supported test types for the "test" field in this data format include +"Drivetrain" and "Drivetrain (Angular)". Supported unit types include "Meters", +"Feet", "Inches", "Radians", "Rotations", and "Degrees". diff --git a/sysid/publish.gradle b/sysid/publish.gradle new file mode 100644 index 00000000000..13569464413 --- /dev/null +++ b/sysid/publish.gradle @@ -0,0 +1,125 @@ +apply plugin: 'maven-publish' + +def baseArtifactId = 'SysId' +def artifactGroupId = 'edu.wpi.first.tools' +def zipBaseName = '_GROUP_edu_wpi_first_tools_ID_SysId_CLS' + +def outputsFolder = file("$project.buildDir/outputs") + +model { + tasks { + // Create the run task. + $.components.sysid.binaries.each { bin -> + if (bin.buildable && bin.name.toLowerCase().contains("debug") && nativeUtils.isNativeDesktopPlatform(bin.targetPlatform)) { + Task run = project.tasks.create("run", Exec) { + commandLine bin.tasks.install.runScriptFile.get().asFile.toString() + } + run.dependsOn bin.tasks.install + } + } + } + publishing { + def sysIdTaskList = [] + $.components.each { component -> + component.binaries.each { binary -> + if (binary in NativeExecutableBinarySpec && binary.component.name.contains("sysid")) { + if (binary.buildable && (binary.name.contains('Release') || binary.name.contains('release'))) { + // We are now in the binary that we want. + // This is the default application path for the ZIP task. + def applicationPath = binary.executable.file + def icon = file("$project.projectDir/src/main/native/mac/ov.icns") + + // Create the macOS bundle. + def bundleTask = project.tasks.create("bundleSysIdOsxApp" + binary.targetPlatform.architecture.name, Copy) { + description("Creates a macOS application bundle for SysId") + from(file("$project.projectDir/Info.plist")) + into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/SysId.app/Contents")) + into("MacOS") { + with copySpec { + from binary.executable.file + } + } + into("Resources") { + with copySpec { + from icon + } + } + + inputs.property "HasDeveloperId", project.hasProperty("developerID") + + doLast { + if (project.hasProperty("developerID")) { + // Get path to binary. + exec { + workingDir rootDir + def args = [ + "sh", + "-c", + "codesign --force --strict --deep " + + "--timestamp --options=runtime " + + "--verbose -s ${project.findProperty("developerID")} " + + "$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/SysId.app/" + ] + commandLine args + } + } + } + } + + // Reset the application path if we are creating a bundle. + if (binary.targetPlatform.operatingSystem.isMacOsX()) { + applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name") + project.build.dependsOn bundleTask + } + + // Create the ZIP. + def task = project.tasks.create("copySysIdExecutable" + binary.targetPlatform.architecture.name, Zip) { + description("Copies the SysId executable to the outputs directory.") + destinationDirectory = outputsFolder + + archiveBaseName = '_M_' + zipBaseName + duplicatesStrategy = 'exclude' + archiveClassifier = nativeUtils.getPublishClassifier(binary) + + from(licenseFile) { + into '/' + } + + from(applicationPath) + + if (binary.targetPlatform.operatingSystem.isWindows()) { + def exePath = binary.executable.file.absolutePath + exePath = exePath.substring(0, exePath.length() - 4) + def pdbPath = new File(exePath + '.pdb') + from(pdbPath) + } + + into(nativeUtils.getPlatformPath(binary)) + } + + if (binary.targetPlatform.operatingSystem.isMacOsX()) { + bundleTask.dependsOn binary.tasks.link + task.dependsOn(bundleTask) + } + + task.dependsOn binary.tasks.link + sysIdTaskList.add(task) + project.build.dependsOn task + project.artifacts { task } + addTaskToCopyAllOutputs(task) + } + } + } + } + + publications { + sysId(MavenPublication) { + sysIdTaskList.each { artifact it } + + artifactId = baseArtifactId + groupId = artifactGroupId + version wpilibVersioning.version.get() + } + } + } +} diff --git a/sysid/scripts/time_plots.py b/sysid/scripts/time_plots.py new file mode 100755 index 00000000000..6878126af2c --- /dev/null +++ b/sysid/scripts/time_plots.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 + +import json +import pathlib + +import matplotlib.pyplot as plt +import pandas as pd +import sys + +# Load data +filename = pathlib.Path(sys.argv[1]) + +UNIT_TO_ABBREVIATION = { + "Meters": "m", + "Feet": "ft", + "Inches": "in", + "Degrees": "deg", + "Rotations": "rot", + "Radians": "rad", +} + +# Make DataFrame to facilitate plotting +if filename.suffix == ".json": + raw_data = json.loads(filename.read_text()) + unit = raw_data["units"] + + # Get Unit + try: + abbreviation = UNIT_TO_ABBREVIATION[unit] + except KeyError: + raise ValueError("Invalid Unit") + + # Make Columns + columns = ["Timestamp (s)", "Test"] + if "Drive" in raw_data["test"]: + columns.extend( + [ + "Left Volts (V)", + "Right Volts (V)", + f"Left Position ({abbreviation})", + f"Right Position ({abbreviation})", + f"Left Velocity ({abbreviation}/s)", + f"Right Velocity ({abbreviation}/s)", + "Gyro Position (deg)", + "Gyro Rate (deg/s)", + ] + ) + unit_columns = columns[4:8] + else: + columns.extend( + ["Volts (V)", f"Position ({abbreviation})", f"Velocity ({abbreviation}/s)"] + ) + unit_columns = columns[3:] + + prepared_data = pd.DataFrame(columns=columns) + for test in raw_data.keys(): + if "-" not in test: + continue + formatted_entry = [[pt[0], test, *pt[1:]] for pt in raw_data[test]] + prepared_data = pd.concat( + [prepared_data, pd.DataFrame(formatted_entry, columns=columns)] + ) + + units_per_rot = raw_data["unitsPerRotation"] + + for column in unit_columns: + prepared_data[column] *= units_per_rot +else: + prepared_data = pd.read_csv(filename) + +# First 2 columns are Timestamp and Test +for column in prepared_data.columns[2:]: + # Configure Plot Labels + plt.figure() + plt.xlabel("Timestamp (s)") + plt.ylabel(column) + + # Configure title without units + print(column) + end = column.find("(") + plt.title(f"{column[:end].strip()} vs Time") + + # Plot data for each test + for test in pd.unique(prepared_data["Test"]): + test_data = prepared_data[prepared_data["Test"] == test] + plt.plot(test_data["Timestamp (s)"], test_data[column], label=test) + plt.legend() + +plt.show() diff --git a/sysid/src/main/generate/WPILibVersion.cpp.in b/sysid/src/main/generate/WPILibVersion.cpp.in new file mode 100644 index 00000000000..b0a44905207 --- /dev/null +++ b/sysid/src/main/generate/WPILibVersion.cpp.in @@ -0,0 +1,7 @@ +/* + * Autogenerated file! Do not manually edit this file. This version is regenerated + * any time the publish task is run, or when this file is deleted. + */ +const char* GetWPILibVersion() { + return "${wpilib_version}"; +} diff --git a/sysid/src/main/native/cpp/App.cpp b/sysid/src/main/native/cpp/App.cpp new file mode 100644 index 00000000000..947ea434a65 --- /dev/null +++ b/sysid/src/main/native/cpp/App.cpp @@ -0,0 +1,219 @@ +// 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 + +#ifndef RUNNING_SYSID_TESTS + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "sysid/view/Analyzer.h" +#include "sysid/view/JSONConverter.h" +#include "sysid/view/Logger.h" +#include "sysid/view/UILayout.h" + +namespace gui = wpi::gui; + +static std::unique_ptr gWindowManager; + +glass::Window* gLoggerWindow; +glass::Window* gAnalyzerWindow; +glass::Window* gProgramLogWindow; +static glass::MainMenuBar gMainMenu; + +std::unique_ptr gJSONConverter; + +glass::LogData gLog; +wpi::Logger gLogger; + +const char* GetWPILibVersion(); + +namespace sysid { +std::string_view GetResource_sysid_16_png(); +std::string_view GetResource_sysid_32_png(); +std::string_view GetResource_sysid_48_png(); +std::string_view GetResource_sysid_64_png(); +std::string_view GetResource_sysid_128_png(); +std::string_view GetResource_sysid_256_png(); +std::string_view GetResource_sysid_512_png(); +} // namespace sysid + +void Application(std::string_view saveDir) { + // Create the wpigui (along with Dear ImGui) and Glass contexts. + gui::CreateContext(); + glass::CreateContext(); + + // Add icons + gui::AddIcon(sysid::GetResource_sysid_16_png()); + gui::AddIcon(sysid::GetResource_sysid_32_png()); + gui::AddIcon(sysid::GetResource_sysid_48_png()); + gui::AddIcon(sysid::GetResource_sysid_64_png()); + gui::AddIcon(sysid::GetResource_sysid_128_png()); + gui::AddIcon(sysid::GetResource_sysid_256_png()); + gui::AddIcon(sysid::GetResource_sysid_512_png()); + + glass::SetStorageName("sysid"); + glass::SetStorageDir(saveDir.empty() ? gui::GetPlatformSaveFileDir() + : saveDir); + + // Add messages from the global sysid logger into the Log window. + gLogger.SetLogger([](unsigned int level, const char* file, unsigned int line, + const char* msg) { + const char* lvl = ""; + if (level >= wpi::WPI_LOG_CRITICAL) { + lvl = "CRITICAL: "; + } else if (level >= wpi::WPI_LOG_ERROR) { + lvl = "ERROR: "; + } else if (level >= wpi::WPI_LOG_WARNING) { + lvl = "WARNING: "; + } else if (level >= wpi::WPI_LOG_INFO) { + lvl = "INFO: "; + } else if (level >= wpi::WPI_LOG_DEBUG) { + lvl = "DEBUG: "; + } + std::string filename = std::filesystem::path{file}.filename().string(); + gLog.Append(fmt::format("{}{} ({}:{})\n", lvl, msg, filename, line)); +#ifndef NDEBUG + fmt::print(stderr, "{}{} ({}:{})\n", lvl, msg, filename, line); +#endif + }); + + gLogger.set_min_level(wpi::WPI_LOG_DEBUG); + // Set the number of workers for the libuv threadpool. + uv_os_setenv("UV_THREADPOOL_SIZE", "6"); + + // Initialize window manager and add views. + auto& storage = glass::GetStorageRoot().GetChild("SysId"); + gWindowManager = std::make_unique(storage); + gWindowManager->GlobalInit(); + + gLoggerWindow = gWindowManager->AddWindow( + "Logger", std::make_unique(storage, gLogger)); + + gAnalyzerWindow = gWindowManager->AddWindow( + "Analyzer", std::make_unique(storage, gLogger)); + + gProgramLogWindow = gWindowManager->AddWindow( + "Program Log", std::make_unique(&gLog)); + + // Set default positions and sizes for windows. + + // Logger window position/size + gLoggerWindow->SetDefaultPos(sysid::kLoggerWindowPos.x, + sysid::kLoggerWindowPos.y); + gLoggerWindow->SetDefaultSize(sysid::kLoggerWindowSize.x, + sysid::kLoggerWindowSize.y); + + // Analyzer window position/size + gAnalyzerWindow->SetDefaultPos(sysid::kAnalyzerWindowPos.x, + sysid::kAnalyzerWindowPos.y); + gAnalyzerWindow->SetDefaultSize(sysid::kAnalyzerWindowSize.x, + sysid::kAnalyzerWindowSize.y); + + // Program log window position/size + gProgramLogWindow->SetDefaultPos(sysid::kProgramLogWindowPos.x, + sysid::kProgramLogWindowPos.y); + gProgramLogWindow->SetDefaultSize(sysid::kProgramLogWindowSize.x, + sysid::kProgramLogWindowSize.y); + gProgramLogWindow->DisableRenamePopup(); + + gJSONConverter = std::make_unique(gLogger); + + // Configure save file. + gui::ConfigurePlatformSaveFile("sysid.ini"); + + // Add menu bar. + gui::AddLateExecute([] { + ImGui::BeginMainMenuBar(); + gMainMenu.WorkspaceMenu(); + gui::EmitViewMenu(); + + if (ImGui::BeginMenu("Widgets")) { + gWindowManager->DisplayMenu(); + ImGui::EndMenu(); + } + + bool about = false; + if (ImGui::BeginMenu("Info")) { + if (ImGui::MenuItem("About")) { + about = true; + } + ImGui::EndMenu(); + } + + bool toCSV = false; + if (ImGui::BeginMenu("JSON Converters")) { + if (ImGui::MenuItem("JSON to CSV Converter")) { + toCSV = true; + } + + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Docs")) { + if (ImGui::MenuItem("Online documentation")) { + wpi::gui::OpenURL( + "https://docs.wpilib.org/en/stable/docs/software/pathplanning/" + "system-identification/"); + } + + ImGui::EndMenu(); + } + + ImGui::EndMainMenuBar(); + + if (toCSV) { + ImGui::OpenPopup("SysId JSON to CSV Converter"); + toCSV = false; + } + + if (ImGui::BeginPopupModal("SysId JSON to CSV Converter")) { + gJSONConverter->DisplayCSVConvert(); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + if (about) { + ImGui::OpenPopup("About"); + about = false; + } + if (ImGui::BeginPopupModal("About")) { + ImGui::Text("SysId: System Identification for Robot Mechanisms"); + ImGui::Separator(); + ImGui::Text("v%s", GetWPILibVersion()); + ImGui::Separator(); + ImGui::Text("Save location: %s", glass::GetStorageDir().c_str()); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + }); + + gui::Initialize("System Identification", sysid::kAppWindowSize.x, + sysid::kAppWindowSize.y); + gui::Main(); + + glass::DestroyContext(); + gui::DestroyContext(); +} + +#endif diff --git a/sysid/src/main/native/cpp/Main.cpp b/sysid/src/main/native/cpp/Main.cpp new file mode 100644 index 00000000000..6fba8480da3 --- /dev/null +++ b/sysid/src/main/native/cpp/Main.cpp @@ -0,0 +1,29 @@ +// 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 + +#ifndef RUNNING_SYSID_TESTS + +void Application(std::string_view saveDir); + +#ifdef _WIN32 +int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine, + int nCmdShow) { + int argc = __argc; + char** argv = __argv; +#else +int main(int argc, char** argv) { +#endif + std::string_view saveDir; + if (argc == 2) { + saveDir = argv[1]; + } + + Application(saveDir); + + return 0; +} + +#endif diff --git a/sysid/src/main/native/cpp/Util.cpp b/sysid/src/main/native/cpp/Util.cpp new file mode 100644 index 00000000000..a9acc537a7a --- /dev/null +++ b/sysid/src/main/native/cpp/Util.cpp @@ -0,0 +1,80 @@ +// 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/Util.h" + +#include +#include + +#include +#include + +void sysid::CreateTooltip(const char* text) { + ImGui::SameLine(); + ImGui::TextDisabled(" (?)"); + + if (ImGui::IsItemHovered()) { + ImGui::BeginTooltip(); + ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f); + ImGui::TextUnformatted(text); + ImGui::PopTextWrapPos(); + ImGui::EndTooltip(); + } +} + +void sysid::CreateErrorPopup(bool& isError, std::string_view errorMessage) { + if (isError) { + ImGui::OpenPopup("Exception Caught!"); + } + + // Handle exceptions. + ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f)); + if (ImGui::BeginPopupModal("Exception Caught!")) { + ImGui::PushTextWrapPos(0.0f); + ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s", + errorMessage.data()); + ImGui::PopTextWrapPos(); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + isError = false; + } + ImGui::EndPopup(); + } +} + +std::string_view sysid::GetAbbreviation(std::string_view unit) { + if (unit == "Meters") { + return "m"; + } else if (unit == "Feet") { + return "ft"; + } else if (unit == "Inches") { + return "in"; + } else if (unit == "Radians") { + return "rad"; + } else if (unit == "Degrees") { + return "deg"; + } else if (unit == "Rotations") { + return "rot"; + } else { + throw std::runtime_error("Invalid Unit"); + } +} + +void sysid::SaveFile(std::string_view contents, + const std::filesystem::path& path) { + // Create the path if it doesn't already exist. + std::filesystem::create_directories(path.root_directory()); + + // Open a fd_ostream to write to file. + std::error_code ec; + wpi::raw_fd_ostream ostream{path.string(), ec}; + + // Check error code. + if (ec) { + throw std::runtime_error("Cannot write to file: " + ec.message()); + } + + // Write contents. + ostream << contents; +} diff --git a/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp new file mode 100644 index 00000000000..1d7565b37bd --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/AnalysisManager.cpp @@ -0,0 +1,568 @@ +// 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/AnalysisManager.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "sysid/Util.h" +#include "sysid/analysis/FilteringUtils.h" +#include "sysid/analysis/JSONConverter.h" +#include "sysid/analysis/TrackWidthAnalysis.h" + +using namespace sysid; + +/** + * Converts a raw data vector into a PreparedData vector with only the + * timestamp, voltage, position, and velocity fields filled out. + * + * @tparam S The size of the arrays in the raw data vector + * @tparam Timestamp The index of the Timestamp data in the raw data vector + * arrays + * @tparam Voltage The index of the Voltage data in the raw data vector arrays + * @tparam Position The index of the Position data in the raw data vector arrays + * @tparam Velocity The index of the Velocity data in the raw data vector arrays + * + * @param data A raw data vector + * + * @return A PreparedData vector + */ +template +static std::vector ConvertToPrepared( + const std::vector>& data) { + std::vector prepared; + for (int i = 0; i < static_cast(data.size()) - 1; ++i) { + const auto& pt1 = data[i]; + const auto& pt2 = data[i + 1]; + prepared.emplace_back(PreparedData{ + units::second_t{pt1[Timestamp]}, pt1[Voltage], pt1[Position], + pt1[Velocity], units::second_t{pt2[Timestamp] - pt1[Timestamp]}}); + } + return prepared; +} + +/** + * To preserve a raw copy of the data, this method saves a raw version + * in the dataset StringMap where the key of the raw data starts with "raw-" + * before the name of the original data. + * + * @tparam S The size of the array data that's being used + * + * @param dataset A reference to the dataset being used + */ +template +static void CopyRawData( + wpi::StringMap>>* dataset) { + auto& data = *dataset; + // Loads the Raw Data + for (auto& it : data) { + auto key = it.first(); + auto& dataset = it.getValue(); + + if (!wpi::contains(key, "raw")) { + data[fmt::format("raw-{}", key)] = dataset; + data[fmt::format("original-raw-{}", key)] = dataset; + } + } +} + +/** + * Assigns the combines the various datasets into a single one for analysis. + * + * @param slowForward The slow forward dataset + * @param slowBackward The slow backward dataset + * @param fastForward The fast forward dataset + * @param fastBackward The fast backward dataset + */ +static Storage CombineDatasets(const std::vector& slowForward, + const std::vector& slowBackward, + const std::vector& fastForward, + const std::vector& fastBackward) { + return Storage{slowForward, slowBackward, fastForward, fastBackward}; +} + +void AnalysisManager::PrepareGeneralData() { + using Data = std::array; + wpi::StringMap> data; + wpi::StringMap> preparedData; + + // Store the raw data columns. + static constexpr size_t kTimeCol = 0; + static constexpr size_t kVoltageCol = 1; + static constexpr size_t kPosCol = 2; + static constexpr size_t kVelCol = 3; + + WPI_INFO(m_logger, "{}", "Reading JSON data."); + // Get the major components from the JSON and store them inside a StringMap. + for (auto&& key : AnalysisManager::kJsonDataKeys) { + data[key] = m_json.at(key).get>(); + } + + WPI_INFO(m_logger, "{}", "Preprocessing raw data."); + // Ensure that voltage and velocity have the same sign. Also multiply + // positions and velocities by the factor. + for (auto it = data.begin(); it != data.end(); ++it) { + for (auto&& pt : it->second) { + pt[kVoltageCol] = std::copysign(pt[kVoltageCol], pt[kVelCol]); + pt[kPosCol] *= m_factor; + pt[kVelCol] *= m_factor; + } + } + + WPI_INFO(m_logger, "{}", "Copying raw data."); + CopyRawData(&data); + + WPI_INFO(m_logger, "{}", "Converting raw data to PreparedData struct."); + // Convert data to PreparedData structs + for (auto& it : data) { + auto key = it.first(); + preparedData[key] = + ConvertToPrepared<4, kTimeCol, kVoltageCol, kPosCol, kVelCol>( + data[key]); + } + + // Store the original datasets + m_originalDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets(preparedData["original-raw-slow-forward"], + preparedData["original-raw-slow-backward"], + preparedData["original-raw-fast-forward"], + preparedData["original-raw-fast-backward"]); + + WPI_INFO(m_logger, "{}", "Initial trimming and filtering."); + sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays, + m_velocityDelays, m_minStepTime, m_maxStepTime, + m_unit); + + WPI_INFO(m_logger, "{}", "Acceleration filtering."); + sysid::AccelFilter(&preparedData); + + WPI_INFO(m_logger, "{}", "Storing datasets."); + // Store the raw datasets + m_rawDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets( + preparedData["raw-slow-forward"], preparedData["raw-slow-backward"], + preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]); + + // Store the filtered datasets + m_filteredDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets( + preparedData["slow-forward"], preparedData["slow-backward"], + preparedData["fast-forward"], preparedData["fast-backward"]); + + m_startTimes = {preparedData["raw-slow-forward"][0].timestamp, + preparedData["raw-slow-backward"][0].timestamp, + preparedData["raw-fast-forward"][0].timestamp, + preparedData["raw-fast-backward"][0].timestamp}; +} + +void AnalysisManager::PrepareAngularDrivetrainData() { + using Data = std::array; + wpi::StringMap> data; + wpi::StringMap> preparedData; + + // Store the relevant raw data columns. + static constexpr size_t kTimeCol = 0; + static constexpr size_t kLVoltageCol = 1; + static constexpr size_t kRVoltageCol = 2; + static constexpr size_t kLPosCol = 3; + static constexpr size_t kRPosCol = 4; + static constexpr size_t kLVelCol = 5; + static constexpr size_t kRVelCol = 6; + static constexpr size_t kAngleCol = 7; + static constexpr size_t kAngularRateCol = 8; + + WPI_INFO(m_logger, "{}", "Reading JSON data."); + // Get the major components from the JSON and store them inside a StringMap. + for (auto&& key : AnalysisManager::kJsonDataKeys) { + data[key] = m_json.at(key).get>(); + } + + WPI_INFO(m_logger, "{}", "Preprocessing raw data."); + // Ensure that voltage and velocity have the same sign. Also multiply + // positions and velocities by the factor. + for (auto it = data.begin(); it != data.end(); ++it) { + for (auto&& pt : it->second) { + pt[kLPosCol] *= m_factor; + pt[kRPosCol] *= m_factor; + pt[kLVelCol] *= m_factor; + pt[kRVelCol] *= m_factor; + + // Stores the average voltages in the left voltage column. + // This aggregates the left and right voltages into a single voltage + // column for the ConvertToPrepared() method. std::copysign() ensures the + // polarity of the voltage matches the direction the robot turns. + pt[kLVoltageCol] = std::copysign( + (std::abs(pt[kLVoltageCol]) + std::abs(pt[kRVoltageCol])) / 2, + pt[kAngularRateCol]); + + // ω = (v_r - v_l) / trackwidth + // v = ωr => v = ω * trackwidth / 2 + // (v_r - v_l) / trackwidth * (trackwidth / 2) = (v_r - v_l) / 2 + // However, since we know this is an angular test, the left and right + // wheel velocities will have opposite signs, allowing us to add their + // absolute values and get the same result (in terms of magnitude). + // std::copysign() is used to make sure the direction of the wheel + // velocities matches the direction the robot turns. + pt[kAngularRateCol] = + std::copysign((std::abs(pt[kRVelCol]) + std::abs(pt[kLVelCol])) / 2, + pt[kAngularRateCol]); + } + } + + WPI_INFO(m_logger, "{}", "Calculating trackwidth"); + // Aggregating all the deltas from all the tests + double leftDelta = 0.0; + double rightDelta = 0.0; + double angleDelta = 0.0; + for (const auto& it : data) { + auto key = it.first(); + auto& trackWidthData = data[key]; + leftDelta += std::abs(trackWidthData.back()[kLPosCol] - + trackWidthData.front()[kLPosCol]); + rightDelta += std::abs(trackWidthData.back()[kRPosCol] - + trackWidthData.front()[kRPosCol]); + angleDelta += std::abs(trackWidthData.back()[kAngleCol] - + trackWidthData.front()[kAngleCol]); + } + m_trackWidth = sysid::CalculateTrackWidth(leftDelta, rightDelta, + units::radian_t{angleDelta}); + + WPI_INFO(m_logger, "{}", "Copying raw data."); + CopyRawData(&data); + + WPI_INFO(m_logger, "{}", "Converting to PreparedData struct."); + // Convert raw data to prepared data + for (const auto& it : data) { + auto key = it.first(); + preparedData[key] = ConvertToPrepared<9, kTimeCol, kLVoltageCol, kAngleCol, + kAngularRateCol>(data[key]); + } + + // Create the distinct datasets and store them + m_originalDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets(preparedData["original-raw-slow-forward"], + preparedData["original-raw-slow-backward"], + preparedData["original-raw-fast-forward"], + preparedData["original-raw-fast-backward"]); + + WPI_INFO(m_logger, "{}", "Applying trimming and filtering."); + sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays, + m_velocityDelays, m_minStepTime, m_maxStepTime); + + WPI_INFO(m_logger, "{}", "Acceleration filtering."); + sysid::AccelFilter(&preparedData); + + WPI_INFO(m_logger, "{}", "Storing datasets."); + // Create the distinct datasets and store them + m_rawDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets( + preparedData["raw-slow-forward"], preparedData["raw-slow-backward"], + preparedData["raw-fast-forward"], preparedData["raw-fast-backward"]); + m_filteredDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets( + preparedData["slow-forward"], preparedData["slow-backward"], + preparedData["fast-forward"], preparedData["fast-backward"]); + + m_startTimes = {preparedData["slow-forward"][0].timestamp, + preparedData["slow-backward"][0].timestamp, + preparedData["fast-forward"][0].timestamp, + preparedData["fast-backward"][0].timestamp}; +} + +void AnalysisManager::PrepareLinearDrivetrainData() { + using Data = std::array; + wpi::StringMap> data; + wpi::StringMap> preparedData; + + // Store the relevant raw data columns. + static constexpr size_t kTimeCol = 0; + static constexpr size_t kLVoltageCol = 1; + static constexpr size_t kRVoltageCol = 2; + static constexpr size_t kLPosCol = 3; + static constexpr size_t kRPosCol = 4; + static constexpr size_t kLVelCol = 5; + static constexpr size_t kRVelCol = 6; + + // Get the major components from the JSON and store them inside a StringMap. + WPI_INFO(m_logger, "{}", "Reading JSON data."); + for (auto&& key : AnalysisManager::kJsonDataKeys) { + data[key] = m_json.at(key).get>(); + } + + // Ensure that voltage and velocity have the same sign. Also multiply + // positions and velocities by the factor. + WPI_INFO(m_logger, "{}", "Preprocessing raw data."); + for (auto it = data.begin(); it != data.end(); ++it) { + for (auto&& pt : it->second) { + pt[kLVoltageCol] = std::copysign(pt[kLVoltageCol], pt[kLVelCol]); + pt[kRVoltageCol] = std::copysign(pt[kRVoltageCol], pt[kRVelCol]); + pt[kLPosCol] *= m_factor; + pt[kRPosCol] *= m_factor; + pt[kLVelCol] *= m_factor; + pt[kRVelCol] *= m_factor; + } + } + + WPI_INFO(m_logger, "{}", "Copying raw data."); + CopyRawData(&data); + + // Convert data to PreparedData + WPI_INFO(m_logger, "{}", "Converting to PreparedData struct."); + for (auto& it : data) { + auto key = it.first(); + + preparedData[fmt::format("left-{}", key)] = + ConvertToPrepared<9, kTimeCol, kLVoltageCol, kLPosCol, kLVelCol>( + data[key]); + preparedData[fmt::format("right-{}", key)] = + ConvertToPrepared<9, kTimeCol, kRVoltageCol, kRPosCol, kRVelCol>( + data[key]); + } + + // Create the distinct raw datasets and store them + auto originalSlowForward = AnalysisManager::DataConcat( + preparedData["left-original-raw-slow-forward"], + preparedData["right-original-raw-slow-forward"]); + auto originalSlowBackward = AnalysisManager::DataConcat( + preparedData["left-original-raw-slow-backward"], + preparedData["right-original-raw-slow-backward"]); + auto originalFastForward = AnalysisManager::DataConcat( + preparedData["left-original-raw-fast-forward"], + preparedData["right-original-raw-fast-forward"]); + auto originalFastBackward = AnalysisManager::DataConcat( + preparedData["left-original-raw-fast-backward"], + preparedData["right-original-raw-fast-backward"]); + m_originalDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets(originalSlowForward, originalSlowBackward, + originalFastForward, originalFastBackward); + m_originalDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kLeft)] = + CombineDatasets(preparedData["left-original-raw-slow-forward"], + preparedData["left-original-raw-slow-backward"], + preparedData["left-original-raw-fast-forward"], + preparedData["left-original-raw-fast-backward"]); + m_originalDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kRight)] = + CombineDatasets(preparedData["right-original-raw-slow-forward"], + preparedData["right-original-raw-slow-backward"], + preparedData["right-original-raw-fast-forward"], + preparedData["right-original-raw-fast-backward"]); + + WPI_INFO(m_logger, "{}", "Applying trimming and filtering."); + sysid::InitialTrimAndFilter(&preparedData, &m_settings, m_positionDelays, + m_velocityDelays, m_minStepTime, m_maxStepTime); + + auto slowForward = AnalysisManager::DataConcat( + preparedData["left-slow-forward"], preparedData["right-slow-forward"]); + auto slowBackward = AnalysisManager::DataConcat( + preparedData["left-slow-backward"], preparedData["right-slow-backward"]); + auto fastForward = AnalysisManager::DataConcat( + preparedData["left-fast-forward"], preparedData["right-fast-forward"]); + auto fastBackward = AnalysisManager::DataConcat( + preparedData["left-fast-backward"], preparedData["right-fast-backward"]); + + WPI_INFO(m_logger, "{}", "Acceleration filtering."); + sysid::AccelFilter(&preparedData); + + WPI_INFO(m_logger, "{}", "Storing datasets."); + + // Create the distinct raw datasets and store them + auto rawSlowForward = + AnalysisManager::DataConcat(preparedData["left-raw-slow-forward"], + preparedData["right-raw-slow-forward"]); + auto rawSlowBackward = + AnalysisManager::DataConcat(preparedData["left-raw-slow-backward"], + preparedData["right-raw-slow-backward"]); + auto rawFastForward = + AnalysisManager::DataConcat(preparedData["left-raw-fast-forward"], + preparedData["right-raw-fast-forward"]); + auto rawFastBackward = + AnalysisManager::DataConcat(preparedData["left-raw-fast-backward"], + preparedData["right-raw-fast-backward"]); + + m_rawDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward, + rawFastBackward); + m_rawDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kLeft)] = + CombineDatasets(preparedData["left-raw-slow-forward"], + preparedData["left-raw-slow-backward"], + preparedData["left-raw-fast-forward"], + preparedData["left-raw-fast-backward"]); + m_rawDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kRight)] = + CombineDatasets(preparedData["right-raw-slow-forward"], + preparedData["right-raw-slow-backward"], + preparedData["right-raw-fast-forward"], + preparedData["right-raw-fast-backward"]); + + // Create the distinct filtered datasets and store them + m_filteredDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kCombined)] = + CombineDatasets(slowForward, slowBackward, fastForward, fastBackward); + m_filteredDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kLeft)] = + CombineDatasets(preparedData["left-slow-forward"], + preparedData["left-slow-backward"], + preparedData["left-fast-forward"], + preparedData["left-fast-backward"]); + m_filteredDataset[static_cast( + AnalysisManager::Settings::DrivetrainDataset::kRight)] = + CombineDatasets(preparedData["right-slow-forward"], + preparedData["right-slow-backward"], + preparedData["right-fast-forward"], + preparedData["right-fast-backward"]); + + m_startTimes = { + rawSlowForward.front().timestamp, rawSlowBackward.front().timestamp, + rawFastForward.front().timestamp, rawFastBackward.front().timestamp}; +} + +AnalysisManager::AnalysisManager(Settings& settings, wpi::Logger& logger) + : m_logger{logger}, + m_settings{settings}, + m_type{analysis::kSimple}, + m_unit{"Meters"}, + m_factor{1} {} + +AnalysisManager::AnalysisManager(std::string_view path, Settings& settings, + wpi::Logger& logger) + : m_logger{logger}, m_settings{settings} { + { + // Read JSON from the specified path + std::error_code ec; + wpi::raw_fd_istream is{path, ec}; + + if (ec) { + throw FileReadingError(path); + } + + is >> m_json; + + WPI_INFO(m_logger, "Read {}", path); + } + + // Check that we have a sysid JSON + if (m_json.find("sysid") == m_json.end()) { + // If it's not a sysid JSON, try converting it from frc-char format + std::string newPath = sysid::ConvertJSON(path, logger); + + // Read JSON from the specified path + std::error_code ec; + wpi::raw_fd_istream is{newPath, ec}; + + if (ec) { + throw FileReadingError(newPath); + } + + is >> m_json; + + WPI_INFO(m_logger, "Read {}", newPath); + } + + WPI_INFO(m_logger, "Parsing initial data of {}", path); + // Get the analysis type from the JSON. + m_type = sysid::analysis::FromName(m_json.at("test").get()); + + // Get the rotation -> output units factor from the JSON. + m_unit = m_json.at("units").get(); + m_factor = m_json.at("unitsPerRotation").get(); + WPI_DEBUG(m_logger, "Parsing units per rotation as {} {} per rotation", + m_factor, m_unit); + + // Reset settings for Dynamic Test Limits + m_settings.stepTestDuration = units::second_t{0.0}; + m_settings.motionThreshold = std::numeric_limits::infinity(); +} + +void AnalysisManager::PrepareData() { + WPI_INFO(m_logger, "Preparing {} data", m_type.name); + if (m_type == analysis::kDrivetrain) { + PrepareLinearDrivetrainData(); + } else if (m_type == analysis::kDrivetrainAngular) { + PrepareAngularDrivetrainData(); + } else { + PrepareGeneralData(); + } + WPI_INFO(m_logger, "{}", "Finished Preparing Data"); +} + +AnalysisManager::FeedforwardGains AnalysisManager::CalculateFeedforward() { + if (m_filteredDataset.empty()) { + throw sysid::InvalidDataError( + "There is no data to perform gain calculation on."); + } + + WPI_INFO(m_logger, "{}", "Calculating Gains"); + // Calculate feedforward gains from the data. + const auto& ff = sysid::CalculateFeedforwardGains(GetFilteredData(), m_type); + FeedforwardGains ffGains = {ff, m_trackWidth}; + + const auto& Ks = std::get<0>(ff)[0]; + const auto& Kv = std::get<0>(ff)[1]; + const auto& Ka = std::get<0>(ff)[2]; + + if (Ka <= 0 || Kv < 0) { + throw InvalidDataError( + fmt::format("The calculated feedforward gains of kS: {}, Kv: {}, Ka: " + "{} are erroneous. Your Ka should be > 0 while your Kv and " + "Ks constants should both >= 0. Try adjusting the " + "filtering and trimming settings or collect better data.", + Ks, Kv, Ka)); + } + + return ffGains; +} + +sysid::FeedbackGains AnalysisManager::CalculateFeedback( + std::vector ff) { + const auto& Kv = ff[1]; + const auto& Ka = ff[2]; + FeedbackGains fb; + if (m_settings.type == FeedbackControllerLoopType::kPosition) { + fb = sysid::CalculatePositionFeedbackGains( + m_settings.preset, m_settings.lqr, Kv, Ka, + m_settings.convertGainsToEncTicks + ? m_settings.gearing * m_settings.cpr * m_factor + : 1); + } else { + fb = sysid::CalculateVelocityFeedbackGains( + m_settings.preset, m_settings.lqr, Kv, Ka, + m_settings.convertGainsToEncTicks + ? m_settings.gearing * m_settings.cpr * m_factor + : 1); + } + + return fb; +} + +void AnalysisManager::OverrideUnits(std::string_view unit, + double unitsPerRotation) { + m_unit = unit; + m_factor = unitsPerRotation; +} + +void AnalysisManager::ResetUnitsFromJSON() { + m_unit = m_json.at("units").get(); + m_factor = m_json.at("unitsPerRotation").get(); +} diff --git a/sysid/src/main/native/cpp/analysis/AnalysisType.cpp b/sysid/src/main/native/cpp/analysis/AnalysisType.cpp new file mode 100644 index 00000000000..18b461fe6d8 --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/AnalysisType.cpp @@ -0,0 +1,23 @@ +// 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/AnalysisType.h" + +using namespace sysid; + +AnalysisType sysid::analysis::FromName(std::string_view name) { + if (name == "Drivetrain") { + return sysid::analysis::kDrivetrain; + } + if (name == "Drivetrain (Angular)") { + return sysid::analysis::kDrivetrainAngular; + } + if (name == "Elevator") { + return sysid::analysis::kElevator; + } + if (name == "Arm") { + return sysid::analysis::kArm; + } + return sysid::analysis::kSimple; +} diff --git a/sysid/src/main/native/cpp/analysis/ArmSim.cpp b/sysid/src/main/native/cpp/analysis/ArmSim.cpp new file mode 100644 index 00000000000..a72f50569d1 --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/ArmSim.cpp @@ -0,0 +1,64 @@ +// 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/ArmSim.h" + +#include + +#include +#include +#include + +using namespace sysid; + +ArmSim::ArmSim(double Ks, double Kv, double Ka, double Kg, double offset, + double initialPosition, double initialVelocity) + // u = Ks sgn(x) + Kv x + Ka a + Kg cos(theta) + // Ka a = u - Ks sgn(x) - Kv x - Kg cos(theta) + // a = 1/Ka u - Ks/Ka sgn(x) - Kv/Ka x - Kg/Ka cos(theta) + // a = -Kv/Ka x + 1/Ka u - Ks/Ka sgn(x) - Kg/Ka cos(theta) + // a = Ax + Bu + c sgn(x) + d cos(theta) + : m_A{-Kv / Ka}, + m_B{1.0 / Ka}, + m_c{-Ks / Ka}, + m_d{-Kg / Ka}, + m_offset{offset} { + Reset(initialPosition, initialVelocity); +} + +void ArmSim::Update(units::volt_t voltage, units::second_t dt) { + // Returns arm acceleration under gravity + auto f = [=, this]( + const Eigen::Vector& x, + const Eigen::Vector& u) -> Eigen::Vector { + return Eigen::Vector{ + x(1), (m_A * x.block<1, 1>(1, 0) + m_B * u + m_c * wpi::sgn(x(1)) + + m_d * std::cos(x(0) + m_offset))(0)}; + }; + + // Max error is large because an accurate sim isn't as important as the sim + // finishing in a timely manner. Otherwise, the timestep can become absurdly + // small for ill-conditioned data (e.g., high velocities with sharp spikes in + // acceleration). + Eigen::Vector u{voltage.value()}; + m_x = frc::RKDP(f, m_x, u, dt, 0.25); +} + +double ArmSim::GetPosition() const { + return m_x(0); +} + +double ArmSim::GetVelocity() const { + return m_x(1); +} + +double ArmSim::GetAcceleration(units::volt_t voltage) const { + Eigen::Vector u{voltage.value()}; + return (m_A * m_x.block<1, 1>(1, 0) + m_B * u + + m_c * wpi::sgn(GetVelocity()) + m_d * std::cos(m_x(0) + m_offset))(0); +} + +void ArmSim::Reset(double position, double velocity) { + m_x = Eigen::Vector{position, velocity}; +} diff --git a/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp b/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp new file mode 100644 index 00000000000..5cfcabed763 --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/ElevatorSim.cpp @@ -0,0 +1,50 @@ +// 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/ElevatorSim.h" + +#include +#include +#include + +using namespace sysid; + +ElevatorSim::ElevatorSim(double Ks, double Kv, double Ka, double Kg, + double initialPosition, double initialVelocity) + // dx/dt = Ax + Bu + c sgn(x) + d + : m_A{{0.0, 1.0}, {0.0, -Kv / Ka}}, + m_B{0.0, 1.0 / Ka}, + m_c{0.0, -Ks / Ka}, + m_d{0.0, -Kg / Ka} { + Reset(initialPosition, initialVelocity); +} + +void ElevatorSim::Update(units::volt_t voltage, units::second_t dt) { + Eigen::Vector u{voltage.value()}; + + // Given dx/dt = Ax + Bu + c sgn(x) + d, + // x_k+1 = e^(AT) x_k + A^-1 (e^(AT) - 1) (Bu + c sgn(x) + d) + Eigen::Matrix Ad; + Eigen::Matrix Bd; + frc::DiscretizeAB<2, 1>(m_A, m_B, dt, &Ad, &Bd); + m_x = Ad * m_x + Bd * u + + Bd * m_B.householderQr().solve(m_c * wpi::sgn(GetVelocity()) + m_d); +} + +double ElevatorSim::GetPosition() const { + return m_x(0); +} + +double ElevatorSim::GetVelocity() const { + return m_x(1); +} + +double ElevatorSim::GetAcceleration(units::volt_t voltage) const { + Eigen::Vector u{voltage.value()}; + return (m_A * m_x + m_B * u + m_c * wpi::sgn(GetVelocity()) + m_d)(1); +} + +void ElevatorSim::Reset(double position, double velocity) { + m_x = Eigen::Vector{position, velocity}; +} diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp new file mode 100644 index 00000000000..0691aaf6ebb --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -0,0 +1,78 @@ +// 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/FeedbackAnalysis.h" + +#include +#include +#include +#include +#include +#include + +#include "sysid/analysis/FeedbackControllerPreset.h" + +using namespace sysid; + +using Kv_t = decltype(1_V / 1_mps); +using Ka_t = decltype(1_V / 1_mps_sq); + +FeedbackGains sysid::CalculatePositionFeedbackGains( + const FeedbackControllerPreset& preset, const LQRParameters& params, + double Kv, double Ka, double encFactor) { + // If acceleration requires no effort, velocity becomes an input for position + // control. We choose an appropriate model in this case to avoid numerical + // instabilities in the LQR. + if (Ka > 1E-7) { + // Create a position system from our feedforward gains. + auto system = frc::LinearSystemId::IdentifyPositionSystem( + Kv_t(Kv), Ka_t(Ka)); + // Create an LQR with 2 states to control -- position and velocity. + frc::LinearQuadraticRegulator<2, 1> controller{ + system, {params.qp, params.qv}, {params.r}, preset.period}; + // Compensate for any latency from sensor measurements, filtering, etc. + controller.LatencyCompensate(system, preset.period, 0.0_s); + + return {controller.K(0, 0) * preset.outputConversionFactor / encFactor, + controller.K(0, 1) * preset.outputConversionFactor / + (encFactor * (preset.normalized ? 1 : preset.period.value()))}; + } + + // This is our special model to avoid instabilities in the LQR. + auto system = frc::LinearSystem<1, 1, 1>( + Eigen::Matrix{0.0}, Eigen::Matrix{1.0}, + Eigen::Matrix{1.0}, Eigen::Matrix{0.0}); + // Create an LQR with one state -- position. + frc::LinearQuadraticRegulator<1, 1> controller{ + system, {params.qp}, {params.r}, preset.period}; + // Compensate for any latency from sensor measurements, filtering, etc. + controller.LatencyCompensate(system, preset.period, 0.0_s); + + return {Kv * controller.K(0, 0) * preset.outputConversionFactor / encFactor, + 0.0}; +} + +FeedbackGains sysid::CalculateVelocityFeedbackGains( + const FeedbackControllerPreset& preset, const LQRParameters& params, + double Kv, double Ka, double encFactor) { + // If acceleration for velocity control requires no effort, the feedback + // control gains approach zero. We special-case it here because numerical + // instabilities arise in LQR otherwise. + if (Ka < 1E-7) { + return {0.0, 0.0}; + } + + // Create a velocity system from our feedforward gains. + auto system = frc::LinearSystemId::IdentifyVelocitySystem( + Kv_t(Kv), Ka_t(Ka)); + // Create an LQR controller with 1 state -- velocity. + frc::LinearQuadraticRegulator<1, 1> controller{ + system, {params.qv}, {params.r}, preset.period}; + // Compensate for any latency from sensor measurements, filtering, etc. + controller.LatencyCompensate(system, preset.period, preset.measurementDelay); + + return {controller.K(0, 0) * preset.outputConversionFactor / + (preset.outputVelocityTimeFactor * encFactor), + 0.0}; +} diff --git a/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp new file mode 100644 index 00000000000..b7a9fce79b7 --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp @@ -0,0 +1,120 @@ +// 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/FeedforwardAnalysis.h" + +#include + +#include +#include + +#include "sysid/analysis/AnalysisManager.h" +#include "sysid/analysis/FilteringUtils.h" +#include "sysid/analysis/OLS.h" + +using namespace sysid; + +/** + * Populates OLS data for (xₖ₊₁ − xₖ)/τ = αxₖ + βuₖ + γ sgn(xₖ). + * + * @param d List of characterization data. + * @param type Type of system being identified. + * @param X Vector representation of X in y = Xβ. + * @param y Vector representation of y in y = Xβ. + */ +static void PopulateOLSData(const std::vector& d, + const AnalysisType& type, + Eigen::Block X, + Eigen::VectorBlock y) { + for (size_t sample = 0; sample < d.size(); ++sample) { + const auto& pt = d[sample]; + + // Add the velocity term (for α) + X(sample, 0) = pt.velocity; + + // Add the voltage term (for β) + X(sample, 1) = pt.voltage; + + // Add the intercept term (for γ) + X(sample, 2) = std::copysign(1, pt.velocity); + + // Add test-specific variables + if (type == analysis::kElevator) { + // Add the gravity term (for Kg) + X(sample, 3) = 1.0; + } else if (type == analysis::kArm) { + // Add the cosine and sine terms (for Kg) + X(sample, 3) = pt.cos; + X(sample, 4) = pt.sin; + } + + // Add the dependent variable (acceleration) + y(sample) = pt.acceleration; + } +} + +std::tuple, double, double> +sysid::CalculateFeedforwardGains(const Storage& data, + const AnalysisType& type) { + // Iterate through the data and add it to our raw vector. + const auto& [slowForward, slowBackward, fastForward, fastBackward] = data; + + const auto size = slowForward.size() + slowBackward.size() + + fastForward.size() + fastBackward.size(); + + // Create a raw vector of doubles with our data in it. + Eigen::MatrixXd X{size, type.independentVariables}; + Eigen::VectorXd y{size}; + + int rowOffset = 0; + PopulateOLSData(slowForward, type, + X.block(rowOffset, 0, slowForward.size(), X.cols()), + y.segment(rowOffset, slowForward.size())); + + rowOffset += slowForward.size(); + PopulateOLSData(slowBackward, type, + X.block(rowOffset, 0, slowBackward.size(), X.cols()), + y.segment(rowOffset, slowBackward.size())); + + rowOffset += slowBackward.size(); + PopulateOLSData(fastForward, type, + X.block(rowOffset, 0, fastForward.size(), X.cols()), + y.segment(rowOffset, fastForward.size())); + + rowOffset += fastForward.size(); + PopulateOLSData(fastBackward, type, + X.block(rowOffset, 0, fastBackward.size(), X.cols()), + y.segment(rowOffset, fastBackward.size())); + + // Perform OLS with accel = alpha*vel + beta*voltage + gamma*signum(vel) + // OLS performs best with the noisiest variable as the dependent var, + // so we regress accel in terms of the other variables. + auto ols = sysid::OLS(X, y); + double alpha = std::get<0>(ols)[0]; // -Kv/Ka + double beta = std::get<0>(ols)[1]; // 1/Ka + double gamma = std::get<0>(ols)[2]; // -Ks/Ka + + // Initialize gains list with Ks, Kv, and Ka + std::vector gains{-gamma / beta, -alpha / beta, 1 / beta}; + + if (type == analysis::kElevator) { + // Add Kg to gains list + double delta = std::get<0>(ols)[3]; // -Kg/Ka + gains.emplace_back(-delta / beta); + } + + if (type == analysis::kArm) { + double delta = std::get<0>(ols)[3]; // -Kg/Ka cos(offset) + double epsilon = std::get<0>(ols)[4]; // Kg/Ka sin(offset) + + // Add Kg to gains list + gains.emplace_back(std::hypot(delta, epsilon) / beta); + + // Add offset to gains list + gains.emplace_back(std::atan2(epsilon, -delta)); + } + + // Gains are Ks, Kv, Ka, Kg (elevator/arm only), offset (arm only) + return std::tuple{gains, std::get<1>(ols), std::get<2>(ols)}; +} diff --git a/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp new file mode 100644 index 00000000000..6c66ef8b424 --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/FilteringUtils.cpp @@ -0,0 +1,417 @@ +// 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/FilteringUtils.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace sysid; + +/** + * Helper function that throws if it detects that the data vector is too small + * for an operation of a certain window size. + * + * @param data The data that is being used. + * @param window The window size for the operation. + * @param operation The operation we're checking the size for (for error + * throwing purposes). + */ +static void CheckSize(const std::vector& data, size_t window, + std::string_view operation) { + if (data.size() < window) { + throw sysid::InvalidDataError( + fmt::format("Not enough data to run {} which has a window size of {}.", + operation, window)); + } +} + +/** + * Helper function that determines if a certain key is storing raw data. + * + * @param key The key of the dataset + * + * @return True, if the key corresponds to a raw dataset. + */ +static bool IsRaw(std::string_view key) { + return wpi::contains(key, "raw") && !wpi::contains(key, "original"); +} + +/** + * Helper function that determines if a certain key is storing filtered data. + * + * @param key The key of the dataset + * + * @return True, if the key corresponds to a filtered dataset. + */ +static bool IsFiltered(std::string_view key) { + return !wpi::contains(key, "raw") && !wpi::contains(key, "original"); +} + +/** + * Fills in the rest of the PreparedData Structs for a PreparedData Vector. + * + * @param data A reference to a vector of the raw data. + * @param unit The units that the data is in (rotations, radians, or degrees) + * for arm mechanisms. + */ +static void PrepareMechData(std::vector* data, + std::string_view unit = "") { + constexpr size_t kWindow = 3; + + CheckSize(*data, kWindow, "Acceleration Calculation"); + + // Calculates the cosine of the position data for single jointed arm analysis + for (size_t i = 0; i < data->size(); ++i) { + auto& pt = data->at(i); + + double cos = 0.0; + double sin = 0.0; + if (unit == "Radians") { + cos = std::cos(pt.position); + sin = std::sin(pt.position); + } else if (unit == "Degrees") { + cos = std::cos(pt.position * std::numbers::pi / 180.0); + sin = std::sin(pt.position * std::numbers::pi / 180.0); + } else if (unit == "Rotations") { + cos = std::cos(pt.position * 2 * std::numbers::pi); + sin = std::sin(pt.position * 2 * std::numbers::pi); + } + pt.cos = cos; + pt.sin = sin; + } + + auto derivative = + CentralFiniteDifference<1, kWindow>(GetMeanTimeDelta(*data)); + + // Load the derivative filter with the first value for accurate initial + // behavior + for (size_t i = 0; i < kWindow; ++i) { + derivative.Calculate(data->at(0).velocity); + } + + for (size_t i = (kWindow - 1) / 2; i < data->size(); ++i) { + data->at(i - (kWindow - 1) / 2).acceleration = + derivative.Calculate(data->at(i).velocity); + } + + // Fill in accelerations past end of derivative filter + for (size_t i = data->size() - (kWindow - 1) / 2; i < data->size(); ++i) { + data->at(i).acceleration = 0.0; + } +} + +std::tuple +sysid::TrimStepVoltageData(std::vector* data, + AnalysisManager::Settings* settings, + units::second_t minStepTime, + units::second_t maxStepTime) { + auto voltageBegins = + std::find_if(data->begin(), data->end(), + [](auto& datum) { return std::abs(datum.voltage) > 0; }); + + units::second_t firstTimestamp = voltageBegins->timestamp; + double firstPosition = voltageBegins->position; + + auto motionBegins = std::find_if( + data->begin(), data->end(), [settings, firstPosition](auto& datum) { + return std::abs(datum.position - firstPosition) > + (settings->motionThreshold * datum.dt.value()); + }); + + units::second_t positionDelay; + if (motionBegins != data->end()) { + positionDelay = motionBegins->timestamp - firstTimestamp; + } else { + positionDelay = 0_s; + } + + auto maxAccel = std::max_element( + data->begin(), data->end(), [](const auto& a, const auto& b) { + return std::abs(a.acceleration) < std::abs(b.acceleration); + }); + + units::second_t velocityDelay; + if (maxAccel != data->end()) { + velocityDelay = maxAccel->timestamp - firstTimestamp; + + // Trim data before max acceleration + data->erase(data->begin(), maxAccel); + } else { + velocityDelay = 0_s; + } + + minStepTime = std::min(data->at(0).timestamp - firstTimestamp, minStepTime); + + // If step duration hasn't been set yet, calculate a default (find the entry + // before the acceleration first hits zero) + if (settings->stepTestDuration <= minStepTime) { + // Get noise floor + const double accelNoiseFloor = GetNoiseFloor( + *data, kNoiseMeanWindow, [](auto&& pt) { return pt.acceleration; }); + // Find latest element with nonzero acceleration + auto endIt = std::find_if( + data->rbegin(), data->rend(), [&](const PreparedData& entry) { + return std::abs(entry.acceleration) > accelNoiseFloor; + }); + + if (endIt != data->rend()) { + // Calculate default duration + settings->stepTestDuration = std::min( + endIt->timestamp - data->front().timestamp + minStepTime + 1_s, + maxStepTime); + } else { + settings->stepTestDuration = maxStepTime; + } + } + + // Find first entry greater than the step test duration + auto maxIt = + std::find_if(data->begin(), data->end(), [&](PreparedData entry) { + return entry.timestamp - data->front().timestamp + minStepTime > + settings->stepTestDuration; + }); + + // Trim data beyond desired step test duration + if (maxIt != data->end()) { + data->erase(maxIt, data->end()); + } + return std::make_tuple(minStepTime, positionDelay, velocityDelay); +} + +double sysid::GetNoiseFloor( + const std::vector& data, int window, + std::function accessorFunction) { + double sum = 0.0; + size_t step = window / 2; + auto averageFilter = frc::LinearFilter::MovingAverage(window); + for (size_t i = 0; i < data.size(); i++) { + double mean = averageFilter.Calculate(accessorFunction(data[i])); + if (i >= step) { + sum += std::pow(accessorFunction(data[i - step]) - mean, 2); + } + } + return std::sqrt(sum / (data.size() - step)); +} + +units::second_t sysid::GetMeanTimeDelta(const std::vector& data) { + std::vector dts; + + for (const auto& pt : data) { + if (pt.dt > 0_s && pt.dt < 500_ms) { + dts.emplace_back(pt.dt); + } + } + + return std::accumulate(dts.begin(), dts.end(), 0_s) / dts.size(); +} + +units::second_t sysid::GetMeanTimeDelta(const Storage& data) { + std::vector dts; + + for (const auto& pt : data.slowForward) { + if (pt.dt > 0_s && pt.dt < 500_ms) { + dts.emplace_back(pt.dt); + } + } + + for (const auto& pt : data.slowBackward) { + if (pt.dt > 0_s && pt.dt < 500_ms) { + dts.emplace_back(pt.dt); + } + } + + for (const auto& pt : data.fastForward) { + if (pt.dt > 0_s && pt.dt < 500_ms) { + dts.emplace_back(pt.dt); + } + } + + for (const auto& pt : data.fastBackward) { + if (pt.dt > 0_s && pt.dt < 500_ms) { + dts.emplace_back(pt.dt); + } + } + + return std::accumulate(dts.begin(), dts.end(), 0_s) / dts.size(); +} + +void sysid::ApplyMedianFilter(std::vector* data, int window) { + CheckSize(*data, window, "Median Filter"); + + frc::MedianFilter medianFilter(window); + + // Load the median filter with the first value for accurate initial behavior + for (int i = 0; i < window; i++) { + medianFilter.Calculate(data->at(0).velocity); + } + + for (size_t i = (window - 1) / 2; i < data->size(); i++) { + data->at(i - (window - 1) / 2).velocity = + medianFilter.Calculate(data->at(i).velocity); + } + + // Run the median filter for the last half window of datapoints by loading the + // median filter with the last recorded velocity value + for (size_t i = data->size() - (window - 1) / 2; i < data->size(); i++) { + data->at(i).velocity = + medianFilter.Calculate(data->at(data->size() - 1).velocity); + } +} + +/** + * Removes a substring from a string reference + * + * @param str The std::string_view that needs modification + * @param removeStr The substring that needs to be removed + * + * @return an std::string without the specified substring + */ +static std::string RemoveStr(std::string_view str, std::string_view removeStr) { + size_t idx = str.find(removeStr); + if (idx == std::string_view::npos) { + return std::string{str}; + } else { + return fmt::format("{}{}", str.substr(0, idx), + str.substr(idx + removeStr.size())); + } +} + +/** + * Figures out the max duration of the Dynamic tests + * + * @param data The raw data String Map + * + * @return The maximum duration of the Dynamic Tests + */ +static units::second_t GetMaxStepTime( + wpi::StringMap>& data) { + auto maxStepTime = 0_s; + for (auto& it : data) { + auto key = it.first(); + auto& dataset = it.getValue(); + + if (IsRaw(key) && wpi::contains(key, "fast")) { + auto duration = dataset.back().timestamp - dataset.front().timestamp; + if (duration > maxStepTime) { + maxStepTime = duration; + } + } + } + return maxStepTime; +} + +void sysid::InitialTrimAndFilter( + wpi::StringMap>* data, + AnalysisManager::Settings* settings, + std::vector& positionDelays, + std::vector& velocityDelays, units::second_t& minStepTime, + units::second_t& maxStepTime, std::string_view unit) { + auto& preparedData = *data; + + // Find the maximum Step Test Duration of the dynamic tests + maxStepTime = GetMaxStepTime(preparedData); + + // Calculate Velocity Threshold if it hasn't been set yet + if (settings->motionThreshold == std::numeric_limits::infinity()) { + for (auto& it : preparedData) { + auto key = it.first(); + auto& dataset = it.getValue(); + if (wpi::contains(key, "slow")) { + settings->motionThreshold = + std::min(settings->motionThreshold, + GetNoiseFloor(dataset, kNoiseMeanWindow, + [](auto&& pt) { return pt.velocity; })); + } + } + } + + for (auto& it : preparedData) { + auto key = it.first(); + auto& dataset = it.getValue(); + + // Trim quasistatic test data to remove all points where voltage is zero or + // velocity < motion threshold. + if (wpi::contains(key, "slow")) { + dataset.erase(std::remove_if(dataset.begin(), dataset.end(), + [&](const auto& pt) { + return std::abs(pt.voltage) <= 0 || + std::abs(pt.velocity) < + settings->motionThreshold; + }), + dataset.end()); + + // Confirm there's still data + if (dataset.empty()) { + throw sysid::NoQuasistaticDataError(); + } + } + + // Apply Median filter + if (IsFiltered(key) && settings->medianWindow > 1) { + ApplyMedianFilter(&dataset, settings->medianWindow); + } + + // Recalculate Accel and Cosine + PrepareMechData(&dataset, unit); + + // Trims filtered Dynamic Test Data + if (IsFiltered(key) && wpi::contains(key, "fast")) { + // Get the filtered dataset name + auto filteredKey = RemoveStr(key, "raw-"); + + // Trim Filtered Data + auto [tempMinStepTime, positionDelay, velocityDelay] = + TrimStepVoltageData(&preparedData[filteredKey], settings, minStepTime, + maxStepTime); + + positionDelays.emplace_back(positionDelay); + velocityDelays.emplace_back(velocityDelay); + + // Set the Raw Data to start at the same time as the Filtered Data + auto startTime = preparedData[filteredKey].front().timestamp; + auto rawStart = + std::find_if(preparedData[key].begin(), preparedData[key].end(), + [&](auto&& pt) { return pt.timestamp == startTime; }); + preparedData[key].erase(preparedData[key].begin(), rawStart); + + // Confirm there's still data + if (preparedData[key].empty()) { + throw sysid::NoDynamicDataError(); + } + } + } +} + +void sysid::AccelFilter(wpi::StringMap>* data) { + auto& preparedData = *data; + + // Remove points with acceleration = 0 + for (auto& it : preparedData) { + auto& dataset = it.getValue(); + + for (size_t i = 0; i < dataset.size(); i++) { + if (dataset.at(i).acceleration == 0.0) { + dataset.erase(dataset.begin() + i); + i--; + } + } + } + + // Confirm there's still data + if (std::any_of(preparedData.begin(), preparedData.end(), + [](const auto& it) { return it.getValue().empty(); })) { + throw sysid::InvalidDataError( + "Acceleration filtering has removed all data."); + } +} diff --git a/sysid/src/main/native/cpp/analysis/JSONConverter.cpp b/sysid/src/main/native/cpp/analysis/JSONConverter.cpp new file mode 100644 index 00000000000..54390349ddb --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include +#include +#include +#include + +#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()); + auto factor = ojson.at("unitsPerRotation").get(); + auto unit = ojson.at("units").get(); + + 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> data; + data.reserve(odata.size()); + + // Transfer the data. + for (auto&& pt : odata) { + data.push_back(std::array{ + 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> data; + data.reserve(odata.size()); + + // Transfer the data. + for (auto&& pt : odata) { + data.push_back(std::array{ + 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()); + auto factor = json.at("unitsPerRotation").get(); + auto unit = json.at("units").get(); + 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>>(); + 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>>(); + 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; +} diff --git a/sysid/src/main/native/cpp/analysis/OLS.cpp b/sysid/src/main/native/cpp/analysis/OLS.cpp new file mode 100644 index 00000000000..d095a485d50 --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/OLS.cpp @@ -0,0 +1,48 @@ +// 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/OLS.h" + +#include +#include + +#include +#include + +using namespace sysid; + +std::tuple, double, double> sysid::OLS( + const Eigen::MatrixXd& X, const Eigen::VectorXd& y) { + assert(X.rows() == y.rows()); + + // The linear model can be written as follows: + // y = Xβ + u, where y is the dependent observed variable, X is the matrix + // of independent variables, β is a vector of coefficients, and u is a + // vector of residuals. + + // We want to minimize u² = uᵀu = (y - Xβ)ᵀ(y - Xβ). + // β = (XᵀX)⁻¹Xᵀy + + // Calculate β that minimizes uᵀu. + Eigen::MatrixXd beta = (X.transpose() * X).llt().solve(X.transpose() * y); + + // We will now calculate R² or the coefficient of determination, which + // tells us how much of the total variation (variation in y) can be + // explained by the regression model. + + // We will first calculate the sum of the squares of the error, or the + // variation in error (SSE). + double SSE = (y - X * beta).squaredNorm(); + + int n = X.cols(); + + // Now we will calculate the total variation in y, known as SSTO. + double SSTO = ((y.transpose() * y) - (1.0 / n) * (y.transpose() * y)).value(); + + double rSquared = (SSTO - SSE) / SSTO; + double adjRSquared = 1.0 - (1.0 - rSquared) * ((n - 1.0) / (n - 3.0)); + double RMSE = std::sqrt(SSE / n); + + return {{beta.data(), beta.data() + beta.rows()}, adjRSquared, RMSE}; +} diff --git a/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp b/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp new file mode 100644 index 00000000000..58a8b309d0b --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp @@ -0,0 +1,47 @@ +// 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/SimpleMotorSim.h" + +#include +#include +#include + +using namespace sysid; + +SimpleMotorSim::SimpleMotorSim(double Ks, double Kv, double Ka, + double initialPosition, double initialVelocity) + // dx/dt = Ax + Bu + c sgn(x) + : m_A{{0.0, 1.0}, {0.0, -Kv / Ka}}, m_B{0.0, 1.0 / Ka}, m_c{0.0, -Ks / Ka} { + Reset(initialPosition, initialVelocity); +} + +void SimpleMotorSim::Update(units::volt_t voltage, units::second_t dt) { + Eigen::Vector u{voltage.value()}; + + // Given dx/dt = Ax + Bu + c sgn(x), + // x_k+1 = e^(AT) x_k + A^-1 (e^(AT) - 1) (Bu + c sgn(x)) + Eigen::Matrix Ad; + Eigen::Matrix Bd; + frc::DiscretizeAB<2, 1>(m_A, m_B, dt, &Ad, &Bd); + m_x = Ad * m_x + Bd * u + + Bd * m_B.householderQr().solve(m_c * wpi::sgn(GetVelocity())); +} + +double SimpleMotorSim::GetPosition() const { + return m_x(0); +} + +double SimpleMotorSim::GetVelocity() const { + return m_x(1); +} + +double SimpleMotorSim::GetAcceleration(units::volt_t voltage) const { + Eigen::Vector u{voltage.value()}; + return (m_A * m_x + m_B * u + m_c * wpi::sgn(GetVelocity()))(1); +} + +void SimpleMotorSim::Reset(double position, double velocity) { + m_x = Eigen::Vector{position, velocity}; +} diff --git a/sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp b/sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp new file mode 100644 index 00000000000..eebfe8c6762 --- /dev/null +++ b/sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp @@ -0,0 +1,12 @@ +// 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/TrackWidthAnalysis.h" + +#include + +double sysid::CalculateTrackWidth(double l, double r, units::radian_t accum) { + // The below comes from solving ω = (vr − vl) / 2r for 2r. + return (std::abs(r) + std::abs(l)) / std::abs(accum.value()); +} diff --git a/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp b/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp new file mode 100644 index 00000000000..ac97cdbca4d --- /dev/null +++ b/sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp @@ -0,0 +1,275 @@ +// 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/telemetry/TelemetryManager.h" + +#include +#include // for ::tolower +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "sysid/Util.h" +#include "sysid/analysis/AnalysisType.h" + +using namespace sysid; + +TelemetryManager::TelemetryManager(const Settings& settings, + wpi::Logger& logger, + nt::NetworkTableInstance instance) + : m_settings(settings), m_logger(logger), m_inst(instance) {} + +void TelemetryManager::BeginTest(std::string_view name) { + // Create a new test params instance for this test. + m_params = + TestParameters{name.starts_with("fast"), name.ends_with("forward"), + m_settings.mechanism == analysis::kDrivetrainAngular, + State::WaitingForEnable}; + + // Add this test to the list of running tests and set the running flag. + m_tests.push_back(std::string{name}); + m_isRunningTest = true; + + // Set the Voltage Command Entry + m_voltageCommand.Set((m_params.fast ? m_settings.stepVoltage + : m_settings.quasistaticRampRate) * + (m_params.forward ? 1 : -1)); + + // Set the test type + m_testType.Set(m_params.fast ? "Dynamic" : "Quasistatic"); + + // Set the rotate entry + m_rotate.Set(m_params.rotate); + + // Set the current mechanism in NT. + m_mechanism.Set(m_settings.mechanism.name); + // Set Overflow to False + m_overflowPub.Set(false); + // Set Mechanism Error to False + m_mechErrorPub.Set(false); + m_inst.Flush(); + + // Display the warning message. + for (auto&& func : m_callbacks) { + func( + "Please enable the robot in autonomous mode, and then " + "disable it " + "before it runs out of space. \n Note: The robot will " + "continue " + "to move until you disable it - It is your " + "responsibility to " + "ensure it does not hit anything!"); + } + + WPI_INFO(m_logger, "Started {} test.", m_tests.back()); +} + +void TelemetryManager::EndTest() { + // If there is no test running, this is a no-op + if (!m_isRunningTest) { + return; + } + + // Disable the running flag and store the data in the JSON. + m_isRunningTest = false; + m_data[m_tests.back()] = m_params.data; + + // Call the cancellation callbacks. + for (auto&& func : m_callbacks) { + std::string msg; + if (m_params.mechError) { + msg += + "\nERROR: The robot indicated that you are using the wrong project " + "for characterizing your mechanism. \nThis most likely means you " + "are trying to characterize a mechanism like a Drivetrain with a " + "deployed config for a General Mechanism (e.g. Arm, Flywheel, and " + "Elevator) or vice versa. Please double check your settings and " + "try again."; + } else if (!m_params.data.empty()) { + std::string units = m_settings.units; + std::transform(m_settings.units.begin(), m_settings.units.end(), + units.begin(), ::tolower); + + if (std::string_view{m_settings.mechanism.name}.starts_with( + "Drivetrain")) { + double p = (m_params.data.back()[3] - m_params.data.front()[3]) * + m_settings.unitsPerRotation; + double s = (m_params.data.back()[4] - m_params.data.front()[4]) * + m_settings.unitsPerRotation; + double g = m_params.data.back()[7] - m_params.data.front()[7]; + + msg = fmt::format( + "The left and right encoders traveled {} {} and {} {} " + "respectively.\nThe gyro angle delta was {} degrees.", + p, units, s, units, g * 180.0 / std::numbers::pi); + } else { + double p = (m_params.data.back()[2] - m_params.data.front()[2]) * + m_settings.unitsPerRotation; + msg = fmt::format("The encoder reported traveling {} {}.", p, units); + } + + if (m_params.overflow) { + msg += + "\nNOTE: the robot stopped recording data early because the entry " + "storage was exceeded."; + } + } else { + msg = "No data was detected."; + } + func(msg); + } + + // Remove previously run test from list of tests if no data was detected. + if (m_params.data.empty()) { + m_tests.pop_back(); + } + + // Send a zero command over NT. + m_voltageCommand.Set(0.0); + m_inst.Flush(); +} + +void TelemetryManager::Update() { + // If there is no test running, these is nothing to update. + if (!m_isRunningTest) { + return; + } + + // Update the NT entries that we're reading. + + int currAckNumber = m_ackNumberSub.Get(); + std::string telemetryValue; + + // Get the FMS Control Word. + for (auto tsValue : m_fmsControlData.ReadQueue()) { + uint32_t ctrl = tsValue.value; + m_params.enabled = ctrl & 0x01; + } + + // Get the string in the data field. + for (auto tsValue : m_telemetry.ReadQueue()) { + telemetryValue = tsValue.value; + } + + // Get the overflow flag + for (auto tsValue : m_overflowSub.ReadQueue()) { + m_params.overflow = tsValue.value; + } + + // Get the mechanism error flag + for (auto tsValue : m_mechErrorSub.ReadQueue()) { + m_params.mechError = tsValue.value; + } + + // Go through our state machine. + if (m_params.state == State::WaitingForEnable) { + if (m_params.enabled) { + m_params.enableStart = wpi::Now() * 1E-6; + m_params.state = State::RunningTest; + m_ackNumber = currAckNumber; + WPI_INFO(m_logger, "{}", "Transitioned to running test state."); + } + } + + if (m_params.state == State::RunningTest) { + // If for some reason we've disconnected, end the test. + if (!m_inst.IsConnected()) { + WPI_WARNING(m_logger, "{}", + "NT connection was dropped when executing the test. The test " + "has been canceled."); + EndTest(); + } + + // If the robot has disabled, then we can move on to the next step. + if (!m_params.enabled) { + m_params.disableStart = wpi::Now() * 1E-6; + m_params.state = State::WaitingForData; + WPI_INFO(m_logger, "{}", "Transitioned to waiting for data."); + } + } + + if (m_params.state == State::WaitingForData) { + double now = wpi::Now() * 1E-6; + m_voltageCommand.Set(0.0); + m_inst.Flush(); + + // Process valid data + if (!telemetryValue.empty() && m_ackNumber < currAckNumber) { + m_params.raw = std::move(telemetryValue); + m_ackNumber = currAckNumber; + } + + // We have the data that we need, so we can parse it and end the test. + if (!m_params.raw.empty() && + wpi::starts_with(m_params.raw, m_tests.back())) { + // Remove test type from start of string + m_params.raw.erase(0, m_params.raw.find(';') + 1); + + // Clean up the string -- remove spaces if there are any. + m_params.raw.erase( + std::remove_if(m_params.raw.begin(), m_params.raw.end(), ::isspace), + m_params.raw.end()); + + // Split the string into individual components. + wpi::SmallVector res; + wpi::split(m_params.raw, res, ','); + + // Convert each string to double. + std::vector values; + values.reserve(res.size()); + for (auto&& str : res) { + values.push_back(wpi::parse_float(str).value()); + } + + // Add the values to our result vector. + for (size_t i = 0; i < values.size() - m_settings.mechanism.rawDataSize; + i += m_settings.mechanism.rawDataSize) { + std::vector d(m_settings.mechanism.rawDataSize); + + std::copy_n(std::make_move_iterator(values.begin() + i), + m_settings.mechanism.rawDataSize, d.begin()); + m_params.data.push_back(std::move(d)); + } + + WPI_INFO(m_logger, + "Received data with size: {} for the {} test in {} seconds.", + m_params.data.size(), m_tests.back(), + m_params.data.back()[0] - m_params.data.front()[0]); + m_ackNumberPub.Set(++m_ackNumber); + EndTest(); + } + + // If we timed out, end the test and let the user know. + if (now - m_params.disableStart > 5.0) { + WPI_WARNING(m_logger, "{}", + "TelemetryManager did not receieve data 5 seconds after " + "completing the test..."); + EndTest(); + } + } +} + +std::string TelemetryManager::SaveJSON(std::string_view location) { + m_data["test"] = m_settings.mechanism.name; + m_data["units"] = m_settings.units; + m_data["unitsPerRotation"] = m_settings.unitsPerRotation; + m_data["sysid"] = true; + + std::string loc = fmt::format("{}/sysid_data{:%Y%m%d-%H%M%S}.json", location, + std::chrono::system_clock::now()); + + sysid::SaveFile(m_data.dump(2), std::filesystem::path{loc}); + WPI_INFO(m_logger, "Wrote JSON to: {}", loc); + + return loc; +} diff --git a/sysid/src/main/native/cpp/view/Analyzer.cpp b/sysid/src/main/native/cpp/view/Analyzer.cpp new file mode 100644 index 00000000000..32709188ca6 --- /dev/null +++ b/sysid/src/main/native/cpp/view/Analyzer.cpp @@ -0,0 +1,851 @@ +// 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/view/Analyzer.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "sysid/Util.h" +#include "sysid/analysis/AnalysisManager.h" +#include "sysid/analysis/AnalysisType.h" +#include "sysid/analysis/FeedbackControllerPreset.h" +#include "sysid/analysis/FilteringUtils.h" +#include "sysid/view/UILayout.h" + +using namespace sysid; + +Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger) + : m_location(""), m_logger(logger) { + // Fill the StringMap with preset values. + m_presets["Default"] = presets::kDefault; + m_presets["WPILib (2020-)"] = presets::kWPILibNew; + m_presets["WPILib (Pre-2020)"] = presets::kWPILibOld; + m_presets["CANCoder"] = presets::kCTRECANCoder; + m_presets["CTRE"] = presets::kCTREDefault; + m_presets["CTRE (Pro)"] = presets::kCTREProDefault; + m_presets["REV Brushless Encoder Port"] = presets::kREVNEOBuiltIn; + m_presets["REV Brushed Encoder Port"] = presets::kREVNonNEO; + m_presets["REV Data Port"] = presets::kREVNonNEO; + m_presets["Venom"] = presets::kVenom; + + ResetData(); + UpdateFeedbackGains(); +} + +void Analyzer::UpdateFeedforwardGains() { + WPI_INFO(m_logger, "{}", "Gain calc"); + try { + const auto& [ff, trackWidth] = m_manager->CalculateFeedforward(); + m_ff = std::get<0>(ff); + m_accelRSquared = std::get<1>(ff); + m_accelRMSE = std::get<2>(ff); + m_trackWidth = trackWidth; + m_settings.preset.measurementDelay = + m_settings.type == FeedbackControllerLoopType::kPosition + ? m_manager->GetPositionDelay() + : m_manager->GetVelocityDelay(); + m_conversionFactor = m_manager->GetFactor(); + PrepareGraphs(); + } catch (const sysid::InvalidDataError& e) { + m_state = AnalyzerState::kGeneralDataError; + HandleError(e.what()); + } catch (const sysid::NoQuasistaticDataError& e) { + m_state = AnalyzerState::kMotionThresholdError; + HandleError(e.what()); + } catch (const sysid::NoDynamicDataError& e) { + m_state = AnalyzerState::kTestDurationError; + HandleError(e.what()); + } catch (const AnalysisManager::FileReadingError& e) { + m_state = AnalyzerState::kFileError; + HandleError(e.what()); + } catch (const wpi::json::exception& e) { + m_state = AnalyzerState::kFileError; + HandleError(e.what()); + } catch (const std::exception& e) { + m_state = AnalyzerState::kFileError; + HandleError(e.what()); + } +} + +void Analyzer::UpdateFeedbackGains() { + if (m_ff[1] > 0 && m_ff[2] > 0) { + const auto& fb = m_manager->CalculateFeedback(m_ff); + m_timescale = units::second_t{m_ff[2] / m_ff[1]}; + m_Kp = fb.Kp; + m_Kd = fb.Kd; + } +} + +bool Analyzer::DisplayGain(const char* text, double* data, + bool readOnly = true) { + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5); + if (readOnly) { + return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G", + ImGuiInputTextFlags_ReadOnly); + } else { + return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G"); + } +} + +static void SetPosition(double beginX, double beginY, double xShift, + double yShift) { + ImGui::SetCursorPos(ImVec2(beginX + xShift * 10 * ImGui::GetFontSize(), + beginY + yShift * 1.75 * ImGui::GetFontSize())); +} + +bool Analyzer::IsErrorState() { + return m_state == AnalyzerState::kMotionThresholdError || + m_state == AnalyzerState::kTestDurationError || + m_state == AnalyzerState::kGeneralDataError || + m_state == AnalyzerState::kFileError; +} + +bool Analyzer::IsDataErrorState() { + return m_state == AnalyzerState::kMotionThresholdError || + m_state == AnalyzerState::kTestDurationError || + m_state == AnalyzerState::kGeneralDataError; +} + +void Analyzer::DisplayFileSelector() { + // Get the current width of the window. This will be used to scale + // our UI elements. + float width = ImGui::GetContentRegionAvail().x; + + // Show the file location along with an option to choose. + if (ImGui::Button("Select")) { + m_selector = std::make_unique( + "Select Data", "", + std::vector{"JSON File", SYSID_PFD_JSON_EXT}); + } + ImGui::SameLine(); + ImGui::SetNextItemWidth(width - ImGui::CalcTextSize("Select").x - + ImGui::GetFontSize() * 5); + ImGui::InputText("##location", &m_location, ImGuiInputTextFlags_ReadOnly); +} + +void Analyzer::ResetData() { + m_plot.ResetData(); + m_manager = std::make_unique(m_settings, m_logger); + m_location = ""; + m_ff = std::vector{1, 1, 1}; + UpdateFeedbackGains(); +} + +bool Analyzer::DisplayResetAndUnitOverride() { + auto type = m_manager->GetAnalysisType(); + auto unit = m_manager->GetUnit(); + + float width = ImGui::GetContentRegionAvail().x; + ImGui::SameLine(width - ImGui::CalcTextSize("Reset").x); + if (ImGui::Button("Reset")) { + ResetData(); + m_state = AnalyzerState::kWaitingForJSON; + return true; + } + + if (type == analysis::kDrivetrain) { + ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple); + if (ImGui::Combo("Dataset", &m_dataset, kDatasets, 3)) { + m_settings.dataset = + static_cast(m_dataset); + PrepareData(); + } + ImGui::SameLine(); + } else { + m_settings.dataset = + AnalysisManager::Settings::DrivetrainDataset::kCombined; + } + + ImGui::Spacing(); + ImGui::Text( + "Units: %s\n" + "Units Per Rotation: %.4f\n" + "Type: %s", + std::string(unit).c_str(), m_conversionFactor, type.name); + + if (type == analysis::kDrivetrainAngular) { + ImGui::SameLine(); + sysid::CreateTooltip( + "Here, the units and units per rotation represent what the wheel " + "positions and velocities were captured in. The track width value " + "will reflect the unit selected here. However, the Kv and Ka will " + "always be in Vs/rad and Vs^2 / rad respectively."); + } + + if (ImGui::Button("Override Units")) { + ImGui::OpenPopup("Override Units"); + } + + auto size = ImGui::GetIO().DisplaySize; + ImGui::SetNextWindowSize(ImVec2(size.x / 4, size.y * 0.2)); + if (ImGui::BeginPopupModal("Override Units")) { + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7); + ImGui::Combo("Units", &m_selectedOverrideUnit, kUnits, + IM_ARRAYSIZE(kUnits)); + unit = kUnits[m_selectedOverrideUnit]; + + if (unit == "Degrees") { + m_conversionFactor = 360.0; + } else if (unit == "Radians") { + m_conversionFactor = 2 * std::numbers::pi; + } else if (unit == "Rotations") { + m_conversionFactor = 1.0; + } + + bool isRotational = m_selectedOverrideUnit > 2; + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 7); + ImGui::InputDouble( + "Units Per Rotation", &m_conversionFactor, 0.0, 0.0, "%.4f", + isRotational ? ImGuiInputTextFlags_ReadOnly : ImGuiInputTextFlags_None); + + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + m_manager->OverrideUnits(unit, m_conversionFactor); + PrepareData(); + } + + ImGui::EndPopup(); + } + + ImGui::SameLine(); + if (ImGui::Button("Reset Units from JSON")) { + m_manager->ResetUnitsFromJSON(); + PrepareData(); + } + + return false; +} + +void Analyzer::ConfigParamsOnFileSelect() { + WPI_INFO(m_logger, "{}", "Configuring Params"); + m_stepTestDuration = m_settings.stepTestDuration.to(); + + // Estimate qp as 1/8 * units-per-rot + m_settings.lqr.qp = 0.125 * m_manager->GetFactor(); + // Estimate qv as 1/4 * max velocity = 1/4 * (12V - kS) / kV + m_settings.lqr.qv = 0.25 * (12.0 - m_ff[0]) / m_ff[1]; +} + +void Analyzer::Display() { + DisplayFileSelector(); + DisplayGraphs(); + + switch (m_state) { + case AnalyzerState::kWaitingForJSON: { + ImGui::Text( + "SysId is currently in theoretical analysis mode.\n" + "To analyze recorded test data, select a " + "data JSON."); + sysid::CreateTooltip( + "Theoretical feedback gains can be calculated from a " + "physical model of the mechanism being controlled. " + "Theoretical gains for several common mechanisms can " + "be obtained from ReCalc (https://reca.lc)."); + ImGui::Spacing(); + ImGui::Spacing(); + + ImGui::SetNextItemOpen(true, ImGuiCond_Once); + if (ImGui::CollapsingHeader("Feedforward Gains (Theoretical)")) { + float beginX = ImGui::GetCursorPosX(); + float beginY = ImGui::GetCursorPosY(); + CollectFeedforwardGains(beginX, beginY); + } + ImGui::SetNextItemOpen(true, ImGuiCond_Once); + if (ImGui::CollapsingHeader("Feedback Analysis")) { + DisplayFeedbackGains(); + } + break; + } + case AnalyzerState::kNominalDisplay: { // Allow the user to select which + // data set they want analyzed and + // add a + // reset button. Also show the units and the units per rotation. + if (DisplayResetAndUnitOverride()) { + return; + } + ImGui::Spacing(); + ImGui::Spacing(); + + ImGui::SetNextItemOpen(true, ImGuiCond_Once); + if (ImGui::CollapsingHeader("Feedforward Analysis")) { + float beginX = ImGui::GetCursorPosX(); + float beginY = ImGui::GetCursorPosY(); + DisplayFeedforwardGains(beginX, beginY); + } + ImGui::SetNextItemOpen(true, ImGuiCond_Once); + if (ImGui::CollapsingHeader("Feedback Analysis")) { + DisplayFeedbackGains(); + } + break; + } + case AnalyzerState::kFileError: { + CreateErrorPopup(m_errorPopup, m_exception); + if (!m_errorPopup) { + m_state = AnalyzerState::kWaitingForJSON; + return; + } + break; + } + case AnalyzerState::kGeneralDataError: + case AnalyzerState::kTestDurationError: + case AnalyzerState::kMotionThresholdError: { + CreateErrorPopup(m_errorPopup, m_exception); + if (DisplayResetAndUnitOverride()) { + return; + } + float beginX = ImGui::GetCursorPosX(); + float beginY = ImGui::GetCursorPosY(); + DisplayFeedforwardParameters(beginX, beginY); + break; + } + } + + // Periodic functions + try { + SelectFile(); + } catch (const AnalysisManager::FileReadingError& e) { + m_state = AnalyzerState::kFileError; + HandleError(e.what()); + } catch (const wpi::json::exception& e) { + m_state = AnalyzerState::kFileError; + HandleError(e.what()); + } +} + +void Analyzer::PrepareData() { + try { + m_manager->PrepareData(); + UpdateFeedforwardGains(); + UpdateFeedbackGains(); + } catch (const sysid::InvalidDataError& e) { + m_state = AnalyzerState::kGeneralDataError; + HandleError(e.what()); + } catch (const sysid::NoQuasistaticDataError& e) { + m_state = AnalyzerState::kMotionThresholdError; + HandleError(e.what()); + } catch (const sysid::NoDynamicDataError& e) { + m_state = AnalyzerState::kTestDurationError; + HandleError(e.what()); + } catch (const AnalysisManager::FileReadingError& e) { + m_state = AnalyzerState::kFileError; + HandleError(e.what()); + } catch (const wpi::json::exception& e) { + m_state = AnalyzerState::kFileError; + HandleError(e.what()); + } catch (const std::exception& e) { + m_state = AnalyzerState::kFileError; + HandleError(e.what()); + } +} + +void Analyzer::PrepareRawGraphs() { + if (m_manager->HasData()) { + AbortDataPrep(); + m_dataThread = std::thread([&] { + m_plot.SetRawData(m_manager->GetOriginalData(), m_manager->GetUnit(), + m_abortDataPrep); + }); + } +} + +void Analyzer::PrepareGraphs() { + if (m_manager->HasData()) { + WPI_INFO(m_logger, "{}", "Graph state"); + AbortDataPrep(); + m_dataThread = std::thread([&] { + m_plot.SetData(m_manager->GetRawData(), m_manager->GetFilteredData(), + m_manager->GetUnit(), m_ff, m_manager->GetStartTimes(), + m_manager->GetAnalysisType(), m_abortDataPrep); + }); + UpdateFeedbackGains(); + m_state = AnalyzerState::kNominalDisplay; + } +} + +void Analyzer::HandleError(std::string_view msg) { + m_exception = msg; + m_errorPopup = true; + if (m_state == AnalyzerState::kFileError) { + m_location = ""; + } + PrepareRawGraphs(); +} + +void Analyzer::DisplayGraphs() { + ImGui::SetNextWindowPos(ImVec2{kDiagnosticPlotWindowPos}, + ImGuiCond_FirstUseEver); + ImGui::SetNextWindowSize(ImVec2{kDiagnosticPlotWindowSize}, + ImGuiCond_FirstUseEver); + ImGui::Begin("Diagnostic Plots"); + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6); + if (ImGui::SliderFloat("Point Size", &m_plot.m_pointSize, 1, 2, "%.2f")) { + if (!IsErrorState()) { + PrepareGraphs(); + } else { + PrepareRawGraphs(); + } + } + + ImGui::SameLine(); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6); + const char* items[] = {"Forward", "Backward"}; + if (ImGui::Combo("Direction", &m_plot.m_direction, items, 2)) { + if (!IsErrorState()) { + PrepareGraphs(); + } else { + PrepareRawGraphs(); + } + } + + // If the plots were already loaded, store the scroll position. Else go to + // the last recorded scroll position if they have just been initialized + bool plotsLoaded = m_plot.DisplayPlots(); + if (plotsLoaded) { + if (m_prevPlotsLoaded) { + m_graphScroll = ImGui::GetScrollY(); + } else { + ImGui::SetScrollY(m_graphScroll); + } + + // If a JSON is selected + if (m_state == AnalyzerState::kNominalDisplay) { + DisplayGain("Acceleration R²", &m_accelRSquared); + CreateTooltip( + "The coefficient of determination of the OLS fit of acceleration " + "versus velocity and voltage. Acceleration is extremely noisy, " + "so this is generally quite small."); + + ImGui::SameLine(); + DisplayGain("Acceleration RMSE", &m_accelRMSE); + CreateTooltip( + "The standard deviation of the residuals from the predicted " + "acceleration." + "This can be interpreted loosely as the mean measured disturbance " + "from the \"ideal\" system equation."); + + DisplayGain("Sim velocity R²", m_plot.GetSimRSquared()); + CreateTooltip( + "The coefficient of determination the simulated velocity. " + "Velocity is much less-noisy than acceleration, so this " + "is pretty close to 1 for a decent fit."); + + ImGui::SameLine(); + DisplayGain("Sim velocity RMSE", m_plot.GetSimRMSE()); + CreateTooltip( + "The standard deviation of the residuals from the simulated velocity " + "predictions - essentially the size of the mean error of the " + "simulated model " + "in the recorded velocity units."); + } + } + m_prevPlotsLoaded = plotsLoaded; + + ImGui::End(); +} + +void Analyzer::SelectFile() { + // If the selector exists and is ready with a result, we can store it. + if (m_selector && m_selector->ready() && !m_selector->result().empty()) { + // Store the location of the file and reset the selector. + WPI_INFO(m_logger, "Opening File: {}", m_selector->result()[0]); + m_location = m_selector->result()[0]; + m_selector.reset(); + WPI_INFO(m_logger, "{}", "Opened File"); + m_manager = + std::make_unique(m_location, m_settings, m_logger); + PrepareData(); + m_dataset = 0; + m_settings.dataset = + AnalysisManager::Settings::DrivetrainDataset::kCombined; + ConfigParamsOnFileSelect(); + UpdateFeedbackGains(); + } +} + +void Analyzer::AbortDataPrep() { + if (m_dataThread.joinable()) { + m_abortDataPrep = true; + m_dataThread.join(); + m_abortDataPrep = false; + } +} + +void Analyzer::DisplayFeedforwardParameters(float beginX, float beginY) { + // Increase spacing to not run into trackwidth in the normal analyzer view + constexpr double kHorizontalOffset = 0.9; + SetPosition(beginX, beginY, kHorizontalOffset, 0); + + bool displayAll = + !IsErrorState() || m_state == AnalyzerState::kGeneralDataError; + + if (displayAll) { + // Wait for enter before refresh so double digit entries like "15" don't + // prematurely refresh with "1". That can cause display stuttering. + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + int window = m_settings.medianWindow; + if (ImGui::InputInt("Window Size", &window, 0, 0, + ImGuiInputTextFlags_EnterReturnsTrue)) { + m_settings.medianWindow = std::clamp(window, 1, 15); + PrepareData(); + } + + CreateTooltip( + "The number of samples in the velocity median " + "filter's sliding window."); + } + + if (displayAll || m_state == AnalyzerState::kMotionThresholdError) { + // Wait for enter before refresh so decimal inputs like "0.2" don't + // prematurely refresh with a velocity threshold of "0". + SetPosition(beginX, beginY, kHorizontalOffset, 1); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + double threshold = m_settings.motionThreshold; + if (ImGui::InputDouble("Velocity Threshold", &threshold, 0.0, 0.0, "%.3f", + ImGuiInputTextFlags_EnterReturnsTrue)) { + m_settings.motionThreshold = std::max(0.0, threshold); + PrepareData(); + } + CreateTooltip("Velocity data below this threshold will be ignored."); + } + + if (displayAll || m_state == AnalyzerState::kTestDurationError) { + SetPosition(beginX, beginY, kHorizontalOffset, 2); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + if (ImGui::SliderFloat("Test Duration", &m_stepTestDuration, + m_manager->GetMinStepTime().value(), + m_manager->GetMaxStepTime().value(), "%.2f")) { + m_settings.stepTestDuration = units::second_t{m_stepTestDuration}; + PrepareData(); + } + } +} + +void Analyzer::CollectFeedforwardGains(float beginX, float beginY) { + SetPosition(beginX, beginY, 0, 0); + if (DisplayGain("Kv", &m_ff[1], false)) { + UpdateFeedbackGains(); + } + + SetPosition(beginX, beginY, 0, 1); + if (DisplayGain("Ka", &m_ff[2], false)) { + UpdateFeedbackGains(); + } + + SetPosition(beginX, beginY, 0, 2); + // Show Timescale + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + DisplayGain("Response Timescale (ms)", + reinterpret_cast(&m_timescale)); + CreateTooltip( + "The characteristic timescale of the system response in milliseconds. " + "Both the control loop period and total signal delay should be " + "at least 3-5 times shorter than this to optimally control the " + "system."); +} + +void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) { + SetPosition(beginX, beginY, 0, 0); + DisplayGain("Ks", &m_ff[0]); + + SetPosition(beginX, beginY, 0, 1); + DisplayGain("Kv", &m_ff[1]); + + SetPosition(beginX, beginY, 0, 2); + DisplayGain("Ka", &m_ff[2]); + + SetPosition(beginX, beginY, 0, 3); + // Show Timescale + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + DisplayGain("Response Timescale (ms)", + reinterpret_cast(&m_timescale)); + CreateTooltip( + "The characteristic timescale of the system response in milliseconds. " + "Both the control loop period and total signal delay should be " + "at least 3-5 times shorter than this to optimally control the " + "system."); + + SetPosition(beginX, beginY, 0, 4); + auto positionDelay = m_manager->GetPositionDelay(); + DisplayGain("Position Measurement Delay (ms)", + reinterpret_cast(&positionDelay)); + CreateTooltip( + "The average elapsed time between the first application of " + "voltage and the first detected change in mechanism position " + "in the step-voltage tests. This includes CAN delays, and " + "may overestimate the true delay for on-motor-controller " + "feedback loops by up to 20ms."); + + SetPosition(beginX, beginY, 0, 5); + auto velocityDelay = m_manager->GetVelocityDelay(); + DisplayGain("Velocity Measurement Delay (ms)", + reinterpret_cast(&velocityDelay)); + CreateTooltip( + "The average elapsed time between the first application of " + "voltage and the maximum calculated mechanism acceleration " + "in the step-voltage tests. This includes CAN delays, and " + "may overestimate the true delay for on-motor-controller " + "feedback loops by up to 20ms."); + + SetPosition(beginX, beginY, 0, 6); + + if (m_manager->GetAnalysisType() == analysis::kElevator) { + DisplayGain("Kg", &m_ff[3]); + } else if (m_manager->GetAnalysisType() == analysis::kArm) { + DisplayGain("Kg", &m_ff[3]); + + double offset; + auto unit = m_manager->GetUnit(); + if (unit == "Radians") { + offset = m_ff[4]; + } else if (unit == "Degrees") { + offset = m_ff[4] / std::numbers::pi * 180.0; + } else if (unit == "Rotations") { + offset = m_ff[4] / (2 * std::numbers::pi); + } + DisplayGain( + fmt::format("Angle offset to horizontal ({})", GetAbbreviation(unit)) + .c_str(), + &offset); + CreateTooltip( + "This is the angle offset which, when added to the angle measurement, " + "zeroes it out when the arm is horizontal. This is needed for the arm " + "feedforward to work."); + } else if (m_trackWidth) { + DisplayGain("Track Width", &*m_trackWidth); + } + double endY = ImGui::GetCursorPosY(); + + DisplayFeedforwardParameters(beginX, beginY); + ImGui::SetCursorPosY(endY); +} + +void Analyzer::DisplayFeedbackGains() { + // Allow the user to select a feedback controller preset. + ImGui::Spacing(); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple); + if (ImGui::Combo("Gain Preset", &m_selectedPreset, kPresetNames, + IM_ARRAYSIZE(kPresetNames))) { + m_settings.preset = m_presets[kPresetNames[m_selectedPreset]]; + m_settings.type = FeedbackControllerLoopType::kVelocity; + m_selectedLoopType = + static_cast(FeedbackControllerLoopType::kVelocity); + m_settings.convertGainsToEncTicks = m_selectedPreset > 2; + UpdateFeedbackGains(); + } + ImGui::SameLine(); + sysid::CreateTooltip( + "Gain presets represent how feedback gains are calculated for your " + "specific feedback controller:\n\n" + "Default, WPILib (2020-): For use with the new WPILib PIDController " + "class.\n" + "WPILib (Pre-2020): For use with the old WPILib PIDController class.\n" + "CTRE: For use with CTRE units. These are the default units that ship " + "with CTRE motor controllers.\n" + "REV (Brushless): For use with NEO and NEO 550 motors on a SPARK MAX.\n" + "REV (Brushed): For use with brushed motors connected to a SPARK MAX."); + + if (m_settings.preset != m_presets[kPresetNames[m_selectedPreset]]) { + ImGui::SameLine(); + ImGui::TextDisabled("(modified)"); + } + + // Show our feedback controller preset values. + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + double value = m_settings.preset.outputConversionFactor * 12; + if (ImGui::InputDouble("Max Controller Output", &value, 0.0, 0.0, "%.1f") && + value > 0) { + m_settings.preset.outputConversionFactor = value / 12.0; + UpdateFeedbackGains(); + } + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + value = m_settings.preset.outputVelocityTimeFactor; + if (ImGui::InputDouble("Velocity Denominator Units (s)", &value, 0.0, 0.0, + "%.1f") && + value > 0) { + m_settings.preset.outputVelocityTimeFactor = value; + UpdateFeedbackGains(); + } + + sysid::CreateTooltip( + "This represents the denominator of the velocity unit used by the " + "feedback controller. For example, CTRE uses 100 ms = 0.1 s."); + + auto ShowPresetValue = [](const char* text, double* data, + float cursorX = 0.0f) { + if (cursorX > 0) { + ImGui::SetCursorPosX(cursorX); + } + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G"); + }; + + // Show controller period. + if (ShowPresetValue("Controller Period (ms)", + reinterpret_cast(&m_settings.preset.period))) { + if (m_settings.preset.period > 0_s && + m_settings.preset.measurementDelay >= 0_s) { + UpdateFeedbackGains(); + } + } + + // Show whether the controller gains are time-normalized. + if (ImGui::Checkbox("Time-Normalized?", &m_settings.preset.normalized)) { + UpdateFeedbackGains(); + } + + // Show position/velocity measurement delay. + if (ShowPresetValue( + "Measurement Delay (ms)", + reinterpret_cast(&m_settings.preset.measurementDelay))) { + if (m_settings.preset.period > 0_s && + m_settings.preset.measurementDelay >= 0_s) { + UpdateFeedbackGains(); + } + } + + sysid::CreateTooltip( + "The average measurement delay of the process variable in milliseconds. " + "This may depend on your encoder settings and choice of motor " + "controller. Default velocity filtering windows are quite long " + "on many motor controllers, so be careful that this value is " + "accurate if the characteristic timescale of the mechanism " + "is small."); + + // Add CPR and Gearing for converting Feedback Gains + ImGui::Separator(); + ImGui::Spacing(); + + if (ImGui::Checkbox("Convert Gains to Encoder Counts", + &m_settings.convertGainsToEncTicks)) { + UpdateFeedbackGains(); + } + sysid::CreateTooltip( + "Whether the feedback gains should be in terms of encoder counts or " + "output units. Because smart motor controllers usually don't have " + "direct access to the output units (i.e. m/s for a drivetrain), they " + "perform feedback on the encoder counts directly. If you are using a " + "PID Controller on the RoboRIO, you are probably performing feedback " + "on the output units directly.\n\nNote that if you have properly set " + "up position and velocity conversion factors with the SPARK MAX, you " + "can leave this box unchecked. The motor controller will perform " + "feedback on the output directly."); + + if (m_settings.convertGainsToEncTicks) { + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5); + if (ImGui::InputDouble("##Numerator", &m_gearingNumerator, 0.0, 0.0, "%.4f", + ImGuiInputTextFlags_EnterReturnsTrue) && + m_gearingNumerator > 0) { + m_settings.gearing = m_gearingNumerator / m_gearingDenominator; + UpdateFeedbackGains(); + } + ImGui::SameLine(); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5); + if (ImGui::InputDouble("##Denominator", &m_gearingDenominator, 0.0, 0.0, + "%.4f", ImGuiInputTextFlags_EnterReturnsTrue) && + m_gearingDenominator > 0) { + m_settings.gearing = m_gearingNumerator / m_gearingDenominator; + UpdateFeedbackGains(); + } + sysid::CreateTooltip( + "The gearing between the encoder and the motor shaft (# of encoder " + "turns / # of motor shaft turns)."); + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5); + if (ImGui::InputInt("CPR", &m_settings.cpr, 0, 0, + ImGuiInputTextFlags_EnterReturnsTrue) && + m_settings.cpr > 0) { + UpdateFeedbackGains(); + } + sysid::CreateTooltip( + "The counts per rotation of your encoder. This is the number of counts " + "reported in user code when the encoder is rotated exactly once. Some " + "common values for various motors/encoders are:\n\n" + "Falcon 500: 2048\nNEO: 1\nCTRE Mag Encoder / CANCoder: 4096\nREV " + "Through Bore Encoder: 8192\n"); + } + + ImGui::Separator(); + ImGui::Spacing(); + + // Allow the user to select a loop type. + ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple); + if (ImGui::Combo("Loop Type", &m_selectedLoopType, kLoopTypes, + IM_ARRAYSIZE(kLoopTypes))) { + m_settings.type = + static_cast(m_selectedLoopType); + if (m_state == AnalyzerState::kWaitingForJSON) { + m_settings.preset.measurementDelay = 0_ms; + } else { + if (m_settings.type == FeedbackControllerLoopType::kPosition) { + m_settings.preset.measurementDelay = m_manager->GetPositionDelay(); + } else { + m_settings.preset.measurementDelay = m_manager->GetVelocityDelay(); + } + } + UpdateFeedbackGains(); + } + + ImGui::Spacing(); + + // Show Kp and Kd. + float beginY = ImGui::GetCursorPosY(); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + DisplayGain("Kp", &m_Kp); + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4); + DisplayGain("Kd", &m_Kd); + + // Come back to the starting y pos. + ImGui::SetCursorPosY(beginY); + + if (m_selectedLoopType == 0) { + std::string unit; + if (m_state != AnalyzerState::kWaitingForJSON) { + unit = fmt::format(" ({})", GetAbbreviation(m_manager->GetUnit())); + } + + ImGui::SetCursorPosX(ImGui::GetFontSize() * 9); + if (DisplayGain(fmt::format("Max Position Error{}", unit).c_str(), + &m_settings.lqr.qp, false)) { + if (m_settings.lqr.qp > 0) { + UpdateFeedbackGains(); + } + } + } + + std::string unit; + if (m_state != AnalyzerState::kWaitingForJSON) { + unit = fmt::format(" ({}/s)", GetAbbreviation(m_manager->GetUnit())); + } + + ImGui::SetCursorPosX(ImGui::GetFontSize() * 9); + if (DisplayGain(fmt::format("Max Velocity Error{}", unit).c_str(), + &m_settings.lqr.qv, false)) { + if (m_settings.lqr.qv > 0) { + UpdateFeedbackGains(); + } + } + ImGui::SetCursorPosX(ImGui::GetFontSize() * 9); + if (DisplayGain("Max Control Effort (V)", &m_settings.lqr.r, false)) { + if (m_settings.lqr.r > 0) { + UpdateFeedbackGains(); + } + } +} diff --git a/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp b/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp new file mode 100644 index 00000000000..d8afbaf06c0 --- /dev/null +++ b/sysid/src/main/native/cpp/view/AnalyzerPlot.cpp @@ -0,0 +1,531 @@ +// 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/view/AnalyzerPlot.h" + +#include +#include +#include + +#include +#include + +#include "sysid/Util.h" +#include "sysid/analysis/AnalysisManager.h" +#include "sysid/analysis/ArmSim.h" +#include "sysid/analysis/ElevatorSim.h" +#include "sysid/analysis/FilteringUtils.h" +#include "sysid/analysis/SimpleMotorSim.h" + +using namespace sysid; + +static ImPlotPoint Getter(int idx, void* data) { + return static_cast(data)[idx]; +} + +template +static std::vector> PopulateTimeDomainSim( + const std::vector& data, + const std::array& startTimes, size_t step, Model model, + double* simSquaredErrorSum, double* squaredVariationSum, + int* timeSeriesPoints) { + // Create the vector of ImPlotPoints that will contain our simulated data. + std::vector> pts; + std::vector tmp; + + auto startTime = data[0].timestamp; + + tmp.emplace_back(startTime.value(), data[0].velocity); + + model.Reset(data[0].position, data[0].velocity); + units::second_t t = 0_s; + + for (size_t i = 1; i < data.size(); ++i) { + const auto& now = data[i]; + const auto& pre = data[i - 1]; + + t += now.timestamp - pre.timestamp; + + // If the current time stamp and previous time stamp are across a test's + // start timestamp, it is the start of a new test and the model needs to be + // reset. + if (std::find(startTimes.begin(), startTimes.end(), now.timestamp) != + startTimes.end()) { + pts.emplace_back(std::move(tmp)); + model.Reset(now.position, now.velocity); + continue; + } + + model.Update(units::volt_t{pre.voltage}, now.timestamp - pre.timestamp); + tmp.emplace_back((startTime + t).value(), model.GetVelocity()); + *simSquaredErrorSum += std::pow(now.velocity - model.GetVelocity(), 2); + *squaredVariationSum += std::pow(now.velocity, 2); + ++(*timeSeriesPoints); + } + + pts.emplace_back(std::move(tmp)); + return pts; +} + +AnalyzerPlot::AnalyzerPlot(wpi::Logger& logger) : m_logger(logger) {} + +void AnalyzerPlot::SetRawTimeData(const std::vector& rawSlow, + const std::vector& rawFast, + std::atomic& abort) { + auto rawSlowStep = std::ceil(rawSlow.size() * 1.0 / kMaxSize * 4); + auto rawFastStep = std::ceil(rawFast.size() * 1.0 / kMaxSize * 4); + // Populate Raw Slow Time Series Data + for (size_t i = 0; i < rawSlow.size(); i += rawSlowStep) { + if (abort) { + return; + } + m_quasistaticData.rawData.emplace_back((rawSlow[i].timestamp).value(), + rawSlow[i].velocity); + } + + // Populate Raw fast Time Series Data + for (size_t i = 0; i < rawFast.size(); i += rawFastStep) { + if (abort) { + return; + } + m_dynamicData.rawData.emplace_back((rawFast[i].timestamp).value(), + rawFast[i].velocity); + } +} + +void AnalyzerPlot::ResetData() { + m_quasistaticData.Clear(); + m_dynamicData.Clear(); + m_regressionData.Clear(); + m_timestepData.Clear(); + + FitPlots(); +} + +void AnalyzerPlot::SetGraphLabels(std::string_view unit) { + std::string_view abbreviation = GetAbbreviation(unit); + m_velocityLabel = fmt::format("Velocity ({}/s)", abbreviation); + m_accelerationLabel = fmt::format("Acceleration ({}/s²)", abbreviation); + m_velPortionAccelLabel = + fmt::format("Velocity-Portion Accel ({}/s²)", abbreviation); +} + +void AnalyzerPlot::SetRawData(const Storage& data, std::string_view unit, + std::atomic& abort) { + const auto& [slowForward, slowBackward, fastForward, fastBackward] = data; + const auto& slow = m_direction == 0 ? slowForward : slowBackward; + const auto& fast = m_direction == 0 ? fastForward : fastBackward; + + SetGraphLabels(unit); + + std::scoped_lock lock(m_mutex); + + ResetData(); + SetRawTimeData(slow, fast, abort); +} + +void AnalyzerPlot::SetData(const Storage& rawData, const Storage& filteredData, + std::string_view unit, + const std::vector& ffGains, + const std::array& startTimes, + AnalysisType type, std::atomic& abort) { + double simSquaredErrorSum = 0; + double squaredVariationSum = 0; + int timeSeriesPoints = 0; + + const auto& Ks = ffGains[0]; + const auto& Kv = ffGains[1]; + const auto& Ka = ffGains[2]; + + auto& [slowForward, slowBackward, fastForward, fastBackward] = filteredData; + auto& [rawSlowForward, rawSlowBackward, rawFastForward, rawFastBackward] = + rawData; + + const auto slow = AnalysisManager::DataConcat(slowForward, slowBackward); + const auto fast = AnalysisManager::DataConcat(fastForward, fastBackward); + const auto rawSlow = + AnalysisManager::DataConcat(rawSlowForward, rawSlowBackward); + const auto rawFast = + AnalysisManager::DataConcat(rawFastForward, rawFastBackward); + + SetGraphLabels(unit); + + std::scoped_lock lock(m_mutex); + + ResetData(); + + // Calculate step sizes to ensure that we only use the memory that we + // allocated. + auto slowStep = std::ceil(slow.size() * 1.0 / kMaxSize * 4); + auto fastStep = std::ceil(fast.size() * 1.0 / kMaxSize * 4); + + units::second_t dtMean = GetMeanTimeDelta(filteredData); + + // Velocity-vs-time plots + { + const auto& slow = m_direction == 0 ? slowForward : slowBackward; + const auto& fast = m_direction == 0 ? fastForward : fastBackward; + const auto& rawSlow = m_direction == 0 ? rawSlowForward : rawSlowBackward; + const auto& rawFast = m_direction == 0 ? rawFastForward : rawFastBackward; + + // Populate quasistatic time-domain graphs + for (size_t i = 0; i < slow.size(); i += slowStep) { + if (abort) { + return; + } + + m_quasistaticData.filteredData.emplace_back((slow[i].timestamp).value(), + slow[i].velocity); + + if (i > 0) { + // If the current timestamp is not in the startTimes array, it is the + // during a test and should be included. If it is in the startTimes + // array, it is the beginning of a new test and the dt will be inflated. + // Therefore we skip those to exclude that dt and effectively reset dt + // calculations. + if (slow[i].dt > 0_s && + std::find(startTimes.begin(), startTimes.end(), + slow[i].timestamp) == startTimes.end()) { + m_timestepData.data.emplace_back( + (slow[i].timestamp).value(), + units::millisecond_t{slow[i].dt}.value()); + } + } + } + + // Populate dynamic time-domain graphs + for (size_t i = 0; i < fast.size(); i += fastStep) { + if (abort) { + return; + } + + m_dynamicData.filteredData.emplace_back((fast[i].timestamp).value(), + fast[i].velocity); + + if (i > 0) { + // If the current timestamp is not in the startTimes array, it is the + // during a test and should be included. If it is in the startTimes + // array, it is the beginning of a new test and the dt will be inflated. + // Therefore we skip those to exclude that dt and effectively reset dt + // calculations. + if (fast[i].dt > 0_s && + std::find(startTimes.begin(), startTimes.end(), + fast[i].timestamp) == startTimes.end()) { + m_timestepData.data.emplace_back( + (fast[i].timestamp).value(), + units::millisecond_t{fast[i].dt}.value()); + } + } + } + + SetRawTimeData(rawSlow, rawFast, abort); + + // Populate simulated time domain data + if (type == analysis::kElevator) { + const auto& Kg = ffGains[3]; + m_quasistaticData.simData = PopulateTimeDomainSim( + rawSlow, startTimes, fastStep, sysid::ElevatorSim{Ks, Kv, Ka, Kg}, + &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints); + m_dynamicData.simData = PopulateTimeDomainSim( + rawFast, startTimes, fastStep, sysid::ElevatorSim{Ks, Kv, Ka, Kg}, + &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints); + } else if (type == analysis::kArm) { + const auto& Kg = ffGains[3]; + const auto& offset = ffGains[4]; + m_quasistaticData.simData = PopulateTimeDomainSim( + rawSlow, startTimes, fastStep, sysid::ArmSim{Ks, Kv, Ka, Kg, offset}, + &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints); + m_dynamicData.simData = PopulateTimeDomainSim( + rawFast, startTimes, fastStep, sysid::ArmSim{Ks, Kv, Ka, Kg, offset}, + &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints); + } else { + m_quasistaticData.simData = PopulateTimeDomainSim( + rawSlow, startTimes, fastStep, sysid::SimpleMotorSim{Ks, Kv, Ka}, + &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints); + m_dynamicData.simData = PopulateTimeDomainSim( + rawFast, startTimes, fastStep, sysid::SimpleMotorSim{Ks, Kv, Ka}, + &simSquaredErrorSum, &squaredVariationSum, &timeSeriesPoints); + } + } + + // Acceleration-vs-velocity plot + + // Find minimum velocity of slow and fast datasets, then find point for line + // of best fit + auto slowMinVel = + std::min_element(slow.cbegin(), slow.cend(), [](auto& a, auto& b) { + return a.velocity < b.velocity; + })->velocity; + auto fastMinVel = + std::min_element(fast.cbegin(), fast.cend(), [](auto& a, auto& b) { + return a.velocity < b.velocity; + })->velocity; + auto minVel = std::min(slowMinVel, fastMinVel); + m_regressionData.fitLine[0] = ImPlotPoint{minVel, -Kv / Ka * minVel}; + + // Find maximum velocity of slow and fast datasets, then find point for line + // of best fit + auto slowMaxVel = + std::max_element(slow.cbegin(), slow.cend(), [](auto& a, auto& b) { + return a.velocity < b.velocity; + })->velocity; + auto fastMaxVel = + std::max_element(fast.cbegin(), fast.cend(), [](auto& a, auto& b) { + return a.velocity < b.velocity; + })->velocity; + auto maxVel = std::max(slowMaxVel, fastMaxVel); + m_regressionData.fitLine[1] = ImPlotPoint{maxVel, -Kv / Ka * maxVel}; + + // Populate acceleration vs velocity graph + for (size_t i = 0; i < slow.size(); i += slowStep) { + if (abort) { + return; + } + + // Calculate portion of acceleration caused by back-EMF + double accelPortion = slow[i].acceleration - 1.0 / Ka * slow[i].voltage + + std::copysign(Ks / Ka, slow[i].velocity); + + if (type == analysis::kElevator) { + const auto& Kg = ffGains[3]; + accelPortion -= Kg / Ka; + } else if (type == analysis::kArm) { + const auto& Kg = ffGains[3]; + accelPortion -= Kg / Ka * slow[i].cos; + } + + m_regressionData.data.emplace_back(slow[i].velocity, accelPortion); + } + for (size_t i = 0; i < fast.size(); i += fastStep) { + if (abort) { + return; + } + + // Calculate portion of voltage that corresponds to change in acceleration. + double accelPortion = fast[i].acceleration - 1.0 / Ka * fast[i].voltage + + std::copysign(Ks / Ka, fast[i].velocity); + + if (type == analysis::kElevator) { + const auto& Kg = ffGains[3]; + accelPortion -= Kg / Ka; + } else if (type == analysis::kArm) { + const auto& Kg = ffGains[3]; + accelPortion -= Kg / Ka * fast[i].cos; + } + + m_regressionData.data.emplace_back(fast[i].velocity, accelPortion); + } + + // Timestep-vs-time plot + + for (size_t i = 0; i < slow.size(); i += slowStep) { + if (i > 0) { + // If the current timestamp is not in the startTimes array, it is the + // during a test and should be included. If it is in the startTimes + // array, it is the beginning of a new test and the dt will be inflated. + // Therefore we skip those to exclude that dt and effectively reset dt + // calculations. + if (slow[i].dt > 0_s && + std::find(startTimes.begin(), startTimes.end(), slow[i].timestamp) == + startTimes.end()) { + m_timestepData.data.emplace_back( + (slow[i].timestamp).value(), + units::millisecond_t{slow[i].dt}.value()); + } + } + } + + for (size_t i = 0; i < fast.size(); i += fastStep) { + if (i > 0) { + // If the current timestamp is not in the startTimes array, it is the + // during a test and should be included. If it is in the startTimes + // array, it is the beginning of a new test and the dt will be inflated. + // Therefore we skip those to exclude that dt and effectively reset dt + // calculations. + if (fast[i].dt > 0_s && + std::find(startTimes.begin(), startTimes.end(), fast[i].timestamp) == + startTimes.end()) { + m_timestepData.data.emplace_back( + (fast[i].timestamp).value(), + units::millisecond_t{fast[i].dt}.value()); + } + } + } + + auto minTime = + units::math::min(slow.front().timestamp, fast.front().timestamp); + m_timestepData.fitLine[0] = + ImPlotPoint{minTime.value(), units::millisecond_t{dtMean}.value()}; + + auto maxTime = units::math::max(slow.back().timestamp, fast.back().timestamp); + m_timestepData.fitLine[1] = + ImPlotPoint{maxTime.value(), units::millisecond_t{dtMean}.value()}; + + // RMSE = std::sqrt(sum((x_i - x^_i)^2) / N) where sum represents the sum of + // all time series points, x_i represents the velocity at a timestep, x^_i + // represents the prediction at the timestep, and N represents the number of + // points + m_RMSE = std::sqrt(simSquaredErrorSum / timeSeriesPoints); + m_accelRSquared = + 1 - m_RMSE / std::sqrt(squaredVariationSum / timeSeriesPoints); + FitPlots(); +} + +void AnalyzerPlot::FitPlots() { + // Set the "fit" flag to true. + m_quasistaticData.fitNextPlot = true; + m_dynamicData.fitNextPlot = true; + m_regressionData.fitNextPlot = true; + m_timestepData.fitNextPlot = true; +} + +double* AnalyzerPlot::GetSimRMSE() { + return &m_RMSE; +} + +double* AnalyzerPlot::GetSimRSquared() { + return &m_accelRSquared; +} + +static void PlotSimData(std::vector>& data) { + for (auto&& pts : data) { + ImPlot::SetNextLineStyle(IMPLOT_AUTO_COL, 1.5); + ImPlot::PlotLineG("Simulation", Getter, pts.data(), pts.size()); + } +} + +bool AnalyzerPlot::DisplayPlots() { + std::unique_lock lock(m_mutex, std::defer_lock); + + if (!lock.try_lock()) { + ImGui::Text("Loading %c", + "|/-\\"[static_cast(ImGui::GetTime() / 0.05f) & 3]); + return false; + } + + ImVec2 plotSize = ImGui::GetContentRegionAvail(); + + // Fit two plots horizontally + plotSize.x = (plotSize.x - ImGui::GetStyle().ItemSpacing.x) / 2.f; + + // Fit two plots vertically while leaving room for three text boxes + const float textBoxHeight = ImGui::GetFontSize() * 1.75; + plotSize.y = + (plotSize.y - textBoxHeight * 3 - ImGui::GetStyle().ItemSpacing.y) / 2.f; + + m_quasistaticData.Plot("Quasistatic Velocity vs. Time", plotSize, + m_velocityLabel.c_str(), m_pointSize); + ImGui::SameLine(); + m_dynamicData.Plot("Dynamic Velocity vs. Time", plotSize, + m_velocityLabel.c_str(), m_pointSize); + + m_regressionData.Plot("Acceleration vs. Velocity", plotSize, + m_velocityLabel.c_str(), m_velPortionAccelLabel.c_str(), + true, true, m_pointSize); + ImGui::SameLine(); + m_timestepData.Plot("Timesteps vs. Time", plotSize, "Time (s)", + "Timestep duration (ms)", true, false, m_pointSize, + [] { ImPlot::SetupAxisLimits(ImAxis_Y1, 0, 50); }); + + return true; +} + +AnalyzerPlot::FilteredDataVsTimePlot::FilteredDataVsTimePlot() { + rawData.reserve(kMaxSize); + filteredData.reserve(kMaxSize); + simData.reserve(kMaxSize); +} + +void AnalyzerPlot::FilteredDataVsTimePlot::Plot(const char* title, + const ImVec2& size, + const char* yLabel, + float pointSize) { + // Generate Sim vs Filtered Plot + if (fitNextPlot) { + ImPlot::SetNextAxesToFit(); + } + + if (ImPlot::BeginPlot(title, size)) { + ImPlot::SetupAxis(ImAxis_X1, "Time (s)", ImPlotAxisFlags_NoGridLines); + ImPlot::SetupAxis(ImAxis_Y1, yLabel, ImPlotAxisFlags_NoGridLines); + ImPlot::SetupLegend(ImPlotLocation_NorthEast); + + // Plot Raw Data + ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0); + ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize); + ImPlot::PlotScatterG("Raw Data", Getter, rawData.data(), rawData.size()); + + // Plot Filtered Data after Raw data + ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0); + ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize); + ImPlot::PlotScatterG("Filtered Data", Getter, filteredData.data(), + filteredData.size()); + + // Plot Simulation Data for Velocity Data + PlotSimData(simData); + + // Disable constant resizing + if (fitNextPlot) { + fitNextPlot = false; + } + + ImPlot::EndPlot(); + } +} + +void AnalyzerPlot::FilteredDataVsTimePlot::Clear() { + rawData.clear(); + filteredData.clear(); + simData.clear(); +} + +AnalyzerPlot::DataWithFitLinePlot::DataWithFitLinePlot() { + data.reserve(kMaxSize); +} + +void AnalyzerPlot::DataWithFitLinePlot::Plot(const char* title, + const ImVec2& size, + const char* xLabel, + const char* yLabel, bool fitX, + bool fitY, float pointSize, + std::function setup) { + if (fitNextPlot) { + if (fitX && fitY) { + ImPlot::SetNextAxesToFit(); + } else if (fitX && !fitY) { + ImPlot::SetNextAxisToFit(ImAxis_X1); + } else if (!fitX && fitY) { + ImPlot::SetNextAxisToFit(ImAxis_Y1); + } + } + + if (ImPlot::BeginPlot(title, size)) { + setup(); + ImPlot::SetupAxis(ImAxis_X1, xLabel, ImPlotAxisFlags_NoGridLines); + ImPlot::SetupAxis(ImAxis_Y1, yLabel, ImPlotAxisFlags_NoGridLines); + ImPlot::SetupLegend(ImPlotLocation_NorthEast); + + // Get a reference to the data that we are plotting. + ImPlot::SetNextMarkerStyle(IMPLOT_AUTO, 1, IMPLOT_AUTO_COL, 0); + ImPlot::SetNextMarkerStyle(ImPlotStyleVar_MarkerSize, pointSize); + ImPlot::PlotScatterG("Filtered Data", Getter, data.data(), data.size()); + + ImPlot::SetNextLineStyle(IMPLOT_AUTO_COL, 1.5); + ImPlot::PlotLineG("Fit", Getter, fitLine.data(), fitLine.size()); + + ImPlot::EndPlot(); + + if (fitNextPlot) { + fitNextPlot = false; + } + } +} + +void AnalyzerPlot::DataWithFitLinePlot::Clear() { + data.clear(); + + // Reset line of best fit + fitLine[0] = ImPlotPoint{0, 0}; + fitLine[1] = ImPlotPoint{0, 0}; +} diff --git a/sysid/src/main/native/cpp/view/JSONConverter.cpp b/sysid/src/main/native/cpp/view/JSONConverter.cpp new file mode 100644 index 00000000000..88eaa6a02f0 --- /dev/null +++ b/sysid/src/main/native/cpp/view/JSONConverter.cpp @@ -0,0 +1,64 @@ +// 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 "sysid/view/JSONConverter.h" + +#include + +#include +#include +#include + +#include "sysid/Util.h" + +using namespace sysid; + +void JSONConverter::DisplayConverter( + const char* tooltip, + std::function converter) { + if (ImGui::Button(tooltip)) { + m_opener = std::make_unique( + tooltip, "", std::vector{"JSON File", SYSID_PFD_JSON_EXT}); + } + + if (m_opener && m_opener->ready()) { + if (!m_opener->result().empty()) { + m_location = m_opener->result()[0]; + try { + converter(m_location, m_logger); + m_timestamp = wpi::Now() * 1E-6; + } catch (const std::exception& e) { + ImGui::OpenPopup("Exception Caught!"); + m_exception = e.what(); + } + } + m_opener.reset(); + } + + if (wpi::Now() * 1E-6 - m_timestamp < 5) { + ImGui::SameLine(); + ImGui::Text("Saved!"); + } + + // Handle exceptions. + ImGui::SetNextWindowSize(ImVec2(480.f, 0.0f)); + if (ImGui::BeginPopupModal("Exception Caught!")) { + ImGui::PushTextWrapPos(0.0f); + ImGui::Text( + "An error occurred when parsing the JSON. This most likely means that " + "the JSON data is incorrectly formatted."); + ImGui::TextColored(ImVec4(1.0f, 0.4f, 0.4f, 1.0f), "%s", + m_exception.c_str()); + ImGui::PopTextWrapPos(); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } +} + +void JSONConverter::DisplayCSVConvert() { + DisplayConverter("Select SysId JSON", sysid::ToCSV); +} diff --git a/sysid/src/main/native/cpp/view/Logger.cpp b/sysid/src/main/native/cpp/view/Logger.cpp new file mode 100644 index 00000000000..5e7773dbd0f --- /dev/null +++ b/sysid/src/main/native/cpp/view/Logger.cpp @@ -0,0 +1,222 @@ +// 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/view/Logger.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "sysid/Util.h" +#include "sysid/analysis/AnalysisType.h" +#include "sysid/view/UILayout.h" + +using namespace sysid; + +Logger::Logger(glass::Storage& storage, wpi::Logger& logger) + : m_logger{logger}, m_ntSettings{"sysid", storage} { + wpi::gui::AddEarlyExecute([&] { m_ntSettings.Update(); }); + + m_ntSettings.EnableServerOption(false); +} + +void Logger::Display() { + // Get the current width of the window. This will be used to scale + // our UI elements. + float width = ImGui::GetContentRegionAvail().x; + + // Add team number input and apply button for NT connection. + m_ntSettings.Display(); + + // Reset and clear the internal manager state. + ImGui::SameLine(); + if (ImGui::Button("Reset Telemetry")) { + m_settings = TelemetryManager::Settings{}; + m_manager = std::make_unique(m_settings, m_logger); + m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]); + } + + // Add NT connection indicator. + static ImVec4 kColorDisconnected{1.0f, 0.4f, 0.4f, 1.0f}; + static ImVec4 kColorConnected{0.2f, 1.0f, 0.2f, 1.0f}; + ImGui::SameLine(); + bool ntConnected = nt::NetworkTableInstance::GetDefault().IsConnected(); + ImGui::TextColored(ntConnected ? kColorConnected : kColorDisconnected, + ntConnected ? "NT Connected" : "NT Disconnected"); + + // Create a Section for project configuration + ImGui::Separator(); + ImGui::Spacing(); + ImGui::Text("Project Parameters"); + + // Add a dropdown for mechanism type. + ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple); + + if (ImGui::Combo("Mechanism", &m_selectedType, kTypes, + IM_ARRAYSIZE(kTypes))) { + m_settings.mechanism = analysis::FromName(kTypes[m_selectedType]); + } + + // Add Dropdown for Units + ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple); + if (ImGui::Combo("Unit Type", &m_selectedUnit, kUnits, + IM_ARRAYSIZE(kUnits))) { + m_settings.units = kUnits[m_selectedUnit]; + } + + sysid::CreateTooltip( + "This is the type of units that your gains will be in. For example, if " + "you want your flywheel gains in terms of radians, then use the radians " + "unit. On the other hand, if your drivetrain will use gains in meters, " + "choose meters."); + + // Rotational units have fixed Units per rotations + m_isRotationalUnits = + (m_settings.units == "Rotations" || m_settings.units == "Degrees" || + m_settings.units == "Radians"); + if (m_settings.units == "Degrees") { + m_settings.unitsPerRotation = 360.0; + } else if (m_settings.units == "Radians") { + m_settings.unitsPerRotation = 2 * std::numbers::pi; + } else if (m_settings.units == "Rotations") { + m_settings.unitsPerRotation = 1.0; + } + + // Units Per Rotations entry + ImGui::SetNextItemWidth(ImGui::GetFontSize() * kTextBoxWidthMultiple); + ImGui::InputDouble("Units Per Rotation", &m_settings.unitsPerRotation, 0.0f, + 0.0f, "%.4f", + m_isRotationalUnits ? ImGuiInputTextFlags_ReadOnly + : ImGuiInputTextFlags_None); + sysid::CreateTooltip( + "The logger assumes that the code will be sending recorded motor shaft " + "rotations over NetworkTables. This value will then be multiplied by the " + "units per rotation to get the measurement in the units you " + "specified.\n\nFor non-rotational units (e.g. meters), this value is " + "usually the wheel diameter times pi (should not include gearing)."); + // Create a section for voltage parameters. + ImGui::Separator(); + ImGui::Spacing(); + ImGui::Text("Voltage Parameters"); + + auto CreateVoltageParameters = [this](const char* text, double* data, + float min, float max) { + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6); + ImGui::PushItemFlag(ImGuiItemFlags_Disabled, + m_manager && m_manager->IsActive()); + float value = static_cast(*data); + if (ImGui::SliderFloat(text, &value, min, max, "%.2f")) { + *data = value; + } + ImGui::PopItemFlag(); + }; + + CreateVoltageParameters("Quasistatic Ramp Rate (V/s)", + &m_settings.quasistaticRampRate, 0.10f, 0.60f); + sysid::CreateTooltip( + "This is the rate at which the voltage will increase during the " + "quasistatic test."); + + CreateVoltageParameters("Dynamic Step Voltage (V)", &m_settings.stepVoltage, + 0.0f, 10.0f); + sysid::CreateTooltip( + "This is the voltage that will be applied for the " + "dynamic voltage (acceleration) tests."); + + // Create a section for tests. + ImGui::Separator(); + ImGui::Spacing(); + ImGui::Text("Tests"); + + auto CreateTest = [this, width](const char* text, const char* itext) { + // Display buttons if we have an NT connection. + if (nt::NetworkTableInstance::GetDefault().IsConnected()) { + // Create button to run tests. + if (ImGui::Button(text)) { + // Open the warning message. + ImGui::OpenPopup("Warning"); + m_manager->BeginTest(itext); + m_opened = text; + } + if (m_opened == text && ImGui::BeginPopupModal("Warning")) { + ImGui::TextWrapped("%s", m_popupText.c_str()); + if (ImGui::Button(m_manager->IsActive() ? "End Test" : "Close")) { + m_manager->EndTest(); + ImGui::CloseCurrentPopup(); + m_opened = ""; + } + ImGui::EndPopup(); + } + } else { + // Show disabled text when there is no connection. + ImGui::TextDisabled("%s", text); + } + + // Show whether the tests were run or not. + bool run = m_manager->HasRunTest(itext); + ImGui::SameLine(width * 0.7); + ImGui::Text(run ? "Run" : "Not Run"); + }; + + CreateTest("Quasistatic Forward", "slow-forward"); + CreateTest("Quasistatic Backward", "slow-backward"); + CreateTest("Dynamic Forward", "fast-forward"); + CreateTest("Dynamic Backward", "fast-backward"); + + m_manager->RegisterDisplayCallback( + [this](const auto& str) { m_popupText = str; }); + + // Display the path to where the JSON will be saved and a button to select the + // location. + ImGui::Separator(); + ImGui::Spacing(); + ImGui::Text("Save Location"); + if (ImGui::Button("Choose")) { + m_selector = std::make_unique("Select Folder"); + } + ImGui::SameLine(); + ImGui::InputText("##savelocation", &m_jsonLocation, + ImGuiInputTextFlags_ReadOnly); + + // Add button to save. + ImGui::SameLine(width * 0.9); + if (ImGui::Button("Save")) { + try { + m_manager->SaveJSON(m_jsonLocation); + } catch (const std::exception& e) { + ImGui::OpenPopup("Exception Caught!"); + m_exception = e.what(); + } + } + + // Handle exceptions. + if (ImGui::BeginPopupModal("Exception Caught!")) { + ImGui::Text("%s", m_exception.c_str()); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + // Run periodic methods. + SelectDataFolder(); + m_ntSettings.Update(); + m_manager->Update(); +} + +void Logger::SelectDataFolder() { + // If the selector exists and is ready with a result, we can store it. + if (m_selector && m_selector->ready()) { + m_jsonLocation = m_selector->result(); + m_selector.reset(); + } +} diff --git a/sysid/src/main/native/include/sysid/Util.h b/sysid/src/main/native/include/sysid/Util.h new file mode 100644 index 00000000000..38cf8b2e42f --- /dev/null +++ b/sysid/src/main/native/include/sysid/Util.h @@ -0,0 +1,89 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include + +// The generated AppleScript by portable-file-dialogs for just *.json does not +// work correctly because it is a uniform type identifier. This means that +// "public." needs to be prepended to it. +#ifdef __APPLE__ +#define SYSID_PFD_JSON_EXT "*.public.json" +#else +#define SYSID_PFD_JSON_EXT "*.json" +#endif + +#ifdef _WIN32 +#define LAUNCH "gradlew" +#define LAUNCH_DETACHED "start /b gradlew" +#define DETACHED_SUFFIX "" +#else +#define LAUNCH "./gradlew" +#define LAUNCH_DETACHED "./gradlew" +#define DETACHED_SUFFIX "&" +#endif + +// Based on https://gcc.gnu.org/onlinedocs/cpp/Stringizing.html +#define EXPAND_STRINGIZE(s) STRINGIZE(s) +#define STRINGIZE(s) #s + +namespace sysid { +static constexpr const char* kUnits[] = {"Meters", "Feet", "Inches", + "Radians", "Rotations", "Degrees"}; + +/** + * Displays a tooltip beside the widget that this method is called after with + * the provided text. + * + * @param text The text to show in the tooltip. + */ +void CreateTooltip(const char* text); + +/** + * Utility function to launch an error popup if an exception is detected. + * + * @param isError True if an exception is detected + * @param errorMessage The error message associated with the exception + */ +void CreateErrorPopup(bool& isError, std::string_view errorMessage); + +/** + * Returns the abbreviation for the unit. + * + * @param unit The unit to return the abbreviation for. + * @return The abbreviation for the unit. + */ +std::string_view GetAbbreviation(std::string_view unit); + +/** + * Saves a file with the provided contents to a specified location. + * + * @param contents The file contents. + * @param path The file location. + */ +void SaveFile(std::string_view contents, const std::filesystem::path& path); + +/** + * Concatenates all the arrays passed as arguments and returns the result. + * + * @tparam Type The array element type. + * @tparam Sizes The array sizes. + * @param arrays Parameter pack of arrays to concatenate. + */ +template +constexpr auto ArrayConcat(const std::array&... arrays) { + std::array result; + size_t index = 0; + + ((std::copy_n(arrays.begin(), Sizes, result.begin() + index), index += Sizes), + ...); + + return result; +} +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h new file mode 100644 index 00000000000..d572578b011 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/AnalysisManager.h @@ -0,0 +1,358 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "sysid/analysis/AnalysisType.h" +#include "sysid/analysis/FeedbackAnalysis.h" +#include "sysid/analysis/FeedbackControllerPreset.h" +#include "sysid/analysis/FeedforwardAnalysis.h" +#include "sysid/analysis/Storage.h" + +namespace sysid { + +/** + * Manages analysis of data. Each instance of this class represents a JSON file + * that is read from storage. + */ +class AnalysisManager { + public: + /** + * Represents settings for an instance of the analysis manager. This contains + * information about the feedback controller preset, loop type, motion + * threshold, acceleration window size, LQR parameters, and the selected + * dataset. + */ + struct Settings { + enum class DrivetrainDataset { kCombined = 0, kLeft = 1, kRight = 2 }; + /** + * The feedback controller preset used to calculate gains. + */ + FeedbackControllerPreset preset = presets::kDefault; + + /** + * The feedback controller loop type (position or velocity). + */ + FeedbackControllerLoopType type = FeedbackControllerLoopType::kVelocity; + + /** + * LQR parameters used for feedback gain calculation. + */ + LQRParameters lqr{1, 1.5, 7}; + + /** + * The motion threshold (units/s) for trimming quasistatic test data. + */ + double motionThreshold = 0.2; + + /** + * The window size for the median filter. + */ + int medianWindow = 1; + + /** + * The duration of the dynamic test that should be considered. A value of + * zero indicates it needs to be set to the default. + */ + units::second_t stepTestDuration = 0_s; + + /** + * The conversion factor of counts per revolution. + */ + int cpr = 1440; + + /** + * The conversion factor of gearing. + */ + double gearing = 1; + + /** + * Whether or not the gains should be in the encoder's units (mainly for use + * in a smart motor controller). + */ + bool convertGainsToEncTicks = false; + + DrivetrainDataset dataset = DrivetrainDataset::kCombined; + }; + + /** + * Stores feedforward. + */ + struct FeedforwardGains { + /** + * Stores the Feedforward gains. + */ + std::tuple, double, double> ffGains; + + /** + * Stores the trackwidth for angular drivetrain tests. + */ + std::optional trackWidth; + }; + + /** + * Exception for File Reading Errors. + */ + struct FileReadingError : public std::exception { + /** + * Creates a FileReadingError object + * + * @param path The path of the file attempted to open + */ + explicit FileReadingError(std::string_view path) { + msg = fmt::format("Unable to read: {}", path); + } + + /** + * The path of the file that was opened. + */ + std::string msg; + const char* what() const noexcept override { return msg.c_str(); } + }; + + /** + * The keys (which contain sysid data) that are in the JSON to analyze. + */ + static constexpr const char* kJsonDataKeys[] = { + "slow-forward", "slow-backward", "fast-forward", "fast-backward"}; + + /** + * Concatenates a list of vectors. The contents of the source vectors are + * copied (not moved) into the new vector. Also sorts the datapoints by + * timestamp to assist with future simulation. + * + * @param sources The source vectors. + * @return The concatenated vector + */ + template + static std::vector DataConcat(const Sources&... sources) { + std::vector result; + (result.insert(result.end(), sources.begin(), sources.end()), ...); + + // Sort data by timestamp to remove the possibility of negative dts in + // future simulations. + std::sort(result.begin(), result.end(), [](const auto& a, const auto& b) { + return a.timestamp < b.timestamp; + }); + + return result; + } + + /** + * Constructs an instance of the analysis manager for theoretical analysis, + * containing settings and gains but no data. + * + * @param settings The settings for this instance of the analysis manager. + * @param logger The logger instance to use for log data. + */ + AnalysisManager(Settings& settings, wpi::Logger& logger); + + /** + * Constructs an instance of the analysis manager with the given path (to the + * JSON) and analysis manager settings. + * + * @param path The path to the JSON containing the sysid data. + * @param settings The settings for this instance of the analysis manager. + * @param logger The logger instance to use for log data. + */ + AnalysisManager(std::string_view path, Settings& settings, + wpi::Logger& logger); + + /** + * Prepares data from the JSON and stores the output in Storage member + * variables. + */ + void PrepareData(); + + /** + * Calculates the gains with the latest data (from the pointers in the + * settings struct that this instance was constructed with). + * + * @return The latest feedforward gains and trackwidth (if needed). + */ + FeedforwardGains CalculateFeedforward(); + + /** + * Calculates feedback gains from the given feedforward gains. + * + * @param ff The feedforward gains. + * @return The calculated feedback gains. + */ + FeedbackGains CalculateFeedback(std::vector ff); + + /** + * Overrides the units in the JSON with the user-provided ones. + * + * @param unit The unit to output gains in. + * @param unitsPerRotation The conversion factor between rotations and the + * selected unit. + */ + void OverrideUnits(std::string_view unit, double unitsPerRotation); + + /** + * Resets the units back to those defined in the JSON. + */ + void ResetUnitsFromJSON(); + + /** + * Returns the analysis type of the current instance (read from the JSON). + * + * @return The analysis type. + */ + const AnalysisType& GetAnalysisType() const { return m_type; } + + /** + * Returns the units of analysis. + * + * @return The units of analysis. + */ + std::string_view GetUnit() const { return m_unit; } + + /** + * Returns the factor (a.k.a. units per rotation) for analysis. + * + * @return The factor (a.k.a. units per rotation) for analysis. + */ + double GetFactor() const { return m_factor; } + + /** + * Returns a reference to the iterator of the currently selected raw datset. + * Unfortunately, due to ImPlot internals, the reference cannot be const so + * the user should be careful not to change any data. + * + * @return A reference to the raw internal data. + */ + Storage& GetRawData() { + return m_rawDataset[static_cast(m_settings.dataset)]; + } + + /** + * Returns a reference to the iterator of the currently selected filtered + * datset. Unfortunately, due to ImPlot internals, the reference cannot be + * const so the user should be careful not to change any data. + * + * @return A reference to the filtered internal data. + */ + Storage& GetFilteredData() { + return m_filteredDataset[static_cast(m_settings.dataset)]; + } + + /** + * Returns the original dataset. + * + * @return The original (untouched) dataset + */ + Storage& GetOriginalData() { + return m_originalDataset[static_cast(m_settings.dataset)]; + } + + /** + * Returns the minimum duration of the Step Voltage Test of the currently + * stored data. + * + * @return The minimum step test duration. + */ + units::second_t GetMinStepTime() const { return m_minStepTime; } + + /** + * Returns the maximum duration of the Step Voltage Test of the currently + * stored data. + * + * @return Maximum step test duration + */ + units::second_t GetMaxStepTime() const { return m_maxStepTime; } + + /** + * Returns the estimated time delay of the measured position, including + * CAN delays. + * + * @return Position delay in milliseconds + */ + units::millisecond_t GetPositionDelay() const { + return std::accumulate(m_positionDelays.begin(), m_positionDelays.end(), + 0_s) / + m_positionDelays.size(); + } + + /** + * Returns the estimated time delay of the measured velocity, including + * CAN delays. + * + * @return Velocity delay in milliseconds + */ + units::millisecond_t GetVelocityDelay() const { + return std::accumulate(m_velocityDelays.begin(), m_velocityDelays.end(), + 0_s) / + m_positionDelays.size(); + } + + /** + * Returns the different start times of the recorded tests. + * + * @return The start times for each test + */ + const std::array& GetStartTimes() const { + return m_startTimes; + } + + bool HasData() const { + return !m_originalDataset[static_cast( + Settings::DrivetrainDataset::kCombined)] + .empty(); + } + + private: + wpi::Logger& m_logger; + + // This is used to store the various datasets (i.e. Combined, Forward, + // Backward, etc.) + wpi::json m_json; + + std::array m_originalDataset; + std::array m_rawDataset; + std::array m_filteredDataset; + + // Stores the various start times of the different tests. + std::array m_startTimes; + + // The settings for this instance. This contains pointers to the feedback + // controller preset, LQR parameters, acceleration window size, etc. + Settings& m_settings; + + // Miscellaneous data from the JSON -- the analysis type, the units, and the + // units per rotation. + AnalysisType m_type; + std::string m_unit; + double m_factor; + + units::second_t m_minStepTime{0}; + units::second_t m_maxStepTime{std::numeric_limits::infinity()}; + std::vector m_positionDelays; + std::vector m_velocityDelays; + + // Stores an optional track width if we are doing the drivetrain angular test. + std::optional m_trackWidth; + + void PrepareGeneralData(); + + void PrepareAngularDrivetrainData(); + + void PrepareLinearDrivetrainData(); +}; +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/AnalysisType.h b/sysid/src/main/native/include/sysid/analysis/AnalysisType.h new file mode 100644 index 00000000000..7feedb3f51a --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/AnalysisType.h @@ -0,0 +1,63 @@ +// 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. + +#pragma once + +#include +#include + +namespace sysid { + +/** + * Stores the type of Analysis and relevant information about the analysis. + */ +struct AnalysisType { + /** + * The number of independent variables for feedforward analysis. + */ + size_t independentVariables; + + /** + * The number of fields in the raw data within the mechanism's JSON. + */ + size_t rawDataSize; + + /** + * Display name for the analysis type. + */ + const char* name; + + /** + * Compares equality of two AnalysisType structs. + * + * @param rhs Another AnalysisType + * @return True if the two analysis types are equal + */ + constexpr bool operator==(const AnalysisType& rhs) const { + return std::string_view{name} == rhs.name && + independentVariables == rhs.independentVariables && + rawDataSize == rhs.rawDataSize; + } + + /** + * Compares inequality of two AnalysisType structs. + * + * @param rhs Another AnalysisType + * @return True if the two analysis types are not equal + */ + constexpr bool operator!=(const AnalysisType& rhs) const { + return !operator==(rhs); + } +}; + +namespace analysis { +constexpr AnalysisType kDrivetrain{3, 9, "Drivetrain"}; +constexpr AnalysisType kDrivetrainAngular{3, 9, "Drivetrain (Angular)"}; +constexpr AnalysisType kElevator{4, 4, "Elevator"}; +constexpr AnalysisType kArm{5, 4, "Arm"}; +constexpr AnalysisType kSimple{3, 4, "Simple"}; + +AnalysisType FromName(std::string_view name); +} // namespace analysis +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/ArmSim.h b/sysid/src/main/native/include/sysid/analysis/ArmSim.h new file mode 100644 index 00000000000..af6c10e5a34 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/ArmSim.h @@ -0,0 +1,79 @@ +// 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. + +#pragma once + +#include +#include +#include + +namespace sysid { +/** + * Simulation of an Arm mechanism based off of a model from SysId Feedforward + * gains. + */ +class ArmSim { + public: + /** + * @param Ks Static friction gain. + * @param Kv Velocity gain. + * @param Ka Acceleration gain. + * @param Kg Gravity cosine gain. + * @param offset Arm position offset. + * @param initialPosition Initial arm position. + * @param initialVelocity Initial arm velocity. + */ + ArmSim(double Ks, double Kv, double Ka, double Kg, double offset = 0.0, + double initialPosition = 0.0, double initialVelocity = 0.0); + + /** + * Simulates affine state-space system: + * dx/dt = Ax + Bu + c sgn(x) + d cos(theta) + * forward dt seconds. + * + * @param voltage Voltage to apply over the timestep. + * @param dt Sammple period. + */ + void Update(units::volt_t voltage, units::second_t dt); + + /** + * Returns the position. + * + * @return The current position + */ + double GetPosition() const; + + /** + * Returns the velocity. + * + * @return The current velocity + */ + double GetVelocity() const; + + /** + * Returns the acceleration for the current state and given input. + * + * @param voltage The voltage that is being applied to the mechanism / input + * @return The acceleration given the state and input + */ + double GetAcceleration(units::volt_t voltage) const; + + /** + * Resets model position and velocity. + * + * @param position The position the mechanism should be reset to + * @param velocity The velocity the mechanism should be reset to + */ + void Reset(double position = 0.0, double velocity = 0.0); + + private: + Eigen::Matrix m_A; + Eigen::Matrix m_B; + Eigen::Vector m_c; + Eigen::Vector m_d; + Eigen::Vector m_x; + double m_offset; +}; + +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h b/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h new file mode 100644 index 00000000000..2d0c5f68c8b --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/ElevatorSim.h @@ -0,0 +1,76 @@ +// 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. + +#pragma once + +#include +#include +#include + +namespace sysid { +/** + * Simulation of an Elevator mechanism based off of a model from SysId + * Feedforward gains. + */ +class ElevatorSim { + public: + /** + * @param Ks Static friction gain. + * @param Kv Velocity gain. + * @param Ka Acceleration gain. + * @param Kg Gravity gain. + * @param initialPosition Initial elevator position. + * @param initialVelocity Initial elevator velocity. + */ + ElevatorSim(double Ks, double Kv, double Ka, double Kg, + double initialPosition = 0.0, double initialVelocity = 0.0); + + /** + * Simulates affine state-space system dx/dt = Ax + Bu + c sgn(x) + d forward + * dt seconds. + * + * @param voltage Voltage to apply over the timestep. + * @param dt Sammple period. + */ + void Update(units::volt_t voltage, units::second_t dt); + + /** + * Returns the position. + * + * @return The current position + */ + double GetPosition() const; + + /** + * Returns the velocity. + * + * @return The current velocity + */ + double GetVelocity() const; + + /** + * Returns the acceleration for the current state and given input. + * + * @param voltage The voltage that is being applied to the mechanism / input + * @return The acceleration given the state and input + */ + double GetAcceleration(units::volt_t voltage) const; + + /** + * Resets model position and velocity. + * + * @param position The position the mechanism should be reset to + * @param velocity The velocity the mechanism should be reset to + */ + void Reset(double position = 0.0, double velocity = 0.0); + + private: + Eigen::Matrix m_A; + Eigen::Matrix m_B; + Eigen::Vector m_c; + Eigen::Vector m_d; + Eigen::Vector m_x; +}; + +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h b/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h new file mode 100644 index 00000000000..51754a241d4 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h @@ -0,0 +1,80 @@ +// 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. + +#pragma once + +namespace sysid { + +struct FeedbackControllerPreset; + +/** + * Represents parameters used to calculate optimal feedback gains using a + * linear-quadratic regulator (LQR). + */ +struct LQRParameters { + /** + * The maximum allowable deviation in position. + */ + double qp; + + /** + * The maximum allowable deviation in velocity. + */ + double qv; + + /** + * The maximum allowable control effort. + */ + double r; +}; + +/** + * Stores feedback controller gains. + */ +struct FeedbackGains { + /** + * The calculated Proportional gain + */ + double Kp; + + /** + * The calculated Derivative gain + */ + double Kd; +}; + +/** + * Calculates position feedback gains for the given controller preset, LQR + * controller gain parameters and feedforward gains. + * + * @param preset The feedback controller preset. + * @param params The parameters for calculating optimal feedback + * gains. + * @param Kv Velocity feedforward gain. + * @param Ka Acceleration feedforward gain. + * @param encFactor The factor to convert the gains from output units to + * encoder units. This is usually encoder EPR * gearing + * * units per rotation. + */ +FeedbackGains CalculatePositionFeedbackGains( + const FeedbackControllerPreset& preset, const LQRParameters& params, + double Kv, double Ka, double encFactor = 1.0); + +/** + * Calculates velocity feedback gains for the given controller preset, LQR + * controller gain parameters and feedforward gains. + * + * @param preset The feedback controller preset. + * @param params The parameters for calculating optimal feedback + * gains. + * @param Kv Velocity feedforward gain. + * @param Ka Acceleration feedforward gain. + * @param encFactor The factor to convert the gains from output units to + * encoder units. This is usually encoder EPR * gearing + * * units per rotation. + */ +FeedbackGains CalculateVelocityFeedbackGains( + const FeedbackControllerPreset& preset, const LQRParameters& params, + double Kv, double Ka, double encFactor = 1.0); +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h b/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h new file mode 100644 index 00000000000..4b13c6c38f0 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h @@ -0,0 +1,164 @@ +// 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. + +#pragma once + +#include + +namespace sysid { +/** + * Represents a preset for a specific feedback controller. This includes info + * about the max controller output, the controller period, whether the gains + * are time-normalized, and whether there are measurement delays from sensors or + * onboard filtering. + */ +struct FeedbackControllerPreset { + /** + * The conversion factor between volts and the final controller output. + */ + double outputConversionFactor; + + /** + * The conversion factor for using controller output for velocity gains. This + * is necessary as some controllers do velocity controls with different time + * units. + */ + double outputVelocityTimeFactor; + + /** + * The period at which the controller runs. + */ + units::millisecond_t period; + + /** + * Whether the controller gains are time-normalized. + */ + bool normalized; + + /** + * The measurement delay in the encoder measurements. + */ + units::millisecond_t measurementDelay; + + /** + * Checks equality between two feedback controller presets. + * + * @param rhs Another FeedbackControllerPreset + * @return If the two presets are equal + */ + constexpr bool operator==(const FeedbackControllerPreset& rhs) const { + return outputConversionFactor == rhs.outputConversionFactor && + outputVelocityTimeFactor == rhs.outputVelocityTimeFactor && + period == rhs.period && normalized == rhs.normalized && + measurementDelay == rhs.measurementDelay; + } + + /** + * Checks inequality between two feedback controller presets. + * + * @param rhs Another FeedbackControllerPreset + * @return If the two presets are not equal + */ + constexpr bool operator!=(const FeedbackControllerPreset& rhs) const { + return !operator==(rhs); + } +}; + +/** + * The loop type for the feedback controller. + */ +enum class FeedbackControllerLoopType { kPosition, kVelocity }; + +namespace presets { +constexpr FeedbackControllerPreset kDefault{1.0, 1.0, 20_ms, true, 0_s}; + +constexpr FeedbackControllerPreset kWPILibNew{kDefault}; +constexpr FeedbackControllerPreset kWPILibOld{1.0 / 12.0, 1.0, 50_ms, false, + 0_s}; + +// Measurement delay from a moving average filter: +// +// A moving average filter with a window size of N is an FIR filter with N taps. +// The average delay of a moving average filter with N taps and a period between +// samples of T is (N - 1)/2 T. +// +// Proof: +// N taps with delays of 0 .. (N - 1) T +// +// average delay = (sum 0 .. N - 1) / N T +// = (sum 1 .. N - 1) / N T +// +// note: sum 1 .. n = n(n + 1) / 2 +// +// = (N - 1)((N - 1) + 1) / (2N) T +// = (N - 1)N / (2N) T +// = (N - 1)/2 T + +// Measurement delay from a backward finite difference: +// +// Velocities calculated via a backward finite difference with a period of T +// look like: +// +// velocity = (position at timestep k - position at timestep k-1) / T +// +// The average delay is T / 2. +// +// Proof: +// average delay = (0 ms + T) / 2 +// = T / 2 + +/** + * https://phoenix-documentation.readthedocs.io/en/latest/ch14_MCSensor.html#changing-velocity-measurement-parameters + * + * Backward finite difference delay = 100 ms / 2 = 50 ms. + * + * 64-tap moving average delay = (64 - 1) / 2 * 1 ms = 31.5 ms. + * + * Total delay = 50 ms + 31.5 ms = 81.5 ms. + */ +constexpr FeedbackControllerPreset kCTRECANCoder{1.0 / 12.0, 60.0, 1_ms, true, + 81.5_ms}; +constexpr FeedbackControllerPreset kCTREDefault{1023.0 / 12.0, 0.1, 1_ms, false, + 81.5_ms}; +/** + * https://api.ctr-electronics.com/phoenixpro/release/cpp/classctre_1_1phoenixpro_1_1hardware_1_1core_1_1_core_c_a_ncoder.html#a718a1a214b58d3c4543e88e3cb51ade5 + * + * Phoenix Pro uses standard units and Voltage output. This means the output + * is 1.0, time factor is 1.0, and closed loop operates at 1 millisecond. All + * Pro devices make use of Kalman filters default-tuned to lowest latency, which + * in testing is roughly 1 millisecond + */ +constexpr FeedbackControllerPreset kCTREProDefault{1.0, 1.0, 1_ms, true, 1_ms}; + +/** + * https://github.com/wpilibsuite/sysid/issues/258#issuecomment-1010658237 + * + * 8-sample moving average with 32 ms between samples. + * + * Total delay = 8-tap moving average delay = (8 - 1) / 2 * 32 ms = 112 ms. + */ +constexpr FeedbackControllerPreset kREVNEOBuiltIn{1.0 / 12.0, 60.0, 1_ms, false, + 112_ms}; + +/** + * https://www.revrobotics.com/content/sw/max/sw-docs/cpp/classrev_1_1_c_a_n_encoder.html#a7e6ce792bc0c0558fb944771df572e6a + * + * Backward finite difference delay = 100 ms / 2 = 50 ms. + * + * 64-tap moving average delay = (64 - 1) / 2 * 1 ms = 31.5 ms. + * + * Total delay = 50 ms + 31.5 ms = 81.5 ms. + */ +constexpr FeedbackControllerPreset kREVNonNEO{1.0 / 12.0, 60.0, 1_ms, false, + 81.5_ms}; + +/** + * https://github.com/wpilibsuite/sysid/pull/138#issuecomment-841734229 + * + * Backward finite difference delay = 10 ms / 2 = 5 ms. + */ +constexpr FeedbackControllerPreset kVenom{4096.0 / 12.0, 60.0, 1_ms, false, + 5_ms}; +} // namespace presets +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h new file mode 100644 index 00000000000..fc9e47c185b --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h @@ -0,0 +1,25 @@ +// 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. + +#pragma once + +#include +#include + +#include "sysid/analysis/AnalysisType.h" +#include "sysid/analysis/Storage.h" + +namespace sysid { + +/** + * Calculates feedforward gains given the data and the type of analysis to + * perform. + * + * @return Tuple containing the coefficients of the analysis along with the + * r-squared (coefficient of determination) and RMSE (standard deviation + * of the residuals) of the fit. + */ +std::tuple, double, double> CalculateFeedforwardGains( + const Storage& data, const AnalysisType& type); +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h new file mode 100644 index 00000000000..5884b3f2d01 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/FilteringUtils.h @@ -0,0 +1,209 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "sysid/analysis/AnalysisManager.h" +#include "sysid/analysis/Storage.h" + +namespace sysid { + +constexpr int kNoiseMeanWindow = 9; + +/** + * Exception for Invalid Data Errors in which we can't pin the cause of error to + * any one specific setting of the GUI. + */ +struct InvalidDataError : public std::exception { + /** + * Creates an InvalidDataError Exception. It adds additional steps after the + * initial error message to inform users in the ways that they could fix their + * data. + * + * @param message The error message + */ + explicit InvalidDataError(std::string_view message) { + m_message = fmt::format( + "{}. Please verify that your units and data is reasonable and then " + "adjust your motion threshold, test duration, and/or window size to " + "try to fix this issue.", + message); + } + + /** + * Stores the error message + */ + std::string m_message; + const char* what() const noexcept override { return m_message.c_str(); } +}; + +/** + * Exception for Quasistatic Data being completely removed. + */ +struct NoQuasistaticDataError : public std::exception { + const char* what() const noexcept override { + return "Quasistatic test trimming removed all data. Please adjust your " + "motion threshold and double check " + "your units and test data to make sure that the robot is reporting " + "reasonable values."; + } +}; + +/** + * Exception for Dynamic Data being completely removed. + */ +struct NoDynamicDataError : public std::exception { + const char* what() const noexcept override { + return "Dynamic test trimming removed all data. Please adjust your test " + "duration and double check " + "your units and test data to make sure that the robot is reporting " + "reasonable values."; + } +}; + +/** + * Calculates the expected acceleration noise to be used as the floor of the + * Voltage Trim. This is done by taking the standard deviation from the moving + * average values of each point. + * + * @param data the prepared data vector containing acceleration data + * @param window the size of the window for the moving average + * @param accessorFunction a function that accesses the desired data from the + * PreparedData struct. + * @return The expected acceleration noise + */ +double GetNoiseFloor( + const std::vector& data, int window, + std::function accessorFunction); + +/** + * Reduces noise in velocity data by applying a median filter. + * + * @tparam S The size of the raw data array + * @tparam Velocity The index of the velocity entry in the raw data. + * @param data the vector of arrays representing sysid data (must contain + * velocity data) + * @param window the size of the window of the median filter (must be odd) + */ +void ApplyMedianFilter(std::vector* data, int window); + +/** + * Trims the step voltage data to discard all points before the maximum + * acceleration and after reaching stead-state velocity. Also trims the end of + * the test based off of user specified test durations, but it will determine a + * default duration if the requested duration is less than the minimum step test + * duration. + * + * @param data A pointer to the step voltage data. + * @param settings A pointer to the settings of an analysis manager object. + * @param minStepTime The current minimum step test duration as one of the + * trimming procedures will remove this amount from the start + * of the test. + * @param maxStepTime The maximum step test duration. + * @return The updated minimum step test duration. + */ +std::tuple +TrimStepVoltageData(std::vector* data, + AnalysisManager::Settings* settings, + units::second_t minStepTime, units::second_t maxStepTime); + +/** + * Compute the mean time delta of the given data. + * + * @param data A reference to all of the collected PreparedData + * @return The mean time delta for all the data points + */ +units::second_t GetMeanTimeDelta(const std::vector& data); + +/** + * Compute the mean time delta of the given data. + * + * @param data A reference to all of the collected PreparedData + * @return The mean time delta for all the data points + */ +units::second_t GetMeanTimeDelta(const Storage& data); + +/** + * Creates a central finite difference filter that computes the nth + * derivative of the input given the specified number of samples. + * + * Since this requires data from the future, it should only be used in offline + * filtering scenarios. + * + * For example, a first derivative filter that uses two samples and a sample + * period of 20 ms would be + * + * 

+ * CentralFiniteDifference<1, 2>(20_ms);
+ *
+ * + * @tparam Derivative The order of the derivative to compute. + * @tparam Samples The number of samples to use to compute the given + * derivative. This must be odd and one more than the order + * of derivative or higher. + * @param period The period in seconds between samples taken by the user. + */ +template +frc::LinearFilter CentralFiniteDifference(units::second_t period) { + static_assert(Samples % 2 != 0, "Number of samples must be odd."); + + // Generate stencil points from -(samples - 1)/2 to (samples - 1)/2 + wpi::array stencil{wpi::empty_array}; + for (int i = 0; i < Samples; ++i) { + stencil[i] = -(Samples - 1) / 2 + i; + } + + return frc::LinearFilter::FiniteDifference( + stencil, period); +} + +/** + * Trims the quasistatic tests, applies a median filter to the velocity data, + * calculates acceleration and cosine (arm only) data, and trims the dynamic + * tests. + * + * @param data A pointer to a data vector recently created by the + * ConvertToPrepared method + * @param settings A reference to the analysis settings + * @param positionDelays A reference to the vector of computed position signal + * delays. + * @param velocityDelays A reference to the vector of computed velocity signal + * delays. + * @param minStepTime A reference to the minimum dynamic test duration as one of + * the trimming procedures will remove this amount from the + * start of the test. + * @param maxStepTime A reference to the maximum dynamic test duration + * @param unit The angular unit that the arm test is in (only for calculating + * cosine data) + */ +void InitialTrimAndFilter(wpi::StringMap>* data, + AnalysisManager::Settings* settings, + std::vector& positionDelays, + std::vector& velocityDelays, + units::second_t& minStepTime, + units::second_t& maxStepTime, + std::string_view unit = ""); + +/** + * Removes all points with acceleration = 0. + * + * @param data A pointer to a PreparedData vector + */ +void AccelFilter(wpi::StringMap>* data); + +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/JSONConverter.h b/sysid/src/main/native/include/sysid/analysis/JSONConverter.h new file mode 100644 index 00000000000..7581d25fc16 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/JSONConverter.h @@ -0,0 +1,24 @@ +// 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. + +#pragma once + +#include +#include + +#include + +namespace sysid { +/** + * Converts a JSON from the old frc-characterization format to the new sysid + * format. + * + * @param path The path to the old JSON. + * @param logger The logger instance for log messages. + * @return The full file path of the newly saved JSON. + */ +std::string ConvertJSON(std::string_view path, wpi::Logger& logger); + +std::string ToCSV(std::string_view path, wpi::Logger& logger); +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/OLS.h b/sysid/src/main/native/include/sysid/analysis/OLS.h new file mode 100644 index 00000000000..cf979041565 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/OLS.h @@ -0,0 +1,26 @@ +// 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. + +#pragma once + +#include +#include +#include + +#include + +namespace sysid { + +/** + * Performs ordinary least squares multiple regression on the provided data and + * returns a vector of coefficients along with the r-squared (coefficient of + * determination) and RMSE (stardard deviation of the residuals) of the fit. + * + * @param X The independent data in y = Xβ. + * @param y The dependent data in y = Xβ. + */ +std::tuple, double, double> OLS(const Eigen::MatrixXd& X, + const Eigen::VectorXd& y); + +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h b/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h new file mode 100644 index 00000000000..a452c640626 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h @@ -0,0 +1,74 @@ +// 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. + +#pragma once + +#include +#include +#include + +namespace sysid { +/** + * Simulation of a Simple Motor mechanism based off of a model from SysId + * Feedforward gains. + */ +class SimpleMotorSim { + public: + /** + * @param Ks Static friction gain. + * @param Kv Velocity gain. + * @param Ka Acceleration gain. + * @param initialPosition Initial flywheel position. + * @param initialVelocity Initial flywheel velocity. + */ + SimpleMotorSim(double Ks, double Kv, double Ka, double initialPosition = 0.0, + double initialVelocity = 0.0); + + /** + * Simulates affine state-space system dx/dt = Ax + Bu + c sgn(x) forward dt + * seconds. + * + * @param voltage Voltage to apply over the timestep. + * @param dt Sammple period. + */ + void Update(units::volt_t voltage, units::second_t dt); + + /** + * Returns the position. + * + * @return The current position + */ + double GetPosition() const; + + /** + * Returns the velocity. + * + * @return The current velocity + */ + double GetVelocity() const; + + /** + * Returns the acceleration for the current state and given input. + * + * @param voltage The voltage that is being applied to the mechanism / input + * @return The acceleration given the state and input + */ + double GetAcceleration(units::volt_t voltage) const; + + /** + * Resets model position and velocity. + * + * @param position The position the mechanism should be reset to + * @param velocity The velocity the mechanism should be reset to + */ + void Reset(double position = 0.0, double velocity = 0.0); + + private: + Eigen::Matrix m_A; + Eigen::Matrix m_B; + Eigen::Vector m_c; + Eigen::Vector m_x; +}; + +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/Storage.h b/sysid/src/main/native/include/sysid/analysis/Storage.h new file mode 100644 index 00000000000..52899a0096e --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/Storage.h @@ -0,0 +1,95 @@ +// 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. + +#pragma once + +#include + +#include + +namespace sysid { + +/** + * Represents each data point after it is cleaned and various parameters are + * calculated. + */ +struct PreparedData { + /** + * The timestamp of the data point. + */ + units::second_t timestamp; + + /** + * The voltage of the data point. + */ + double voltage; + + /** + * The position of the data point. + */ + double position; + + /** + * The velocity of the data point. + */ + double velocity; + + /** + * The difference in timestamps between this point and the next point. + */ + units::second_t dt = 0_s; + + /** + * The acceleration of the data point + */ + double acceleration = 0.0; + + /** + * The cosine value of the data point. This is only used for arm data where we + * take the cosine of the position. + */ + double cos = 0.0; + + /** + * The sine value of the data point. This is only used for arm data where we + * take the sine of the position. + */ + double sin = 0.0; + + /** + * Equality operator between PreparedData structs + * + * @param rhs Another PreparedData struct + * @return If the other struct is equal to this one + */ + constexpr bool operator==(const PreparedData& rhs) const { + return timestamp == rhs.timestamp && voltage == rhs.voltage && + position == rhs.position && velocity == rhs.velocity && + dt == rhs.dt && acceleration == rhs.acceleration && cos == rhs.cos; + } +}; + +/** + * Storage used by the analysis manger. + */ +struct Storage { + /** + * Dataset for slow (aka quasistatic) test + */ + std::vector slowForward; + std::vector slowBackward; + + /** + * Dataset for fast (aka dynamic) test + */ + std::vector fastForward; + std::vector fastBackward; + + bool empty() const { + return slowForward.empty() || slowBackward.empty() || fastForward.empty() || + fastBackward.empty(); + } +}; + +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h b/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h new file mode 100644 index 00000000000..b0220721e81 --- /dev/null +++ b/sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h @@ -0,0 +1,19 @@ +// 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. + +#pragma once + +#include + +namespace sysid { +/** + * Calculates the track width given the left distance, right distance, and + * accumulated gyro angle. + * + * @param l The distance traveled by the left side of the drivetrain. + * @param r The distance traveled by the right side of the drivetrain. + * @param accum The accumulated gyro angle. + */ +double CalculateTrackWidth(double l, double r, units::radian_t accum); +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h b/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h new file mode 100644 index 00000000000..b636bc1d2c4 --- /dev/null +++ b/sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h @@ -0,0 +1,237 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "sysid/analysis/AnalysisType.h" + +namespace sysid { +/** + * This class is reponsible for collecting data from the robot and storing it + * inside a JSON. + */ +class TelemetryManager { + public: + /** + * Represents settings for an instance of the TelemetryManager class. This + * contains information about the quasistatic ramp rate for slow tests, the + * step voltage for fast tests, and the mechanism type for characterization. + */ + struct Settings { + /** + * The rate at which the voltage should increase during the quasistatic test + * (V/s). + */ + double quasistaticRampRate = 0.25; + + /** + * The voltage that the dynamic test should run at (V). + */ + double stepVoltage = 7.0; + + /** + * The units the mechanism moves per recorded rotation. The sysid project + * will be recording things in rotations of the shaft so the + * unitsPerRotation is to convert those measurements to the units the user + * wants to use. + */ + double unitsPerRotation = 1.0; + + /** + * The name of the units used. + * Valid units: "Meters", "Feet", "Inches", "Radians", "Degrees", + * "Rotations" + */ + std::string units = "Meters"; + + /** + * The type of mechanism that will be analyzed. + * Supported mechanisms: Drivetrain, Angular Drivetrain, Elevator, Arm, + * Simple motor. + */ + AnalysisType mechanism = analysis::kDrivetrain; + }; + + /** + * Constructs an instance of the telemetry manager with the provided settings + * and NT instance to collect data over. + * + * @param settings The settings for this instance of the telemetry manager. + * @param logger The logger instance to use for log data. + * @param instance The NT instance to collect data over. The default value of + * this parameter should suffice in production; it should only + * be changed during unit testing. + */ + explicit TelemetryManager(const Settings& settings, wpi::Logger& logger, + nt::NetworkTableInstance instance = + nt::NetworkTableInstance::GetDefault()); + + /** + * Begins a test with the given parameters. + * + * @param name The name of the test. + */ + void BeginTest(std::string_view name); + + /** + * Ends the currently running test. If there is no test running, this is a + * no-op. + */ + void EndTest(); + + /** + * Updates the telemetry manager -- this adds a new autospeed entry and + * collects newest data from the robot. This must be called periodically by + * the user. + */ + void Update(); + + /** + * Registers a callback that's called by the TelemetryManager when there is a + * message to display to the user. + * + * @param callback Callback function that runs based off of the message + */ + void RegisterDisplayCallback(std::function callback) { + m_callbacks.emplace_back(std::move(callback)); + } + + /** + * Saves a JSON with the stored data at the given location. + * + * @param location The location to save the JSON at (this is the folder that + * should contain the saved JSON). + * @return The full file path of the saved JSON. + */ + std::string SaveJSON(std::string_view location); + + /** + * Returns whether a test is currently running. + * + * @return Whether a test is currently running. + */ + bool IsActive() const { return m_isRunningTest; } + + /** + * Returns whether the specified test is running or has run. + * + * @param name The test to check. + * + * @return Whether the specified test is running or has run. + */ + bool HasRunTest(std::string_view name) const { + return std::find(m_tests.cbegin(), m_tests.cend(), name) != m_tests.end(); + } + + /** + * Gets the size of the stored data. + * + * @return The size of the stored data + */ + size_t GetCurrentDataSize() const { return m_params.data.size(); } + + private: + enum class State { WaitingForEnable, RunningTest, WaitingForData }; + + /** + * Stores information about a currently running test. This information + * includes whether the robot will be traveling quickly (dynamic) or slowly + * (quasistatic), the direction of movement, the start time of the test, + * whether the robot is enabled, the current speed of the robot, and the + * collected data. + */ + struct TestParameters { + bool fast = false; + bool forward = false; + bool rotate = false; + + State state = State::WaitingForEnable; + + double enableStart = 0.0; + double disableStart = 0.0; + + bool enabled = false; + double speed = 0.0; + + std::string raw; + std::vector> data{}; + bool overflow = false; + bool mechError = false; + + TestParameters() = default; + TestParameters(bool fast, bool forward, bool rotate, State state) + : fast{fast}, forward{forward}, rotate{rotate}, state{state} {} + }; + + // Settings for this instance. + const Settings& m_settings; + + // Logger. + wpi::Logger& m_logger; + + // Test parameters for the currently running test. + TestParameters m_params; + bool m_isRunningTest = false; + + // A list of running or already run tests. + std::vector m_tests; + + // Stores the test data. + wpi::json m_data; + + // Display callbacks. + wpi::SmallVector, 1> m_callbacks; + + // NetworkTables instance and entries. + nt::NetworkTableInstance m_inst; + std::shared_ptr table = m_inst.GetTable("SmartDashboard"); + nt::DoublePublisher m_voltageCommand = + table->GetDoubleTopic("SysIdVoltageCommand").Publish(); + nt::StringPublisher m_testType = + table->GetStringTopic("SysIdTestType").Publish(); + nt::BooleanPublisher m_rotate = + table->GetBooleanTopic("SysIdRotate").Publish(); + nt::StringPublisher m_mechanism = + table->GetStringTopic("SysIdTest").Publish(); + nt::BooleanPublisher m_overflowPub = + table->GetBooleanTopic("SysIdOverflow").Publish(); + nt::BooleanSubscriber m_overflowSub = + table->GetBooleanTopic("SysIdOverflow").Subscribe(false); + nt::BooleanPublisher m_mechErrorPub = + table->GetBooleanTopic("SysIdWrongMech").Publish(); + nt::BooleanSubscriber m_mechErrorSub = + table->GetBooleanTopic("SysIdWrongMech").Subscribe(false); + nt::StringSubscriber m_telemetry = + table->GetStringTopic("SysIdTelemetry").Subscribe(""); + nt::IntegerSubscriber m_fmsControlData = + m_inst.GetTable("FMSInfo") + ->GetIntegerTopic("FMSControlData") + .Subscribe(0); + nt::DoublePublisher m_ackNumberPub = + table->GetDoubleTopic("SysIdAckNumber").Publish(); + nt::DoubleSubscriber m_ackNumberSub = + table->GetDoubleTopic("SysIdAckNumber").Subscribe(0); + + int m_ackNumber; +}; +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/view/Analyzer.h b/sysid/src/main/native/include/sysid/view/Analyzer.h new file mode 100644 index 00000000000..2f30f610000 --- /dev/null +++ b/sysid/src/main/native/include/sysid/view/Analyzer.h @@ -0,0 +1,261 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "sysid/analysis/AnalysisManager.h" +#include "sysid/analysis/AnalysisType.h" +#include "sysid/analysis/FeedbackAnalysis.h" +#include "sysid/analysis/FeedbackControllerPreset.h" +#include "sysid/view/AnalyzerPlot.h" + +struct ImPlotPoint; + +namespace glass { +class Storage; +} // namespace glass + +namespace sysid { +/** + * The Analyzer GUI takes care of providing the user with a user interface to + * load their data, visualize the data, adjust certain variables, and then view + * the calculated gains. + */ +class Analyzer : public glass::View { + public: + /** + * The different display and processing states for the GUI + */ + enum class AnalyzerState { + kWaitingForJSON, + kNominalDisplay, + kMotionThresholdError, + kTestDurationError, + kGeneralDataError, + kFileError + }; + /** + * The different motor controller timing presets that can be used. + */ + static constexpr const char* kPresetNames[] = {"Default", + "WPILib (2020-)", + "WPILib (Pre-2020)", + "CANCoder", + "CTRE (Pro)", + "CTRE", + "REV Brushless Encoder Port", + "REV Brushed Encoder Port", + "REV Data Port", + "Venom"}; + + /** + * The different control loops that can be used. + */ + static constexpr const char* kLoopTypes[] = {"Position", "Velocity"}; + + /** + * Linear drivetrain analysis subsets + */ + static constexpr const char* kDatasets[] = {"Combined", "Left", "Right"}; + + /** + * Creates the Analyzer widget + * + * @param storage Glass Storage + * @param logger The program logger + */ + Analyzer(glass::Storage& storage, wpi::Logger& logger); + + /** + * Displays the analyzer widget + */ + void Display() override; + + ~Analyzer() override { AbortDataPrep(); }; + + private: + /** + * Handles the logic for selecting a json to analyze + */ + void SelectFile(); + + /** + * Kills the data preparation thread + */ + void AbortDataPrep(); + + /** + * Displays the settings to adjust trimming and filtering for feedforward + * gains. + */ + void DisplayFeedforwardParameters(float beginX, float beginY); + + /** + * Displays the graphs of the data. + */ + void DisplayGraphs(); + + /** + * Displays the file selection widget. + */ + void DisplayFileSelector(); + + /** + * Resets the current analysis data. + */ + void ResetData(); + + /** + * Sets up the reset button and Unit override buttons. + * + * @return True if the tool had been reset. + */ + bool DisplayResetAndUnitOverride(); + + /** + * Prepares the data for analysis. + */ + void PrepareData(); + + /** + * Sets up the graphs to display Raw Data. + */ + void PrepareRawGraphs(); + + /** + * Sets up the graphs to display filtered/processed data. + */ + void PrepareGraphs(); + + /** + * True if the stored state is associated with an error. + */ + bool IsErrorState(); + + /** + * True if the stored state is associated with a data processing error. + */ + bool IsDataErrorState(); + + /** + * Displays inputs to allow the collecting of theoretical feedforward gains. + */ + void CollectFeedforwardGains(float beginX, float beginY); + + /** + * Displays calculated feedforward gains. + */ + void DisplayFeedforwardGains(float beginX, float beginY); + + /** + * Displays calculated feedback gains. + */ + void DisplayFeedbackGains(); + + /** + * Estimates ideal step test duration, qp, and qv for the LQR based off of the + * data given + */ + void ConfigParamsOnFileSelect(); + + /** + * Updates feedforward gains from the analysis manager. + */ + void UpdateFeedforwardGains(); + + /** + * Updates feedback gains from the analysis manager. + */ + void UpdateFeedbackGains(); + + /** + * Handles logic of displaying a gain on ImGui + */ + bool DisplayGain(const char* text, double* data, bool readOnly); + + /** + * Handles errors when they pop up. + */ + void HandleError(std::string_view msg); + + // State of the Display GUI + AnalyzerState m_state = AnalyzerState::kWaitingForJSON; + + // Stores the exception message. + std::string m_exception; + + bool m_calcDefaults = false; + + // This is true if the error popup needs to be displayed + bool m_errorPopup = false; + + // Everything related to feedback controller calculations. + AnalysisManager::Settings m_settings; + wpi::StringMap m_presets; + + int m_selectedLoopType = 1; + int m_selectedPreset = 0; + + // Feedforward and feedback gains. + std::vector m_ff; + double m_accelRSquared; + double m_accelRMSE; + double m_Kp; + double m_Kd; + units::millisecond_t m_timescale; + + // Track width + std::optional m_trackWidth; + + // Units + int m_selectedOverrideUnit = 0; + double m_conversionFactor = 0.0; + + // Data analysis + std::unique_ptr m_manager; + int m_dataset = 0; + int m_window = 8; + double m_threshold = 0.2; + float m_stepTestDuration = 0.0; + + double m_gearingNumerator = 1.0; + double m_gearingDenominator = 1.0; + + bool combinedGraphFit = false; + + // File manipulation + std::unique_ptr m_selector; + std::string m_location; + + // Logger + wpi::Logger& m_logger; + + // Plot + AnalyzerPlot m_plot{m_logger}; + bool m_prevPlotsLoaded = false; + + // Stores graph scroll bar position and states for keeping track of scroll + // positions after loading graphs + float m_graphScroll = 0; + + std::atomic m_abortDataPrep{false}; + std::thread m_dataThread; +}; +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h b/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h new file mode 100644 index 00000000000..e5fbe8ca959 --- /dev/null +++ b/sysid/src/main/native/include/sysid/view/AnalyzerPlot.h @@ -0,0 +1,203 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "sysid/analysis/AnalysisType.h" +#include "sysid/analysis/Storage.h" + +namespace sysid { +/** + * Class that helps with plotting data in the analyzer view. + */ +class AnalyzerPlot { + public: + float m_pointSize = 1.25; + // 0 for forward, 1 for reverse + int m_direction = 0; + + /** + * Constructs an instance of the analyzer plot helper and allocates memory for + * all data vectors. + * + * @param logger The program logger + */ + explicit AnalyzerPlot(wpi::Logger& logger); + + /** + * Sets the data to be displayed on the plots. + * + * @param rawData Raw data storage. + * @param filteredData Filtered data storage. + * @param unit Unit of the dataset + * @param ff List of feedforward gains (Ks, Kv, Ka, and optionally + * Kg). + * @param startTimes Array of dataset start times. + * @param type Type of analysis. + * @param abort Aborts analysis early if set to true from another + * thread. + */ + void SetData(const Storage& rawData, const Storage& filteredData, + std::string_view unit, const std::vector& ff, + const std::array& startTimes, + AnalysisType type, std::atomic& abort); + + /** + * Utility method to plot the raw time series data + * + * @param rawSlow The raw slow (quasistatic) test data + * @param rawFast The raw fast (dynamic) test data + * @param abort Aborts analysis early if set to true from another thread + */ + void SetRawTimeData(const std::vector& rawSlow, + const std::vector& rawFast, + std::atomic& abort); + + /** + * Utility method to reset everything before generating the points to plot. + */ + void ResetData(); + + /** + * Utility method to get set the graph labels based off of the units + * + * @param unit Unit of the dataset + */ + void SetGraphLabels(std::string_view unit); + + /** + * Sets up only the raw time series data to be plotted. This is mainly + * intended to be used if the filtered data has issues with it. + * + * @param data The raw data. + * @param unit Unit of the dataset. + * @param abort Aborts analysis early if set to true from another thread. + */ + void SetRawData(const Storage& data, std::string_view unit, + std::atomic& abort); + + /** + * Displays time domain plots. + * + * @return Returns true if plots aren't in the loading state + */ + bool DisplayPlots(); + + /** + * Sets certain flags to true so that the GUI automatically fits the plots + */ + void FitPlots(); + + /** + * Gets the pointer to the stored Root Mean Squared Error for display + * + * @return A pointer to the RMSE + */ + double* GetSimRMSE(); + + /** + * Gets the pointer to the stored simulated velocity R-squared for display + * + * @return A pointer to the R-squared + */ + double* GetSimRSquared(); + + private: + // The maximum size of each vector (dataset to plot) + static constexpr size_t kMaxSize = 2048; + + struct FilteredDataVsTimePlot { + std::vector rawData; + std::vector filteredData; + + // Simulated time domain data + std::vector> simData; + + // Stores whether this was the first call to Plot() since setting data + bool fitNextPlot = false; + + FilteredDataVsTimePlot(); + + /** + * Plots data and fit line. + * + * @param title Plot title. + * @param size Plot size. + * @param yLabel Y axis label. + * @param pointSize The size of the data point markers (in pixels). + */ + void Plot(const char* title, const ImVec2& size, const char* yLabel, + float pointSize); + + /** + * Clears plot. + */ + void Clear(); + }; + + struct DataWithFitLinePlot { + std::vector data; + std::array fitLine; + + // Stores whether this was the first call to Plot() since setting data + bool fitNextPlot = false; + + DataWithFitLinePlot(); + + /** + * Plots data and fit line. + * + * @param title Plot title. + * @param size Plot size. + * @param xLabel X axis label. + * @param yLabel Y axis label. + * @param fitX True if X axis should be autofitted. + * @param fitY True if Y axis should be autofitted. + * @param pointSize The size of the data point markers (in pixels). + * @param setup Callback within BeginPlot() block that performs custom plot + * setup. + */ + void Plot( + const char* title, const ImVec2& size, const char* xLabel, + const char* yLabel, bool fitX, bool fitY, float pointSize, + std::function setup = [] {}); + + /** + * Clears plot. + */ + void Clear(); + }; + + std::string m_velocityLabel; + std::string m_accelerationLabel; + std::string m_velPortionAccelLabel; + + // Thread safety + wpi::spinlock m_mutex; + + // Logger + wpi::Logger& m_logger; + + FilteredDataVsTimePlot m_quasistaticData; + FilteredDataVsTimePlot m_dynamicData; + DataWithFitLinePlot m_regressionData; + DataWithFitLinePlot m_timestepData; + + double m_RMSE; + double m_accelRSquared; +}; +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/view/JSONConverter.h b/sysid/src/main/native/include/sysid/view/JSONConverter.h new file mode 100644 index 00000000000..89bfa3290d1 --- /dev/null +++ b/sysid/src/main/native/include/sysid/view/JSONConverter.h @@ -0,0 +1,57 @@ +// 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. + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +namespace sysid { +/** + * Helps with converting different JSONs into different formats. Primarily + * enables users to convert an old 2020 FRC-Characterization JSON into a SysId + * JSON or a SysId JSON into a CSV file. + */ +class JSONConverter { + public: + /** + * Creates a JSONConverter widget + * + * @param logger The program logger + */ + explicit JSONConverter(wpi::Logger& logger) : m_logger(logger) {} + + /** + * Function to display the SysId JSON to CSV converter. + */ + void DisplayCSVConvert(); + + private: + /** + * Helper method to display a specific JSON converter + * + * @param tooltip The tooltip describing the JSON converter + * @param converter The function that takes a filename path and performs the + * previously specifid JSON conversion. + */ + void DisplayConverter( + const char* tooltip, + std::function converter); + + wpi::Logger& m_logger; + + std::string m_location; + std::unique_ptr m_opener; + + std::string m_exception; + + double m_timestamp = 0; +}; +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/view/Logger.h b/sysid/src/main/native/include/sysid/view/Logger.h new file mode 100644 index 00000000000..d06d6508165 --- /dev/null +++ b/sysid/src/main/native/include/sysid/view/Logger.h @@ -0,0 +1,82 @@ +// 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. + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +#include "sysid/telemetry/TelemetryManager.h" + +namespace glass { +class Storage; +} // namespace glass + +namespace sysid { +/** + * The logger GUI takes care of running the system idenfitication tests over + * NetworkTables and logging the data. This data is then stored in a JSON file + * which can be used for analysis. + */ +class Logger : public glass::View { + public: + /** + * Makes a logger widget. + * + * @param storage The glass storage object + * @param logger A logger object that keeps track of the program's logs + */ + Logger(glass::Storage& storage, wpi::Logger& logger); + + /** + * Displays the logger widget. + */ + void Display() override; + + /** + * The different mechanism / analysis types that are supported. + */ + static constexpr const char* kTypes[] = {"Drivetrain", "Drivetrain (Angular)", + "Arm", "Elevator", "Simple"}; + + /** + * The different units that are supported. + */ + static constexpr const char* kUnits[] = {"Meters", "Feet", "Inches", + "Radians", "Rotations", "Degrees"}; + + private: + /** + * Handles the logic of selecting a folder to save the SysId JSON to + */ + void SelectDataFolder(); + + wpi::Logger& m_logger; + + TelemetryManager::Settings m_settings; + int m_selectedType = 0; + int m_selectedUnit = 0; + + std::unique_ptr m_manager = + std::make_unique(m_settings, m_logger); + + std::unique_ptr m_selector; + std::string m_jsonLocation; + + glass::NetworkTablesSettings m_ntSettings; + + bool m_isRotationalUnits = false; + + std::string m_popupText; + + std::string m_opened; + std::string m_exception; +}; +} // namespace sysid diff --git a/sysid/src/main/native/include/sysid/view/UILayout.h b/sysid/src/main/native/include/sysid/view/UILayout.h new file mode 100644 index 00000000000..732a1aaf658 --- /dev/null +++ b/sysid/src/main/native/include/sysid/view/UILayout.h @@ -0,0 +1,95 @@ +// 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. + +#pragma once + +#include + +namespace sysid { +/** + * constexpr shim for ImVec2. + */ +struct Vector2d { + /** + * X coordinate. + */ + float x = 0; + + /** + * Y coordinate. + */ + float y = 0; + + /** + * Vector2d addition operator. + * + * @param rhs Vector to add. + * @return Sum of two vectors. + */ + constexpr Vector2d operator+(const Vector2d& rhs) const { + return Vector2d{x + rhs.x, y + rhs.y}; + } + + /** + * Vector2d subtraction operator. + * + * @param rhs Vector to subtract. + * @return Difference of two vectors. + */ + constexpr Vector2d operator-(const Vector2d& rhs) const { + return Vector2d{x - rhs.x, y - rhs.y}; + } + + /** + * Conversion operator to ImVec2. + */ + explicit operator ImVec2() const { return ImVec2{x, y}; } +}; + +// App window size +inline constexpr Vector2d kAppWindowSize{1280, 720}; + +// Menubar height +inline constexpr int kMenubarHeight = 20; + +// Gap between window edges +inline constexpr int kWindowGap = 5; + +// Left column position and size +inline constexpr Vector2d kLeftColPos{kWindowGap, kMenubarHeight + kWindowGap}; +inline constexpr Vector2d kLeftColSize{ + 310, kAppWindowSize.y - kLeftColPos.y - kWindowGap}; + +// Left column contents +inline constexpr Vector2d kLoggerWindowPos = kLeftColPos; +inline constexpr Vector2d kLoggerWindowSize{ + kLeftColSize.x, kAppWindowSize.y - kWindowGap - kLoggerWindowPos.y}; + +// Center column position and size +inline constexpr Vector2d kCenterColPos = + kLeftColPos + Vector2d{kLeftColSize.x + kWindowGap, 0}; +inline constexpr Vector2d kCenterColSize{ + 360, kAppWindowSize.y - kLeftColPos.y - kWindowGap}; + +// Center column contents +inline constexpr Vector2d kAnalyzerWindowPos = kCenterColPos; +inline constexpr Vector2d kAnalyzerWindowSize{kCenterColSize.x, 550}; +inline constexpr Vector2d kProgramLogWindowPos = + kAnalyzerWindowPos + Vector2d{0, kAnalyzerWindowSize.y + kWindowGap}; +inline constexpr Vector2d kProgramLogWindowSize{ + kCenterColSize.x, kAppWindowSize.y - kWindowGap - kProgramLogWindowPos.y}; + +// Right column position and size +inline constexpr Vector2d kRightColPos = + kCenterColPos + Vector2d{kCenterColSize.x + kWindowGap, 0}; +inline constexpr Vector2d kRightColSize = + kAppWindowSize - kRightColPos - Vector2d{kWindowGap, kWindowGap}; + +// Right column contents +inline constexpr Vector2d kDiagnosticPlotWindowPos = kRightColPos; +inline constexpr Vector2d kDiagnosticPlotWindowSize = kRightColSize; + +// Text box width as a multiple of the font size +inline constexpr int kTextBoxWidthMultiple = 10; +} // namespace sysid diff --git a/sysid/src/main/native/mac/sysid.icns b/sysid/src/main/native/mac/sysid.icns new file mode 100644 index 00000000000..53f0d699ac3 Binary files /dev/null and b/sysid/src/main/native/mac/sysid.icns differ diff --git a/sysid/src/main/native/resources/sysid-128.png b/sysid/src/main/native/resources/sysid-128.png new file mode 100644 index 00000000000..1da8dfeaa88 Binary files /dev/null and b/sysid/src/main/native/resources/sysid-128.png differ diff --git a/sysid/src/main/native/resources/sysid-16.png b/sysid/src/main/native/resources/sysid-16.png new file mode 100644 index 00000000000..851739f3926 Binary files /dev/null and b/sysid/src/main/native/resources/sysid-16.png differ diff --git a/sysid/src/main/native/resources/sysid-256.png b/sysid/src/main/native/resources/sysid-256.png new file mode 100644 index 00000000000..a66aa94a257 Binary files /dev/null and b/sysid/src/main/native/resources/sysid-256.png differ diff --git a/sysid/src/main/native/resources/sysid-32.png b/sysid/src/main/native/resources/sysid-32.png new file mode 100644 index 00000000000..3332bd193f8 Binary files /dev/null and b/sysid/src/main/native/resources/sysid-32.png differ diff --git a/sysid/src/main/native/resources/sysid-48.png b/sysid/src/main/native/resources/sysid-48.png new file mode 100644 index 00000000000..bee41b77c44 Binary files /dev/null and b/sysid/src/main/native/resources/sysid-48.png differ diff --git a/sysid/src/main/native/resources/sysid-512.png b/sysid/src/main/native/resources/sysid-512.png new file mode 100644 index 00000000000..5fc1d8ed7be Binary files /dev/null and b/sysid/src/main/native/resources/sysid-512.png differ diff --git a/sysid/src/main/native/resources/sysid-64.png b/sysid/src/main/native/resources/sysid-64.png new file mode 100644 index 00000000000..efb193d3cd6 Binary files /dev/null and b/sysid/src/main/native/resources/sysid-64.png differ diff --git a/sysid/src/main/native/win/sysid.ico b/sysid/src/main/native/win/sysid.ico new file mode 100644 index 00000000000..89f6d3f24de Binary files /dev/null and b/sysid/src/main/native/win/sysid.ico differ diff --git a/sysid/src/main/native/win/sysid.rc b/sysid/src/main/native/win/sysid.rc new file mode 100644 index 00000000000..21d11e8f5b8 --- /dev/null +++ b/sysid/src/main/native/win/sysid.rc @@ -0,0 +1 @@ +IDI_ICON1 ICON "sysid.ico" diff --git a/sysid/src/test/native/cpp/Main.cpp b/sysid/src/test/native/cpp/Main.cpp new file mode 100644 index 00000000000..e993c1f14ef --- /dev/null +++ b/sysid/src/test/native/cpp/Main.cpp @@ -0,0 +1,11 @@ +// 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 + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp b/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp new file mode 100644 index 00000000000..0abb2a1e9cc --- /dev/null +++ b/sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp @@ -0,0 +1,18 @@ +// 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 + +#include "sysid/analysis/AnalysisType.h" + +TEST(AnalysisTypeTest, FromName) { + EXPECT_EQ(sysid::analysis::kDrivetrain, + sysid::analysis::FromName("Drivetrain")); + EXPECT_EQ(sysid::analysis::kDrivetrainAngular, + sysid::analysis::FromName("Drivetrain (Angular)")); + EXPECT_EQ(sysid::analysis::kElevator, sysid::analysis::FromName("Elevator")); + EXPECT_EQ(sysid::analysis::kArm, sysid::analysis::FromName("Arm")); + EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Simple")); + EXPECT_EQ(sysid::analysis::kSimple, sysid::analysis::FromName("Random")); +} diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp new file mode 100644 index 00000000000..44f664c800d --- /dev/null +++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp @@ -0,0 +1,105 @@ +// 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 + +#include "sysid/analysis/FeedbackAnalysis.h" +#include "sysid/analysis/FeedbackControllerPreset.h" + +TEST(FeedbackAnalysisTest, Velocity1) { + auto Kv = 3.060; + auto Ka = 0.327; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains( + sysid::presets::kDefault, params, Kv, Ka); + + EXPECT_NEAR(Kp, 2.11, 0.05); + EXPECT_NEAR(Kd, 0.00, 0.05); +} + +TEST(FeedbackAnalysisTest, Velocity2) { + auto Kv = 0.0693; + auto Ka = 0.1170; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains( + sysid::presets::kDefault, params, Kv, Ka); + + EXPECT_NEAR(Kp, 3.11, 0.05); + EXPECT_NEAR(Kd, 0.00, 0.05); +} + +TEST(FeedbackAnalysisTest, VelocityConversion) { + auto Kv = 0.0693; + auto Ka = 0.1170; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains( + sysid::presets::kDefault, params, Kv, Ka, 3.0 * 1023); + + // This should have the same Kp as the test above, but scaled by a factor of 3 + // * 1023. + EXPECT_NEAR(Kp, 3.11 / (3 * 1023), 0.005); + EXPECT_NEAR(Kd, 0.00, 0.05); +} + +TEST(FeedbackAnalysisTest, VelocityCTRE) { + auto Kv = 1.97; + auto Ka = 0.179; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains( + sysid::presets::kCTRECANCoder, params, Kv, Ka); + + EXPECT_NEAR(Kp, 0.000417, 0.00005); + EXPECT_NEAR(Kd, 0.00, 0.05); +} + +TEST(FeedbackAnalysisTest, VelocityCTREConversion) { + auto Kv = 1.97; + auto Ka = 0.179; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains( + sysid::presets::kCTRECANCoder, params, Kv, Ka, 3.0); + + // This should have the same Kp as the test above, but scaled by a factor + // of 3. + EXPECT_NEAR(Kp, 0.000417 / 3, 0.00005); + EXPECT_NEAR(Kd, 0.00, 0.05); +} + +TEST(FeedbackAnalysisTest, VelocityREV) { + auto Kv = 1.97; + auto Ka = 0.179; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains( + sysid::presets::kREVNEOBuiltIn, params, Kv, Ka); + + EXPECT_NEAR(Kp, 0.00241, 0.005); + EXPECT_NEAR(Kd, 0.00, 0.05); +} + +TEST(FeedbackAnalysisTest, VelocityREVConversion) { + auto Kv = 1.97; + auto Ka = 0.179; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculateVelocityFeedbackGains( + sysid::presets::kREVNEOBuiltIn, params, Kv, Ka, 3.0); + + // This should have the same Kp as the test above, but scaled by a factor + // of 3. + EXPECT_NEAR(Kp, 0.00241 / 3, 0.005); + EXPECT_NEAR(Kd, 0.00, 0.05); +} diff --git a/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp new file mode 100644 index 00000000000..a52840d620d --- /dev/null +++ b/sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp @@ -0,0 +1,251 @@ +// 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 + +#include +#include +#include + +#include "sysid/analysis/AnalysisManager.h" +#include "sysid/analysis/ArmSim.h" +#include "sysid/analysis/ElevatorSim.h" +#include "sysid/analysis/FeedforwardAnalysis.h" +#include "sysid/analysis/SimpleMotorSim.h" + +/** + * Return simulated test data for a given simulation model. + * + * @param Ks Static friction gain. + * @param Kv Velocity gain. + * @param Ka Acceleration gain. + * @param Kg Gravity cosine gain. + */ +template +sysid::Storage CollectData(Model& model) { + constexpr auto kUstep = 0.25_V / 1_s; + constexpr units::volt_t kUmax = 7_V; + constexpr units::second_t T = 5_ms; + constexpr units::second_t kTestDuration = 5_s; + + sysid::Storage storage; + auto& [slowForward, slowBackward, fastForward, fastBackward] = storage; + + // Slow forward test + auto voltage = 0_V; + for (int i = 0; i < (kTestDuration / T).value(); ++i) { + slowForward.emplace_back(sysid::PreparedData{ + i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, + model.GetAcceleration(voltage), std::cos(model.GetPosition()), + std::sin(model.GetPosition())}); + + model.Update(voltage, T); + voltage += kUstep * T; + } + + // Slow backward test + model.Reset(); + voltage = 0_V; + for (int i = 0; i < (kTestDuration / T).value(); ++i) { + slowBackward.emplace_back(sysid::PreparedData{ + i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, + model.GetAcceleration(voltage), std::cos(model.GetPosition()), + std::sin(model.GetPosition())}); + + model.Update(voltage, T); + voltage -= kUstep * T; + } + + // Fast forward test + model.Reset(); + voltage = 0_V; + for (int i = 0; i < (kTestDuration / T).value(); ++i) { + fastForward.emplace_back(sysid::PreparedData{ + i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, + model.GetAcceleration(voltage), std::cos(model.GetPosition()), + std::sin(model.GetPosition())}); + + model.Update(voltage, T); + voltage = kUmax; + } + + // Fast backward test + model.Reset(); + voltage = 0_V; + for (int i = 0; i < (kTestDuration / T).value(); ++i) { + fastBackward.emplace_back(sysid::PreparedData{ + i * T, voltage.value(), model.GetPosition(), model.GetVelocity(), T, + model.GetAcceleration(voltage), std::cos(model.GetPosition()), + std::sin(model.GetPosition())}); + + model.Update(voltage, T); + voltage = -kUmax; + } + + return storage; +} + +TEST(FeedforwardAnalysisTest, Arm1) { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; + constexpr double Kg = 0.211; + + for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) { + sysid::ArmSim model{Ks, Kv, Ka, Kg, offset}; + auto ff = sysid::CalculateFeedforwardGains(CollectData(model), + sysid::analysis::kArm); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); + EXPECT_NEAR(gains[3], Kg, 0.003); + EXPECT_NEAR(gains[4], offset, 0.007); + } +} + +TEST(FeedforwardAnalysisTest, Arm2) { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + constexpr double Kg = 0.122; + + for (const auto& offset : {-2.0, -1.0, 0.0, 1.0, 2.0}) { + sysid::ArmSim model{Ks, Kv, Ka, Kg, offset}; + auto ff = sysid::CalculateFeedforwardGains(CollectData(model), + sysid::analysis::kArm); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); + EXPECT_NEAR(gains[3], Kg, 0.003); + EXPECT_NEAR(gains[4], offset, 0.007); + } +} + +TEST(FeedforwardAnalysisTest, Drivetrain1) { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + auto ff = sysid::CalculateFeedforwardGains(CollectData(model), + sysid::analysis::kDrivetrain); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); +} + +TEST(FeedforwardAnalysisTest, Drivetrain2) { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + auto ff = sysid::CalculateFeedforwardGains(CollectData(model), + sysid::analysis::kDrivetrain); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); +} + +TEST(FeedforwardAnalysisTest, DrivetrainAngular1) { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + auto ff = sysid::CalculateFeedforwardGains( + CollectData(model), sysid::analysis::kDrivetrainAngular); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); +} + +TEST(FeedforwardAnalysisTest, DrivetrainAngular2) { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + auto ff = sysid::CalculateFeedforwardGains( + CollectData(model), sysid::analysis::kDrivetrainAngular); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); +} + +TEST(FeedforwardAnalysisTest, Elevator1) { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; + constexpr double Kg = -0.211; + + sysid::ElevatorSim model{Ks, Kv, Ka, Kg}; + auto ff = sysid::CalculateFeedforwardGains(CollectData(model), + sysid::analysis::kElevator); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); + EXPECT_NEAR(gains[3], Kg, 0.003); +} + +TEST(FeedforwardAnalysisTest, Elevator2) { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + constexpr double Kg = -0.122; + + sysid::ElevatorSim model{Ks, Kv, Ka, Kg}; + auto ff = sysid::CalculateFeedforwardGains(CollectData(model), + sysid::analysis::kElevator); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); + EXPECT_NEAR(gains[3], Kg, 0.003); +} + +TEST(FeedforwardAnalysisTest, Simple1) { + constexpr double Ks = 1.01; + constexpr double Kv = 3.060; + constexpr double Ka = 0.327; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + auto ff = sysid::CalculateFeedforwardGains(CollectData(model), + sysid::analysis::kSimple); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); +} + +TEST(FeedforwardAnalysisTest, Simple2) { + constexpr double Ks = 0.547; + constexpr double Kv = 0.0693; + constexpr double Ka = 0.1170; + + sysid::SimpleMotorSim model{Ks, Kv, Ka}; + auto ff = sysid::CalculateFeedforwardGains(CollectData(model), + sysid::analysis::kSimple); + auto& gains = std::get<0>(ff); + + EXPECT_NEAR(gains[0], Ks, 0.003); + EXPECT_NEAR(gains[1], Kv, 0.003); + EXPECT_NEAR(gains[2], Ka, 0.003); +} diff --git a/sysid/src/test/native/cpp/analysis/FilterTest.cpp b/sysid/src/test/native/cpp/analysis/FilterTest.cpp new file mode 100644 index 00000000000..a7b03492747 --- /dev/null +++ b/sysid/src/test/native/cpp/analysis/FilterTest.cpp @@ -0,0 +1,170 @@ +// 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 +#include +#include + +#include + +#include "sysid/analysis/AnalysisManager.h" +#include "sysid/analysis/FeedforwardAnalysis.h" +#include "sysid/analysis/FilteringUtils.h" +#include "sysid/analysis/Storage.h" + +TEST(FilterTest, MedianFilter) { + std::vector testData{ + sysid::PreparedData{0_s, 0, 0, 0}, sysid::PreparedData{0_s, 0, 0, 1}, + sysid::PreparedData{0_s, 0, 0, 10}, sysid::PreparedData{0_s, 0, 0, 5}, + sysid::PreparedData{0_s, 0, 0, 3}, sysid::PreparedData{0_s, 0, 0, 0}, + sysid::PreparedData{0_s, 0, 0, 1000}, sysid::PreparedData{0_s, 0, 0, 7}, + sysid::PreparedData{0_s, 0, 0, 6}, sysid::PreparedData{0_s, 0, 0, 5}}; + std::vector expectedData{ + sysid::PreparedData{0_s, 0, 0, 0}, sysid::PreparedData{0_s, 0, 0, 1}, + sysid::PreparedData{0_s, 0, 0, 5}, sysid::PreparedData{0_s, 0, 0, 5}, + sysid::PreparedData{0_s, 0, 0, 3}, sysid::PreparedData{0_s, 0, 0, 3}, + sysid::PreparedData{0_s, 0, 0, 7}, sysid::PreparedData{0_s, 0, 0, 7}, + sysid::PreparedData{0_s, 0, 0, 6}, sysid::PreparedData{0_s, 0, 0, 5}}; + + sysid::ApplyMedianFilter(&testData, 3); + EXPECT_EQ(expectedData, testData); +} + +TEST(FilterTest, NoiseFloor) { + std::vector testData = { + {0_s, 1, 2, 3, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 1, 0}, + {2_s, 1, 2, 3, 5_ms, 2, 0}, {3_s, 1, 2, 3, 5_ms, 5, 0}, + {4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0}, + {6_s, 1, 2, 3, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0}, + {8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0}}; + double noiseFloor = + GetNoiseFloor(testData, 2, [](auto&& pt) { return pt.acceleration; }); + EXPECT_NEAR(0.953, noiseFloor, 0.001); +} + +TEST(FilterTest, StepTrim) { + std::vector testData = { + {0_s, 1, 2, 3, 5_ms, 0, 0}, {1_s, 1, 2, 3, 5_ms, 0.25, 0}, + {2_s, 1, 2, 3, 5_ms, 0.5, 0}, {3_s, 1, 2, 3, 5_ms, 0.45, 0}, + {4_s, 1, 2, 3, 5_ms, 0.35, 0}, {5_s, 1, 2, 3, 5_ms, 0.15, 0}, + {6_s, 1, 2, 3, 5_ms, 0, 0}, {7_s, 1, 2, 3, 5_ms, 0.02, 0}, + {8_s, 1, 2, 3, 5_ms, 0.01, 0}, {9_s, 1, 2, 3, 5_ms, 0, 0}, + }; + + std::vector expectedData = { + {2_s, 1, 2, 3, 5_ms, 0.5, 0}, + {3_s, 1, 2, 3, 5_ms, 0.45, 0}, + {4_s, 1, 2, 3, 5_ms, 0.35, 0}, + {5_s, 1, 2, 3, 5_ms, 0.15, 0}}; + + auto maxTime = 9_s; + auto minTime = maxTime; + + sysid::AnalysisManager::Settings settings; + auto [tempMinTime, positionDelay, velocityDelay] = + sysid::TrimStepVoltageData(&testData, &settings, minTime, maxTime); + minTime = tempMinTime; + + EXPECT_EQ(expectedData[0].acceleration, testData[0].acceleration); + EXPECT_EQ(expectedData.back().acceleration, testData.back().acceleration); + EXPECT_EQ(5, settings.stepTestDuration.value()); + EXPECT_EQ(2, minTime.value()); +} + +template +void AssertCentralResults(F&& f, DfDx&& dfdx, units::second_t h, double min, + double max) { + static_assert(Samples % 2 != 0, "Number of samples must be odd."); + + auto filter = sysid::CentralFiniteDifference(h); + + for (int i = min / h.value(); i < max / h.value(); ++i) { + // Let filter initialize + if (i < static_cast(min / h.value()) + Samples) { + filter.Calculate(f(i * h.value())); + continue; + } + + // For central finite difference, the derivative computed at this point is + // half the window size in the past. + // The order of accuracy is O(h^(N - d)) where N is number of stencil + // points and d is order of derivative + EXPECT_NEAR(dfdx((i - static_cast((Samples - 1) / 2)) * h.value()), + filter.Calculate(f(i * h.value())), + std::pow(h.value(), Samples - Derivative)); + } +} + +/** + * Test central finite difference. + */ +TEST(LinearFilterOutputTest, CentralFiniteDifference) { + constexpr auto h = 5_ms; + + AssertCentralResults<1, 3>( + [](double x) { + // f(x) = x² + return x * x; + }, + [](double x) { + // df/dx = 2x + return 2.0 * x; + }, + h, -20.0, 20.0); + + AssertCentralResults<1, 3>( + [](double x) { + // f(x) = std::sin(x) + return std::sin(x); + }, + [](double x) { + // df/dx = std::cos(x) + return std::cos(x); + }, + h, -20.0, 20.0); + + AssertCentralResults<1, 3>( + [](double x) { + // f(x) = ln(x) + return std::log(x); + }, + [](double x) { + // df/dx = 1 / x + return 1.0 / x; + }, + h, 1.0, 20.0); + + AssertCentralResults<2, 5>( + [](double x) { + // f(x) = x^2 + return x * x; + }, + [](double x) { + // d²f/dx² = 2 + return 2.0; + }, + h, -20.0, 20.0); + + AssertCentralResults<2, 5>( + [](double x) { + // f(x) = std::sin(x) + return std::sin(x); + }, + [](double x) { + // d²f/dx² = -std::sin(x) + return -std::sin(x); + }, + h, -20.0, 20.0); + + AssertCentralResults<2, 5>( + [](double x) { + // f(x) = ln(x) + return std::log(x); + }, + [](double x) { + // d²f/dx² = -1 / x² + return -1.0 / (x * x); + }, + h, 1.0, 20.0); +} diff --git a/sysid/src/test/native/cpp/analysis/OLSTest.cpp b/sysid/src/test/native/cpp/analysis/OLSTest.cpp new file mode 100644 index 00000000000..bf205165a93 --- /dev/null +++ b/sysid/src/test/native/cpp/analysis/OLSTest.cpp @@ -0,0 +1,42 @@ +// 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 + +#include "sysid/analysis/OLS.h" + +TEST(OLSTest, TwoVariablesTwoPoints) { + // (1, 3) and (2, 5). Should produce y = 2x + 1. + Eigen::MatrixXd X{{1.0, 1.0}, {1.0, 2.0}}; + Eigen::VectorXd y{{3.0}, {5.0}}; + + auto [coefficients, cod, rmse] = sysid::OLS(X, y); + EXPECT_EQ(coefficients.size(), 2u); + + EXPECT_NEAR(coefficients[0], 1.0, 0.05); + EXPECT_NEAR(coefficients[1], 2.0, 0.05); + EXPECT_NEAR(cod, 1.0, 1E-4); +} + +TEST(OLSTest, TwoVariablesFivePoints) { + // (2, 4), (3, 5), (5, 7), (7, 10), (9, 15) + // Should produce 1.518x + 0.305. + Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 5}, {1, 7}, {1, 9}}; + Eigen::VectorXd y{{4}, {5}, {7}, {10}, {15}}; + + auto [coefficients, cod, rmse] = sysid::OLS(X, y); + EXPECT_EQ(coefficients.size(), 2u); + + EXPECT_NEAR(coefficients[0], 0.305, 0.05); + EXPECT_NEAR(coefficients[1], 1.518, 0.05); + EXPECT_NEAR(cod, 0.985, 0.05); +} + +#ifndef NDEBUG +TEST(OLSTest, MalformedData) { + Eigen::MatrixXd X{{1, 2}, {1, 3}, {1, 4}}; + Eigen::VectorXd y{{4}, {5}}; + EXPECT_DEATH(sysid::OLS(X, y), ""); +} +#endif diff --git a/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp new file mode 100644 index 00000000000..e7b662fe9d0 --- /dev/null +++ b/sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp @@ -0,0 +1,12 @@ +// 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 + +#include "sysid/analysis/TrackWidthAnalysis.h" + +TEST(TrackWidthAnalysisTest, Calculate) { + double result = sysid::CalculateTrackWidth(-0.5386, 0.5386, 90_deg); + EXPECT_NEAR(result, 0.6858, 1E-4); +}