Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[sysid] Add SysId (#5672)

Browse Source 
The source is copied from this commit:
625ff04784.
This commit is contained in:
Tyler Veness
2023-10-01 15:09:09 -07:00
committed by GitHub
parent 8d2cbfce16
commit a331ed2374
67 changed files with 7568 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -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)

1
settings.gradle
View File

@@ -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'

35
sysid/.styleguide Normal file
View File

@@ -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
}

41
sysid/CMakeLists.txt Normal file
View File

@@ -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()

32
sysid/Info.plist Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
<plist version="1.0">
<dict>
<key>CFBundleName</key>
<string>SysId</string>
<key>CFBundleExecutable</key>
<string>sysid</string>
<key>CFBundleDisplayName</key>
<string>SysId</string>
<key>CFBundleIdentifier</key>
<string>edu.wpi.first.tools.SysId</string>
<key>CFBundleIconFile</key>
<string>sysid.icns</string>
<key>CFBundlePackageType</key>
<string>APPL</string>
<key>CFBundleSupportedPlatforms</key>
<array>
<string>MacOSX</string>
</array>
<key>CFBundleInfoDictionaryVersion</key>
<string>6.0</string>
<key>CFBundleShortVersionString</key>
<string>2021</string>
<key>CFBundleVersion</key>
<string>2021</string>
<key>LSMinimumSystemVersion</key>
<string>10.14</string>
<key>NSHighResolutionCapable</key>
<true/>
</dict>
</plist>

172
sysid/build.gradle Normal file
View File

@@ -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'

66
sysid/docs/arm-ols-with-angle-offset.md Normal file
View File

@@ -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(ε, -δ)
```

210
sysid/docs/data-collection.md Normal file
View File

@@ -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".

125
sysid/publish.gradle Normal file
View File

@@ -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()
}
}
}
}

89
sysid/scripts/time_plots.py Executable file
View File

@@ -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()

7
sysid/src/main/generate/WPILibVersion.cpp.in Normal file
View File

@@ -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}";
}

219
sysid/src/main/native/cpp/App.cpp Normal file
View File

@@ -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 <cstdio>
#ifndef RUNNING_SYSID_TESTS
#include <filesystem>
#include <memory>
#include <string_view>
#include <fmt/format.h>
#include <glass/Context.h>
#include <glass/MainMenuBar.h>
#include <glass/Storage.h>
#include <glass/Window.h>
#include <glass/WindowManager.h>
#include <glass/other/Log.h>
#include <imgui.h>
#include <uv.h>
#include <wpi/Logger.h>
#include <wpigui.h>
#include <wpigui_openurl.h>
#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<glass::WindowManager> gWindowManager;
glass::Window* gLoggerWindow;
glass::Window* gAnalyzerWindow;
glass::Window* gProgramLogWindow;
static glass::MainMenuBar gMainMenu;
std::unique_ptr<sysid::JSONConverter> 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<glass::WindowManager>(storage);
gWindowManager->GlobalInit();
gLoggerWindow = gWindowManager->AddWindow(
"Logger", std::make_unique<sysid::Logger>(storage, gLogger));
gAnalyzerWindow = gWindowManager->AddWindow(
"Analyzer", std::make_unique<sysid::Analyzer>(storage, gLogger));
gProgramLogWindow = gWindowManager->AddWindow(
"Program Log", std::make_unique<glass::LogView>(&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<sysid::JSONConverter>(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

29
sysid/src/main/native/cpp/Main.cpp Normal file
View File

@@ -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 <string_view>
#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

80
sysid/src/main/native/cpp/Util.cpp Normal file
View File

@@ -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 <filesystem>
#include <stdexcept>
#include <imgui.h>
#include <wpi/raw_ostream.h>
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;
}

568
sysid/src/main/native/cpp/analysis/AnalysisManager.cpp Normal file
View File

@@ -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 <cmath>
#include <cstddef>
#include <functional>
#include <stdexcept>
#include <fmt/format.h>
#include <units/angle.h>
#include <units/math.h>
#include <wpi/StringExtras.h>
#include <wpi/StringMap.h>
#include <wpi/raw_istream.h>
#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 <size_t S, size_t Timestamp, size_t Voltage, size_t Position,
size_t Velocity>
static std::vector<PreparedData> ConvertToPrepared(
const std::vector<std::array<double, S>>& data) {
std::vector<PreparedData> prepared;
for (int i = 0; i < static_cast<int>(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 <size_t S>
static void CopyRawData(
wpi::StringMap<std::vector<std::array<double, S>>>* 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<PreparedData>& slowForward,
const std::vector<PreparedData>& slowBackward,
const std::vector<PreparedData>& fastForward,
const std::vector<PreparedData>& fastBackward) {
return Storage{slowForward, slowBackward, fastForward, fastBackward};
}
void AnalysisManager::PrepareGeneralData() {
using Data = std::array<double, 4>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> 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<std::vector<Data>>();
}
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<int>(
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<int>(
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<int>(
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<double, 9>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> 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<std::vector<Data>>();
}
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<int>(
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<int>(
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<int>(
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<double, 9>;
wpi::StringMap<std::vector<Data>> data;
wpi::StringMap<std::vector<PreparedData>> 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<std::vector<Data>>();
}
// 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<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(originalSlowForward, originalSlowBackward,
originalFastForward, originalFastBackward);
m_originalDataset[static_cast<int>(
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<int>(
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<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(rawSlowForward, rawSlowBackward, rawFastForward,
rawFastBackward);
m_rawDataset[static_cast<int>(
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<int>(
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<int>(
AnalysisManager::Settings::DrivetrainDataset::kCombined)] =
CombineDatasets(slowForward, slowBackward, fastForward, fastBackward);
m_filteredDataset[static_cast<int>(
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<int>(
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<std::string>());
// Get the rotation -> output units factor from the JSON.
m_unit = m_json.at("units").get<std::string>();
m_factor = m_json.at("unitsPerRotation").get<double>();
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<double>::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<double> 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<std::string>();
m_factor = m_json.at("unitsPerRotation").get<double>();
}

23
sysid/src/main/native/cpp/analysis/AnalysisType.cpp Normal file
View File

@@ -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;
}

64
sysid/src/main/native/cpp/analysis/ArmSim.cpp Normal file
View File

@@ -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 <cmath>
#include <frc/StateSpaceUtil.h>
#include <frc/system/NumericalIntegration.h>
#include <wpi/MathExtras.h>
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<double, 2>& x,
const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 2> {
return Eigen::Vector<double, 2>{
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<double, 1> 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<double, 1> 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<double, 2>{position, velocity};
}

50
sysid/src/main/native/cpp/analysis/ElevatorSim.cpp Normal file
View File

@@ -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 <frc/StateSpaceUtil.h>
#include <frc/system/Discretization.h>
#include <wpi/MathExtras.h>
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<double, 1> 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<double, 2, 2> Ad;
Eigen::Matrix<double, 2, 1> 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<double, 1> 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<double, 2>{position, velocity};
}

78
sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp Normal file
View File

@@ -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 <frc/controller/LinearQuadraticRegulator.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/acceleration.h>
#include <units/velocity.h>
#include <units/voltage.h>
#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<units::meter>(
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<double, 1, 1>{0.0}, Eigen::Matrix<double, 1, 1>{1.0},
Eigen::Matrix<double, 1, 1>{1.0}, Eigen::Matrix<double, 1, 1>{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<units::meter>(
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};
}

120
sysid/src/main/native/cpp/analysis/FeedforwardAnalysis.cpp Normal file
View File

@@ -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 <cmath>
#include <units/math.h>
#include <units/time.h>
#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<PreparedData>& d,
const AnalysisType& type,
Eigen::Block<Eigen::MatrixXd> X,
Eigen::VectorBlock<Eigen::VectorXd> 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<std::vector<double>, 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<double> 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)};
}

417
sysid/src/main/native/cpp/analysis/FilteringUtils.cpp Normal file
View File

@@ -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 <limits>
#include <numbers>
#include <numeric>
#include <stdexcept>
#include <vector>
#include <fmt/format.h>
#include <frc/filter/LinearFilter.h>
#include <frc/filter/MedianFilter.h>
#include <units/math.h>
#include <wpi/StringExtras.h>
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<PreparedData>& 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<PreparedData>* 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<units::second_t, units::second_t, units::second_t>
sysid::TrimStepVoltageData(std::vector<PreparedData>* 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<PreparedData>& data, int window,
std::function<double(const PreparedData&)> accessorFunction) {
double sum = 0.0;
size_t step = window / 2;
auto averageFilter = frc::LinearFilter<double>::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<PreparedData>& data) {
std::vector<units::second_t> 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<units::second_t> 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<PreparedData>* data, int window) {
CheckSize(*data, window, "Median Filter");
frc::MedianFilter<double> 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<std::vector<PreparedData>>& 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<std::vector<PreparedData>>* data,
AnalysisManager::Settings* settings,
std::vector<units::second_t>& positionDelays,
std::vector<units::second_t>& 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<double>::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<std::vector<PreparedData>>* 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.");
}
}

165
sysid/src/main/native/cpp/analysis/JSONConverter.cpp Normal file
View File

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

48
sysid/src/main/native/cpp/analysis/OLS.cpp Normal file
View File

@@ -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 <tuple>
#include <vector>
#include <Eigen/Cholesky>
#include <Eigen/Core>
using namespace sysid;
std::tuple<std::vector<double>, 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};
}

47
sysid/src/main/native/cpp/analysis/SimpleMotorSim.cpp Normal file
View File

@@ -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 <frc/StateSpaceUtil.h>
#include <frc/system/Discretization.h>
#include <wpi/MathExtras.h>
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<double, 1> 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<double, 2, 2> Ad;
Eigen::Matrix<double, 2, 1> 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<double, 1> 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<double, 2>{position, velocity};
}

12
sysid/src/main/native/cpp/analysis/TrackWidthAnalysis.cpp Normal file
View File

@@ -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 <cmath>
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());
}

275
sysid/src/main/native/cpp/telemetry/TelemetryManager.cpp Normal file
View File

@@ -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 <algorithm>
#include <cctype> // for ::tolower
#include <numbers>
#include <stdexcept>
#include <string>
#include <utility>
#include <fmt/chrono.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/Logger.h>
#include <wpi/SmallVector.h>
#include <wpi/StringExtras.h>
#include <wpi/raw_ostream.h>
#include <wpi/timestamp.h>
#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<std::string_view, 16> res;
wpi::split(m_params.raw, res, ',');
// Convert each string to double.
std::vector<double> values;
values.reserve(res.size());
for (auto&& str : res) {
values.push_back(wpi::parse_float<double>(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<double> 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;
}

851
sysid/src/main/native/cpp/view/Analyzer.cpp Normal file
View File

@@ -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 <algorithm>
#include <exception>
#include <filesystem>
#include <numbers>
#include <thread>
#include <fmt/core.h>
#include <glass/Context.h>
#include <glass/Storage.h>
#include <imgui.h>
#include <imgui_internal.h>
#include <imgui_stdlib.h>
#include <wpi/json.h>
#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<pfd::open_file>(
"Select Data", "",
std::vector<std::string>{"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<AnalysisManager>(m_settings, m_logger);
m_location = "";
m_ff = std::vector<double>{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<AnalysisManager::Settings::DrivetrainDataset>(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<float>();
// 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<AnalysisManager>(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<double*>(&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<double*>(&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<double*>(&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<double*>(&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<int>(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<double*>(&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<double*>(&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<FeedbackControllerLoopType>(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();
}
}
}

531
sysid/src/main/native/cpp/view/AnalyzerPlot.cpp Normal file
View File

@@ -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 <algorithm>
#include <cmath>
#include <mutex>
#include <fmt/format.h>
#include <units/math.h>
#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<ImPlotPoint*>(data)[idx];
}
template <typename Model>
static std::vector<std::vector<ImPlotPoint>> PopulateTimeDomainSim(
const std::vector<PreparedData>& data,
const std::array<units::second_t, 4>& 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<std::vector<ImPlotPoint>> pts;
std::vector<ImPlotPoint> 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<PreparedData>& rawSlow,
const std::vector<PreparedData>& rawFast,
std::atomic<bool>& 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<bool>& 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<double>& ffGains,
const std::array<units::second_t, 4>& startTimes,
AnalysisType type, std::atomic<bool>& 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<std::vector<ImPlotPoint>>& 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<int>(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<void()> 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};
}

64
sysid/src/main/native/cpp/view/JSONConverter.cpp Normal file
View File

@@ -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 <exception>
#include <imgui.h>
#include <portable-file-dialogs.h>
#include <wpi/timestamp.h>
#include "sysid/Util.h"
using namespace sysid;
void JSONConverter::DisplayConverter(
const char* tooltip,
std::function<std::string(std::string_view, wpi::Logger&)> converter) {
if (ImGui::Button(tooltip)) {
m_opener = std::make_unique<pfd::open_file>(
tooltip, "", std::vector<std::string>{"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);
}

222
sysid/src/main/native/cpp/view/Logger.cpp Normal file
View File

@@ -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 <exception>
#include <numbers>
#include <glass/Context.h>
#include <glass/Storage.h>
#include <imgui.h>
#include <imgui_internal.h>
#include <imgui_stdlib.h>
#include <networktables/NetworkTable.h>
#include <units/angle.h>
#include <wpigui.h>
#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<TelemetryManager>(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<float>(*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<pfd::select_folder>("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();
}
}

89
sysid/src/main/native/include/sysid/Util.h Normal file
View File

@@ -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 <algorithm>
#include <array>
#include <filesystem>
#include <string>
#include <string_view>
// 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 <typename Type, size_t... Sizes>
constexpr auto ArrayConcat(const std::array<Type, Sizes>&... arrays) {
std::array<Type, (Sizes + ...)> result;
size_t index = 0;
((std::copy_n(arrays.begin(), Sizes, result.begin() + index), index += Sizes),
...);
return result;
}
} // namespace sysid

358
sysid/src/main/native/include/sysid/analysis/AnalysisManager.h Normal file
View File

@@ -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 <algorithm>
#include <array>
#include <exception>
#include <limits>
#include <numeric>
#include <optional>
#include <string>
#include <string_view>
#include <tuple>
#include <vector>
#include <units/time.h>
#include <wpi/Logger.h>
#include <wpi/json.h>
#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<std::vector<double>, double, double> ffGains;
/**
* Stores the trackwidth for angular drivetrain tests.
*/
std::optional<double> 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 <typename... Sources>
static std::vector<PreparedData> DataConcat(const Sources&... sources) {
std::vector<PreparedData> 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<double> 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<int>(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<int>(m_settings.dataset)];
}
/**
* Returns the original dataset.
*
* @return The original (untouched) dataset
*/
Storage& GetOriginalData() {
return m_originalDataset[static_cast<int>(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<units::second_t, 4>& GetStartTimes() const {
return m_startTimes;
}
bool HasData() const {
return !m_originalDataset[static_cast<int>(
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<Storage, 3> m_originalDataset;
std::array<Storage, 3> m_rawDataset;
std::array<Storage, 3> m_filteredDataset;
// Stores the various start times of the different tests.
std::array<units::second_t, 4> 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<double>::infinity()};
std::vector<units::second_t> m_positionDelays;
std::vector<units::second_t> m_velocityDelays;
// Stores an optional track width if we are doing the drivetrain angular test.
std::optional<double> m_trackWidth;
void PrepareGeneralData();
void PrepareAngularDrivetrainData();
void PrepareLinearDrivetrainData();
};
} // namespace sysid

63
sysid/src/main/native/include/sysid/analysis/AnalysisType.h Normal file
View File

@@ -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 <cstddef>
#include <string_view>
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

79
sysid/src/main/native/include/sysid/analysis/ArmSim.h Normal file
View File

@@ -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 <Eigen/Core>
#include <units/time.h>
#include <units/voltage.h>
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<double, 1, 1> m_A;
Eigen::Matrix<double, 1, 1> m_B;
Eigen::Vector<double, 1> m_c;
Eigen::Vector<double, 1> m_d;
Eigen::Vector<double, 2> m_x;
double m_offset;
};
} // namespace sysid

76
sysid/src/main/native/include/sysid/analysis/ElevatorSim.h Normal file
View File

@@ -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 <Eigen/Core>
#include <units/time.h>
#include <units/voltage.h>
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<double, 2, 2> m_A;
Eigen::Matrix<double, 2, 1> m_B;
Eigen::Vector<double, 2> m_c;
Eigen::Vector<double, 2> m_d;
Eigen::Vector<double, 2> m_x;
};
} // namespace sysid

80
sysid/src/main/native/include/sysid/analysis/FeedbackAnalysis.h Normal file
View File

@@ -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

164
sysid/src/main/native/include/sysid/analysis/FeedbackControllerPreset.h Normal file
View File

@@ -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 <units/time.h>
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

25
sysid/src/main/native/include/sysid/analysis/FeedforwardAnalysis.h Normal file
View File

@@ -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 <tuple>
#include <vector>
#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<std::vector<double>, double, double> CalculateFeedforwardGains(
const Storage& data, const AnalysisType& type);
} // namespace sysid

209
sysid/src/main/native/include/sysid/analysis/FilteringUtils.h Normal file
View File

@@ -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 <algorithm>
#include <cmath>
#include <exception>
#include <functional>
#include <string>
#include <string_view>
#include <tuple>
#include <utility>
#include <vector>
#include <frc/filter/LinearFilter.h>
#include <units/time.h>
#include <wpi/array.h>
#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<PreparedData>& data, int window,
std::function<double(const PreparedData&)> 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<PreparedData>* 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<units::second_t, units::second_t, units::second_t>
TrimStepVoltageData(std::vector<PreparedData>* 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<PreparedData>& 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
*
* <pre><code>
* CentralFiniteDifference<1, 2>(20_ms);
* </code></pre>
*
* @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 <int Derivative, int Samples>
frc::LinearFilter<double> 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<int, Samples> stencil{wpi::empty_array};
for (int i = 0; i < Samples; ++i) {
stencil[i] = -(Samples - 1) / 2 + i;
}
return frc::LinearFilter<double>::FiniteDifference<Derivative, Samples>(
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<std::vector<PreparedData>>* data,
AnalysisManager::Settings* settings,
std::vector<units::second_t>& positionDelays,
std::vector<units::second_t>& 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<std::vector<PreparedData>>* data);
} // namespace sysid

24
sysid/src/main/native/include/sysid/analysis/JSONConverter.h Normal file
View File

@@ -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 <string>
#include <string_view>
#include <wpi/Logger.h>
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

26
sysid/src/main/native/include/sysid/analysis/OLS.h Normal file
View File

@@ -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 <cstddef>
#include <tuple>
#include <vector>
#include <Eigen/Core>
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<std::vector<double>, double, double> OLS(const Eigen::MatrixXd& X,
const Eigen::VectorXd& y);
} // namespace sysid

74
sysid/src/main/native/include/sysid/analysis/SimpleMotorSim.h Normal file
View File

@@ -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 <Eigen/Core>
#include <units/time.h>
#include <units/voltage.h>
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<double, 2, 2> m_A;
Eigen::Matrix<double, 2, 1> m_B;
Eigen::Vector<double, 2> m_c;
Eigen::Vector<double, 2> m_x;
};
} // namespace sysid

95
sysid/src/main/native/include/sysid/analysis/Storage.h Normal file
View File

@@ -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 <vector>
#include <units/time.h>
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<PreparedData> slowForward;
std::vector<PreparedData> slowBackward;
/**
* Dataset for fast (aka dynamic) test
*/
std::vector<PreparedData> fastForward;
std::vector<PreparedData> fastBackward;
bool empty() const {
return slowForward.empty() || slowBackward.empty() || fastForward.empty() ||
fastBackward.empty();
}
};
} // namespace sysid

19
sysid/src/main/native/include/sysid/analysis/TrackWidthAnalysis.h Normal file
View File

@@ -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 <units/angle.h>
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

237
sysid/src/main/native/include/sysid/telemetry/TelemetryManager.h Normal file
View File

@@ -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 <array>
#include <cstddef>
#include <functional>
#include <memory>
#include <string>
#include <string_view>
#include <utility>
#include <vector>
#include <networktables/BooleanTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/IntegerTopic.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/StringTopic.h>
#include <units/time.h>
#include <wpi/Logger.h>
#include <wpi/SmallVector.h>
#include <wpi/json.h>
#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<void(std::string_view)> 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<std::vector<double>> 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<std::string> m_tests;
// Stores the test data.
wpi::json m_data;
// Display callbacks.
wpi::SmallVector<std::function<void(std::string_view)>, 1> m_callbacks;
// NetworkTables instance and entries.
nt::NetworkTableInstance m_inst;
std::shared_ptr<nt::NetworkTable> 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

261
sysid/src/main/native/include/sysid/view/Analyzer.h Normal file
View File

@@ -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 <functional>
#include <memory>
#include <optional>
#include <string>
#include <string_view>
#include <thread>
#include <vector>
#include <glass/View.h>
#include <implot.h>
#include <portable-file-dialogs.h>
#include <units/time.h>
#include <units/voltage.h>
#include <wpi/Logger.h>
#include <wpi/StringMap.h>
#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<FeedbackControllerPreset> m_presets;
int m_selectedLoopType = 1;
int m_selectedPreset = 0;
// Feedforward and feedback gains.
std::vector<double> m_ff;
double m_accelRSquared;
double m_accelRMSE;
double m_Kp;
double m_Kd;
units::millisecond_t m_timescale;
// Track width
std::optional<double> m_trackWidth;
// Units
int m_selectedOverrideUnit = 0;
double m_conversionFactor = 0.0;
// Data analysis
std::unique_ptr<AnalysisManager> 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<pfd::open_file> 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<bool> m_abortDataPrep{false};
std::thread m_dataThread;
};
} // namespace sysid

203
sysid/src/main/native/include/sysid/view/AnalyzerPlot.h Normal file
View File

@@ -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 <array>
#include <atomic>
#include <functional>
#include <string>
#include <string_view>
#include <vector>
#include <imgui.h>
#include <implot.h>
#include <units/time.h>
#include <wpi/Logger.h>
#include <wpi/spinlock.h>
#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<double>& ff,
const std::array<units::second_t, 4>& startTimes,
AnalysisType type, std::atomic<bool>& 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<PreparedData>& rawSlow,
const std::vector<PreparedData>& rawFast,
std::atomic<bool>& 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<bool>& 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<ImPlotPoint> rawData;
std::vector<ImPlotPoint> filteredData;
// Simulated time domain data
std::vector<std::vector<ImPlotPoint>> 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<ImPlotPoint> data;
std::array<ImPlotPoint, 2> 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<void()> 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

57
sysid/src/main/native/include/sysid/view/JSONConverter.h Normal file
View File

@@ -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 <functional>
#include <memory>
#include <string>
#include <string_view>
#include <glass/View.h>
#include <portable-file-dialogs.h>
#include <wpi/Logger.h>
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<std::string(std::string_view, wpi::Logger&)> converter);
wpi::Logger& m_logger;
std::string m_location;
std::unique_ptr<pfd::open_file> m_opener;
std::string m_exception;
double m_timestamp = 0;
};
} // namespace sysid

82
sysid/src/main/native/include/sysid/view/Logger.h Normal file
View File

@@ -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 <memory>
#include <string>
#include <glass/DataSource.h>
#include <glass/View.h>
#include <glass/networktables/NetworkTablesSettings.h>
#include <portable-file-dialogs.h>
#include <wpi/Logger.h>
#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<TelemetryManager> m_manager =
std::make_unique<TelemetryManager>(m_settings, m_logger);
std::unique_ptr<pfd::select_folder> 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

95
sysid/src/main/native/include/sysid/view/UILayout.h Normal file
View File

@@ -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 <imgui.h>
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

BIN
sysid/src/main/native/mac/sysid.icns Normal file
View File

Binary file not shown.

BIN
sysid/src/main/native/resources/sysid-128.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 8.5 KiB

BIN
sysid/src/main/native/resources/sysid-16.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 857 B

BIN
sysid/src/main/native/resources/sysid-256.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 16 KiB

BIN
sysid/src/main/native/resources/sysid-32.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.0 KiB

BIN
sysid/src/main/native/resources/sysid-48.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.4 KiB

BIN
sysid/src/main/native/resources/sysid-512.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 30 KiB

BIN
sysid/src/main/native/resources/sysid-64.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 4.4 KiB

BIN
sysid/src/main/native/win/sysid.ico Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 53 KiB

1
sysid/src/main/native/win/sysid.rc Normal file
View File

@@ -0,0 +1 @@
IDI_ICON1 ICON "sysid.ico"

11
sysid/src/test/native/cpp/Main.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}

18
sysid/src/test/native/cpp/analysis/AnalysisTypeTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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"));
}

105
sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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);
}

251
sysid/src/test/native/cpp/analysis/FeedforwardAnalysisTest.cpp Normal file
View File

@@ -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 <cmath>
#include <gtest/gtest.h>
#include <units/time.h>
#include <units/voltage.h>
#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 <typename Model>
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);
}

170
sysid/src/test/native/cpp/analysis/FilterTest.cpp Normal file
View File

@@ -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 <array>
#include <cmath>
#include <vector>
#include <gtest/gtest.h>
#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<sysid::PreparedData> 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<sysid::PreparedData> 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<sysid::PreparedData> 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<sysid::PreparedData> 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<sysid::PreparedData> 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 <int Derivative, int Samples, typename F, typename DfDx>
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<Derivative, Samples>(h);
for (int i = min / h.value(); i < max / h.value(); ++i) {
// Let filter initialize
if (i < static_cast<int>(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<int>((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);
}

42
sysid/src/test/native/cpp/analysis/OLSTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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

12
sysid/src/test/native/cpp/analysis/TrackWidthAnalysisTest.cpp Normal file
View File

@@ -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 <gtest/gtest.h>
#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);
}