diff --git a/.github/workflows/enforce-bump-flag.yml b/.github/workflows/enforce-bump-flag.yml new file mode 100644 index 0000000..697c1fc --- /dev/null +++ b/.github/workflows/enforce-bump-flag.yml @@ -0,0 +1,51 @@ +name: Enforce Version Bump Flag + +on: + pull_request: + types: [opened, edited, synchronize] + branches: + - main + - master + +jobs: + check-pr-description: + name: Check for bump flag + runs-on: ubuntu-latest + steps: + - name: Verify exactly one bump level in description + env: + PR_BODY: ${{ github.event.pull_request.body }} + run: | + MAJOR=0 + MINOR=0 + PATCH=0 + + # Check for Major variants + if echo "$PR_BODY" | grep -iqE '(bump version major|bump major version|#major)'; then + MAJOR=1 + fi + + # Check for Minor variants + if echo "$PR_BODY" | grep -iqE '(bump version minor|bump minor version|#minor)'; then + MINOR=1 + fi + + # Check for Patch variants + if echo "$PR_BODY" | grep -iqE '(bump version patch|bump patch version|#patch)'; then + PATCH=1 + fi + + # Calculate total unique levels found + TOTAL=$((MAJOR + MINOR + PATCH)) + + if [ "$TOTAL" -eq 1 ]; then + echo "✅ Exactly one version bump level found." + exit 0 + elif [ "$TOTAL" -eq 0 ]; then + echo "❌ Error: PR description must contain a version bump flag." + echo "Expected one of: #major, bump version minor, bump patch version, etc." + exit 1 + else + echo "❌ Error: Conflicting version bump levels found. Please specify ONLY major, minor, OR patch." + exit 1 + fi diff --git a/CMakeLists.txt b/CMakeLists.txt index 4628d1e..2bceeff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,6 +80,7 @@ FetchContent_MakeAvailable(jsd) ####### Build ####### add_subdirectory(src) +add_subdirectory(utils) ####### Test Suite ####### diff --git a/README.md b/README.md index e350a2f..593b1ef 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,7 @@ To learn more about fastcat, checkout the following documents: - [Fastcat Primer](doc/fastcat_primer.md) - [Complete list of Fastcat Device Configuration YAML Parameters](doc/fastcat_device_config_parameters.md) +- [Command-line Utilities](doc/utilities.md) — `jsd_slaveinfo`, `elmo_vel_profile`, `elmo_pos_profile` - 2021 Aeroconf paper submission `Fastcat: An Open-Source Library for Composable EtherCAT Control Systems` - README for build details @@ -57,6 +58,39 @@ $ cmake .. $ make ``` +### Network Port Access for EtherCAT + +The shipped utilities (`jsd_slaveinfo`, `elmo_vel_profile`, `elmo_pos_profile`) and any modules built using fastcat open raw EtherCAT sockets and need `CAP_NET_ADMIN` + `CAP_NET_RAW` permissions. +This can be achieved either by running the binary as the root user, or by using the `sudo setcap` command to permit non-root users to access the raw socket. +For example, you can permit a non-root user to run the `elmo_vel_profile` binary with the command: + +```bash +$ sudo setcap cap_net_admin,cap_net_raw=eip /absolute/path/to/build/bin/elmo_vel_profile +``` + +**The raw socketr permissions are cleared on every relink**, so you must re-run `sudo setcap` after each rebuild. + +#### Auto-setcap during build + +For dev machines, build with `-DAUTO_SETCAP=ON` to automatically run `sudo setcap` on `elmo_vel_profile` and `elmo_pos_profile` after each link: + +```bash +$ cmake -S . -B build -DAUTO_SETCAP=ON +$ cmake --build build -j +``` + +The option defaults to **OFF** so fastcat builds cleanly on machines without sudoers configured (CI, headless servers, etc.). `jsd_slaveinfo` is built by the JSD dependency and is not covered by this flag — set its capabilities manually. + +#### Password-less sudo for `setcap` + +`AUTO_SETCAP=ON` only works without prompting if `sudo setcap` is whitelisted in sudoers. Run `sudo visudo` and add a single line (replace `` with your account name): + +``` + ALL=(ALL) NOPASSWD: /usr/sbin/setcap +``` + +This narrowly grants password-less access to `setcap` only — `sudo` for any other command still prompts as usual. + ### Tests The following commands will execute the unit tests: @@ -174,4 +208,4 @@ Violations of these rules will be considered errors and should be patched immedi ## License -fastcat is licensed under the Apache License 2.0. See [LICENSE](LICENSE) for more details. \ No newline at end of file +fastcat is licensed under the Apache License 2.0. See [LICENSE](LICENSE) for more details. diff --git a/doc/utilities.md b/doc/utilities.md new file mode 100644 index 0000000..29568cf --- /dev/null +++ b/doc/utilities.md @@ -0,0 +1,208 @@ +# fastcat Utilities + +fastcat ships three command-line utilities. After a top-level build, all of +them land in `build/bin/`: + +| Binary | Purpose | +|---|---| +| `jsd_slaveinfo` | Enumerate EtherCAT slaves on a given interface | +| `elmo_vel_profile` | Run a trapezoidal velocity profile on a single actuator | +| `elmo_pos_profile` | Run a trapezoidal position profile on a single actuator | + +`jsd_slaveinfo` is built as part of fastcat's [JSD](https://github.com/nasa-jpl/jsd) +dependency. The two `elmo_*_profile` binaries are native fastcat utilities +under `utils/`. + +## Source layout + +| Path | Contents | +|---|---| +| `utils/profile_utils.{h,cpp}` | Logic shared by both profile tools: argument parsing, profile duration math, Elmo state-machine helpers, and the reset / control-loop / graceful-halt sequences | +| `utils/elmo_vel_profile.cpp` | `main()` for the velocity tool — CSV schema and the `ACTUATOR_PROF_VEL_CMD` it issues | +| `utils/elmo_pos_profile.cpp` | `main()` for the position tool — CSV schema and the `ACTUATOR_PROF_POS_CMD` it issues | +| `utils/test_profile_utils.cc` | GTest unit tests for the hardware-independent parts of `profile_utils` | + +The unit tests are registered with CTest alongside the main test suite and need +no hardware: + +``` +ctest -R test_profile_utils --output-on-failure +``` + +## Capabilities + +All three utilities open raw EtherCAT sockets, which requires `CAP_NET_ADMIN` +and `CAP_NET_RAW`. Grant the capabilities directly to the binary instead of +running under `sudo`: + +``` +sudo setcap cap_net_admin,cap_net_raw=eip build/bin/ +``` + +`setcap` requires the absolute path. **Capabilities are cleared on every +rebuild**, so re-run `setcap` after each `cmake --build`. + +### Auto-setcap during build (optional) + +For dev machines with password-less sudo configured, build with +`-DAUTO_SETCAP=ON` to automatically run `setcap` on the profile binaries +after each link: + +``` +cmake -S . -B build -DAUTO_SETCAP=ON +cmake --build build -j +``` + +The option defaults to **OFF** so that fastcat builds cleanly on machines +without sudoers configured (CI, headless servers, etc.). + +To enable password-less `setcap`, run `sudo visudo` and add: + +``` + ALL=(ALL) NOPASSWD: /usr/sbin/setcap +``` + +Note: `AUTO_SETCAP` only applies to `elmo_vel_profile` and `elmo_pos_profile`. +`jsd_slaveinfo` is built by the JSD dependency and is not covered by this +option — set its capabilities manually after each rebuild. + +--- + +## `jsd_slaveinfo` + +Enumerates EtherCAT slaves on the given network interface and prints a +summary of each one (vendor ID, product code, configured PDOs, etc.). Useful +for confirming the bus is wired correctly and that every expected drive shows +up before launching a fastcat-based application. + +### Usage + +``` +./build/bin/jsd_slaveinfo +``` + +Example: + +``` +./build/bin/jsd_slaveinfo eth_ecat +``` + +For more detailed documentation see the JSD project's own +[README](https://github.com/nasa-jpl/jsd). + +--- + +## `elmo_vel_profile` + +Runs a trapezoidal velocity profile on one actuator described by a fastcat +YAML config. The profile accelerates from rest to a cruise speed, holds for a +specified duration, then decelerates back to zero. Telemetry is streamed to a +timestamped CSV file in the current working directory. + +### Usage + +``` +./build/bin/elmo_vel_profile +``` + +| Argument | Description | +|---|---| +| `config` | Path to fastcat YAML config (relative to CWD) | +| `actuator_name` | Must match a `name:` field of a device in the YAML | +| `accel` | Trapezoid acceleration / deceleration (rad/s²) | +| `cruise_speed` | Cruise velocity (rad/s) | +| `cruise_duration` | Hold time at cruise speed (s) | + +### Example + +``` +cd build/bin +./elmo_vel_profile ../../example_configs/single_elmo.yaml gold_act_1 2.0 5.0 3.0 +``` + +### Telemetry output + +Written to `__vel_prof_telem.csv` in CWD with one row per +control tick: + +| Column | Units | Description | +|---|---|---| +| `unix_time_s` | seconds since 1970-01-01 UTC | Wall-clock timestamp for correlating with other telemetry sources | +| `relative_time_s` | seconds | Time since the start of the control loop | +| `cmd_velocity_rad_s` | rad/s | Velocity commanded by the trapezoid generator | +| `actual_velocity_rad_s` | rad/s | Velocity reported by the drive (heavily quantized at low encoder resolution) | +| `actual_current_A` | A | Motor current reported by the drive | + +--- + +## `elmo_pos_profile` + +Runs a trapezoidal position profile on one actuator. The trapezoid shape is +chosen automatically based on the requested distance: triangular if the move +is short enough that the drive can't reach `max_velocity` before having to +decelerate, otherwise trapezoidal with a cruise phase at `max_velocity`. +Negative `relative_position` values move in the negative direction. + +### Usage + +``` +./build/bin/elmo_pos_profile +``` + +| Argument | Description | +|---|---| +| `config` | Path to fastcat YAML config (relative to CWD) | +| `actuator_name` | Must match a `name:` field of a device in the YAML | +| `accel` | Acceleration / deceleration (rad/s²) | +| `max_velocity` | Cruise velocity cap (rad/s) | +| `relative_position` | Position change relative to the current position (rad); may be negative | + +### Example + +``` +cd build/bin +./elmo_pos_profile ../../example_configs/single_elmo.yaml gold_act_1 2.0 5.0 3.0 +./elmo_pos_profile ../../example_configs/single_elmo.yaml gold_act_1 2.0 5.0 -3.0 +``` + +### Telemetry output + +Written to `__pos_prof_telem.csv` in CWD with one row per +control tick: + +| Column | Units | Description | +|---|---|---| +| `unix_time_s` | seconds since 1970-01-01 UTC | Wall-clock timestamp | +| `relative_time_s` | seconds | Time since the start of the control loop | +| `cmd_position_rad` | rad | Position commanded by the trapezoid generator | +| `actual_position_rad` | rad | Position reported by the drive | +| `actual_velocity_rad_s` | rad/s | Velocity reported by the drive (noisy at low resolution) | +| `actual_current_A` | A | Motor current reported by the drive | + +--- + +## Loop rate guidance + +Both profile utilities take their loop rate from the YAML's +`target_loop_rate_hz`. **64 Hz is the recommended minimum.** Each binary +prints a warning at startup if the configured rate is below 64 Hz, but does +not refuse to run. + +The reason is encoder quantization noise on the velocity feedback signal. +For a typical low-resolution encoder (42 counts per revolution), the +single-tick velocity resolution is roughly `2π × loop_rate / counts_per_rev`: + +| Loop rate | Velocity resolution per encoder count | +|---|---| +| 32 Hz | ~4.8 rad/s | +| 64 Hz | ~2.4 rad/s | +| 128 Hz | ~1.2 rad/s | + +At 32 Hz on a 42 cpr encoder, the quantization noise on `actual_velocity` is +large enough to trip the Elmo drive's internal speed-tracking limit during a +position profile (CSP mode), even when the *actual* mechanical velocity is +well within bounds. 64 Hz is sufficient headroom for the Elmo's default +tracking thresholds; higher rates further suppress quantization noise. + +This applies equally to higher-resolution encoders — the noise is just less +visible because each count corresponds to a smaller velocity step. diff --git a/utils/CMakeLists.txt b/utils/CMakeLists.txt new file mode 100644 index 0000000..b5bd3f5 --- /dev/null +++ b/utils/CMakeLists.txt @@ -0,0 +1,47 @@ +message("Building fastcat profile utilities") + +include_directories( + ${CMAKE_BINARY_DIR}/include + ${YAMLCPP_INCLUDE_DIR} + ) + +add_library(profile_utils STATIC profile_utils.cpp) +target_include_directories(profile_utils PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(profile_utils fastcat ${YAMLCPP_LIBRARY} pthread rt) + +add_executable(elmo_vel_profile elmo_vel_profile.cpp) +target_link_libraries(elmo_vel_profile profile_utils) + +add_executable(elmo_pos_profile elmo_pos_profile.cpp) +target_link_libraries(elmo_pos_profile profile_utils) + +option(AUTO_SETCAP "Run sudo setcap on profile utilities post-build" OFF) +if(AUTO_SETCAP) + foreach(tgt elmo_vel_profile elmo_pos_profile) + add_custom_command(TARGET ${tgt} POST_BUILD + COMMAND sudo setcap cap_net_admin,cap_net_raw=eip $ + COMMENT "Setting capabilities on ${tgt}") + endforeach() +endif() + +####### Unit tests ####### +# The profile utilities are unit tested independently of the top-level test +# suite so that everything for these tools lives under utils/. +if(BUILD_TESTS AND CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) + find_package(GTest) + find_package(Threads REQUIRED) + if(GTEST_FOUND) + # utils/ is added before the root enable_testing(), so opt in here + enable_testing() + add_executable(test_profile_utils test_profile_utils.cc) + target_link_libraries(test_profile_utils PRIVATE + ${GTEST_BOTH_LIBRARIES} + Threads::Threads + profile_utils + ) + target_include_directories(test_profile_utils PUBLIC ${GTEST_INCLUDE_DIRS}) + add_test(NAME test_profile_utils COMMAND test_profile_utils) + else() + message(WARNING "GTest not found, profile utility tests will not be built") + endif() +endif() diff --git a/utils/elmo_pos_profile.cpp b/utils/elmo_pos_profile.cpp new file mode 100644 index 0000000..dcceabd --- /dev/null +++ b/utils/elmo_pos_profile.cpp @@ -0,0 +1,183 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include "fastcat/fastcat.h" +#include "profile_utils.h" + +namespace +{ +// Extra time beyond the computed profile duration to stay in the RUN phase, so +// the drive has slack to settle on target before we halt it. +constexpr double kRunMarginS = 1.0; + +void PrintUsage(const char* program) +{ + std::cerr << "Usage: " << program + << " " + "" + << std::endl; + std::cerr << " config_path: relative path to fastcat YAML config" + << std::endl; + std::cerr << " actuator_name: name of actuator in YAML" << std::endl; + std::cerr << " accel: acceleration/deceleration (rad/s²)" << std::endl; + std::cerr << " max_velocity: maximum velocity (rad/s)" << std::endl; + std::cerr << " relative_position: position change (rad, can be negative)" + << std::endl; +} + +} // namespace + +int main(int argc, char** argv) +{ + profile_utils::PosProfileArgs args; + std::string error; + if (!profile_utils::ParsePosProfileArgs( + std::vector(argv + 1, argv + argc), args, error)) { + std::cerr << "Error: " << error << std::endl; + PrintUsage(argv[0]); + return 1; + } + + YAML::Node node; + try { + node = YAML::LoadFile(args.config_path); + } catch (const YAML::Exception& e) { + std::cerr << "Error loading YAML file '" << args.config_path + << "': " << e.what() << std::endl; + return 1; + } + + if (!profile_utils::ValidateActuatorName(node, args.actuator_name)) { + return 1; + } + + profile_utils::InstallSignalHandlers(); + + fastcat::Manager mgr; + std::cout << "Initializing fastcat manager..." << std::endl; + if (!mgr.ConfigFromYaml(node)) { + std::cerr << "Error: Failed to configure manager from YAML" << std::endl; + return 1; + } + + double loop_rate = mgr.GetTargetLoopRate(); + std::cout << "Loop rate: " << loop_rate << " Hz" << std::endl; + if (loop_rate < profile_utils::kMinRecommendedLoopRateHz) { + std::cerr << "Warning: Loop rate " << loop_rate + << " Hz is below minimum recommended " + << profile_utils::kMinRecommendedLoopRateHz << " Hz" << std::endl; + } + + if (!profile_utils::RunResetPhase(mgr, args.actuator_name, loop_rate)) { + mgr.Shutdown(); + return 1; + } + + std::string csv_filename = profile_utils::MakeTelemetryFilename("pos_prof"); + std::ofstream csv_file(csv_filename); + if (!csv_file.is_open()) { + std::cerr << "Error: Failed to open CSV file: " << csv_filename + << std::endl; + mgr.Shutdown(); + return 1; + } + + csv_file << "unix_time_s,relative_time_s,cmd_position_rad,actual_position_" + "rad,actual_velocity_rad_s,actual_current_A" + << std::endl; + std::cout << "Writing telemetry to: " << csv_filename << std::endl; + + double distance = std::abs(args.relative_position); + double expected_duration = profile_utils::TrapezoidalMoveDuration( + distance, args.accel, args.max_velocity); + + // Resolved in ISSUE against the measured position, then read back by + // on_run_complete to report the residual position error. + double target_position = 0.0; + + profile_utils::ProfileHooks hooks; + hooks.run_ticks = profile_utils::TicksFromSeconds( + expected_duration + kRunMarginS, loop_rate); + + hooks.issue = [&](const fastcat::GoldActuatorState& act_state) { + double initial_position = act_state.actual_position; + target_position = initial_position + args.relative_position; + + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_PROF_POS_CMD; + cmd.actuator_prof_pos_cmd.target_position = target_position; + cmd.actuator_prof_pos_cmd.profile_velocity = args.max_velocity; + cmd.actuator_prof_pos_cmd.profile_accel = args.accel; + // Absolute, since we resolved the target against the measured position + cmd.actuator_prof_pos_cmd.relative = 0; + + std::cout << "Profile position command issued" << std::endl; + std::cout << " Initial position: " << initial_position << " rad" + << std::endl; + std::cout << " Target position: " << target_position << " rad" + << std::endl; + std::cout << " Relative move: " << args.relative_position << " rad" + << std::endl; + return cmd; + }; + + hooks.write_csv_row = [&](const fastcat::GoldActuatorState& act_state, + double unix_time_s, double elapsed_s) { + csv_file << std::fixed << std::setprecision(6) << unix_time_s << "," + << elapsed_s << "," << act_state.cmd_position << "," + << act_state.actual_position << "," << act_state.actual_velocity + << "," << act_state.actual_current << std::endl; + }; + + hooks.log_run_tick = [](const fastcat::GoldActuatorState& act_state, + double elapsed_s) { + std::cout << " t=" << std::fixed << std::setprecision(2) << elapsed_s + << "s pos=" << std::setprecision(3) << act_state.actual_position + << " rad vel=" << act_state.actual_velocity + << " rad/s current=" << act_state.actual_current << " A" + << std::endl; + }; + + hooks.on_run_complete = [&](const fastcat::GoldActuatorState& act_state) { + std::cout << " Final position: " << act_state.actual_position << " rad" + << std::endl; + std::cout << " Position error: " + << (target_position - act_state.actual_position) << " rad" + << std::endl; + }; + + std::cout << "\nStarting control loop at " << loop_rate << " Hz..." + << std::endl; + std::cout << "Profile parameters: accel=" << args.accel + << " rad/s², max_vel=" << args.max_velocity + << " rad/s, distance=" << distance << " rad" << std::endl; + std::cout << "Expected motion duration: " << expected_duration << " s" + << std::endl; + + profile_utils::ProfileResult result = + profile_utils::RunProfileLoop(mgr, args.actuator_name, loop_rate, hooks); + + csv_file.close(); + + profile_utils::HaltAndSettle(mgr, args.actuator_name, loop_rate); + + std::cout << "\nShutting down..." << std::endl; + mgr.Shutdown(); + + std::cout << "\n=== Summary ===" << std::endl; + std::cout << "Commanded move: " << std::fixed << std::setprecision(3) + << args.relative_position << " rad" << std::endl; + std::cout << "Peak cmd velocity: " << result.peak_cmd_velocity << " rad/s" + << std::endl; + std::cout << "Peak actual velocity: " << result.peak_actual_velocity + << " rad/s (noisy at low encoder resolution)" << std::endl; + std::cout << "Telemetry saved to: " << csv_filename << std::endl; + + return 0; +} diff --git a/utils/elmo_vel_profile.cpp b/utils/elmo_vel_profile.cpp new file mode 100644 index 0000000..7466ae9 --- /dev/null +++ b/utils/elmo_vel_profile.cpp @@ -0,0 +1,156 @@ +#include +#include +#include +#include +#include + +#include + +#include "fastcat/fastcat.h" +#include "profile_utils.h" + +namespace +{ +// Extra time beyond the computed profile duration to stay in the RUN phase, so +// the drive has slack to finish decelerating before we halt it. +constexpr double kRunMarginS = 0.5; + +void PrintUsage(const char* program) +{ + std::cerr << "Usage: " << program + << " " + "" + << std::endl; + std::cerr << " config_path: relative path to fastcat YAML config" + << std::endl; + std::cerr << " actuator_name: name of actuator in YAML" << std::endl; + std::cerr << " accel: acceleration/deceleration (rad/s²)" << std::endl; + std::cerr << " cruise_speed: cruise velocity (rad/s, may be negative)" + << std::endl; + std::cerr << " cruise_duration: hold time at cruise (s)" << std::endl; +} + +} // namespace + +int main(int argc, char** argv) +{ + profile_utils::VelProfileArgs args; + std::string error; + if (!profile_utils::ParseVelProfileArgs( + std::vector(argv + 1, argv + argc), args, error)) { + std::cerr << "Error: " << error << std::endl; + PrintUsage(argv[0]); + return 1; + } + + YAML::Node node; + try { + node = YAML::LoadFile(args.config_path); + } catch (const YAML::Exception& e) { + std::cerr << "Error loading YAML file '" << args.config_path + << "': " << e.what() << std::endl; + return 1; + } + + if (!profile_utils::ValidateActuatorName(node, args.actuator_name)) { + return 1; + } + + profile_utils::InstallSignalHandlers(); + + fastcat::Manager mgr; + std::cout << "Initializing fastcat manager..." << std::endl; + if (!mgr.ConfigFromYaml(node)) { + std::cerr << "Error: Failed to configure manager from YAML" << std::endl; + return 1; + } + + double loop_rate = mgr.GetTargetLoopRate(); + std::cout << "Loop rate: " << loop_rate << " Hz" << std::endl; + if (loop_rate < profile_utils::kMinRecommendedLoopRateHz) { + std::cerr << "Warning: Loop rate " << loop_rate + << " Hz is below minimum recommended " + << profile_utils::kMinRecommendedLoopRateHz << " Hz" << std::endl; + } + + if (!profile_utils::RunResetPhase(mgr, args.actuator_name, loop_rate)) { + mgr.Shutdown(); + return 1; + } + + std::string csv_filename = profile_utils::MakeTelemetryFilename("vel_prof"); + std::ofstream csv_file(csv_filename); + if (!csv_file.is_open()) { + std::cerr << "Error: Failed to open CSV file: " << csv_filename + << std::endl; + mgr.Shutdown(); + return 1; + } + + csv_file << "unix_time_s,relative_time_s,cmd_velocity_rad_s,actual_velocity_" + "rad_s,actual_current_A" + << std::endl; + std::cout << "Writing telemetry to: " << csv_filename << std::endl; + + double expected_duration = profile_utils::VelocityProfileDuration( + args.accel, args.cruise_speed, args.cruise_duration); + + profile_utils::ProfileHooks hooks; + hooks.run_ticks = profile_utils::TicksFromSeconds( + expected_duration + kRunMarginS, loop_rate); + + hooks.issue = [&](const fastcat::GoldActuatorState&) { + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_PROF_VEL_CMD; + cmd.actuator_prof_vel_cmd.target_velocity = args.cruise_speed; + cmd.actuator_prof_vel_cmd.profile_accel = args.accel; + cmd.actuator_prof_vel_cmd.max_duration = args.cruise_duration; + + std::cout << "Profile velocity command issued" << std::endl; + return cmd; + }; + + hooks.write_csv_row = [&](const fastcat::GoldActuatorState& act_state, + double unix_time_s, double elapsed_s) { + csv_file << std::fixed << std::setprecision(6) << unix_time_s << "," + << elapsed_s << "," << act_state.cmd_velocity << "," + << act_state.actual_velocity << "," << act_state.actual_current + << std::endl; + }; + + hooks.log_run_tick = [](const fastcat::GoldActuatorState& act_state, + double elapsed_s) { + std::cout << " t=" << std::fixed << std::setprecision(2) << elapsed_s + << "s vel=" << std::setprecision(3) << act_state.actual_velocity + << " rad/s current=" << act_state.actual_current << " A" + << std::endl; + }; + + std::cout << "\nStarting control loop at " << loop_rate << " Hz..." + << std::endl; + std::cout << "Profile parameters: accel=" << args.accel + << " rad/s², cruise=" << args.cruise_speed + << " rad/s, duration=" << args.cruise_duration << " s" << std::endl; + std::cout << "Expected motion duration: " << expected_duration << " s" + << std::endl; + + profile_utils::ProfileResult result = + profile_utils::RunProfileLoop(mgr, args.actuator_name, loop_rate, hooks); + + csv_file.close(); + + profile_utils::HaltAndSettle(mgr, args.actuator_name, loop_rate); + + std::cout << "\nShutting down..." << std::endl; + mgr.Shutdown(); + + std::cout << "\n=== Summary ===" << std::endl; + std::cout << "Peak cmd velocity: " << std::fixed << std::setprecision(3) + << result.peak_cmd_velocity << " rad/s" << std::endl; + std::cout << "Peak actual velocity: " << std::fixed << std::setprecision(3) + << result.peak_actual_velocity + << " rad/s (noisy at low encoder resolution)" << std::endl; + std::cout << "Telemetry saved to: " << csv_filename << std::endl; + + return 0; +} diff --git a/utils/profile_utils.cpp b/utils/profile_utils.cpp new file mode 100644 index 0000000..870de13 --- /dev/null +++ b/utils/profile_utils.cpp @@ -0,0 +1,550 @@ +#include "profile_utils.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "jsd/jsd_elmo_common.h" + +namespace profile_utils +{ +std::atomic g_shutdown{false}; + +namespace +{ +void SignalHandler(int signum) +{ + std::cout << "\nReceived signal " << signum << ", shutting down..." + << std::endl; + g_shutdown = true; +} + +// "SWITCH_ON_DISABLED (0x40)" +std::string DescribeElmoState(uint32_t elmo_state_machine_state) +{ + std::ostringstream oss; + oss << jsd_elmo_state_machine_state_to_string( + static_cast( + elmo_state_machine_state)) + << " (0x" << std::hex << elmo_state_machine_state << std::dec << ")"; + return oss.str(); +} + +} // namespace + +void InstallSignalHandlers() +{ + std::signal(SIGINT, SignalHandler); + std::signal(SIGTERM, SignalHandler); +} + +bool IsSafeElmoState(uint32_t elmo_state_machine_state) +{ + for (auto safe_state : kSafeElmoStates) { + if (elmo_state_machine_state == static_cast(safe_state)) { + return true; + } + } + return false; +} + +int TicksFromSeconds(double seconds, double loop_rate_hz) +{ + return static_cast(seconds * loop_rate_hz); +} + +std::vector CollectDeviceNames(const YAML::Node& node) +{ + std::vector names; + if (!node["buses"]) { + return names; + } + for (const auto& bus : node["buses"]) { + if (!bus["devices"]) { + continue; + } + for (const auto& device : bus["devices"]) { + if (device["name"]) { + names.push_back(device["name"].as()); + } + } + } + return names; +} + +bool ValidateActuatorName(const YAML::Node& node, + const std::string& actuator_name) +{ + std::vector available_names = CollectDeviceNames(node); + for (const auto& name : available_names) { + if (name == actuator_name) { + return true; + } + } + + std::cerr << "Error: Actuator '" << actuator_name + << "' not found in YAML config." << std::endl; + std::cerr << "Available actuator names: "; + for (size_t i = 0; i < available_names.size(); ++i) { + std::cerr << "'" << available_names[i] << "'"; + if (i < available_names.size() - 1) std::cerr << ", "; + } + std::cerr << std::endl; + return false; +} + +std::string MakeTelemetryFilename(const std::string& tag, std::time_t when) +{ + std::tm tm_when; + localtime_r(&when, &tm_when); + + std::ostringstream oss; + oss << std::put_time(&tm_when, "%Y%m%d_%H%M%S") << "_" << tag + << "_telem.csv"; + return oss.str(); +} + +std::string MakeTelemetryFilename(const std::string& tag) +{ + return MakeTelemetryFilename( + tag, std::chrono::system_clock::to_time_t( + std::chrono::system_clock::now())); +} + +double TrapezoidalMoveDuration(double distance, double accel, + double max_velocity) +{ + double accel_time = max_velocity / accel; + double accel_distance = 0.5 * accel * accel_time * accel_time; + + if (distance < 2.0 * accel_distance) { + // Triangular profile - max_velocity is never reached + return 2.0 * std::sqrt(distance / accel); + } + double cruise_distance = distance - 2.0 * accel_distance; + return 2.0 * accel_time + cruise_distance / max_velocity; +} + +double VelocityProfileDuration(double accel, double cruise_speed, + double cruise_duration) +{ + double ramp_time = std::abs(cruise_speed) / accel; + return 2.0 * ramp_time + cruise_duration; +} + +bool ParseDouble(const std::string& text, double& out) +{ + if (text.empty()) { + return false; + } + try { + size_t consumed = 0; + double value = std::stod(text, &consumed); + if (consumed != text.size()) { + return false; + } + out = value; + return true; + } catch (const std::logic_error&) { + return false; + } +} + +namespace +{ +// Fills `out` from `args[index]`, or sets `error` naming the offending field. +bool ParseArg(const std::vector& args, size_t index, + const char* field, double& out, std::string& error) +{ + if (!ParseDouble(args[index], out)) { + error = std::string(field) + ": '" + args[index] + "' is not a number"; + return false; + } + return true; +} + +constexpr size_t kExpectedArgCount = 5; + +bool CheckArgCount(const std::vector& args, std::string& error) +{ + if (args.size() != kExpectedArgCount) { + error = "expected " + std::to_string(kExpectedArgCount) + + " arguments, got " + std::to_string(args.size()); + return false; + } + return true; +} + +} // namespace + +bool ParseVelProfileArgs(const std::vector& args, + VelProfileArgs& out, std::string& error) +{ + if (!CheckArgCount(args, error)) { + return false; + } + + out.config_path = args[0]; + out.actuator_name = args[1]; + if (!ParseArg(args, 2, "accel", out.accel, error) || + !ParseArg(args, 3, "cruise_speed", out.cruise_speed, error) || + !ParseArg(args, 4, "cruise_duration", out.cruise_duration, error)) { + return false; + } + + if (out.accel <= 0) { + error = "acceleration must be positive"; + return false; + } + if (out.cruise_speed == 0) { + error = "cruise_speed must be non-zero"; + return false; + } + if (out.cruise_duration <= 0) { + error = "cruise_duration must be positive"; + return false; + } + return true; +} + +bool ParsePosProfileArgs(const std::vector& args, + PosProfileArgs& out, std::string& error) +{ + if (!CheckArgCount(args, error)) { + return false; + } + + out.config_path = args[0]; + out.actuator_name = args[1]; + if (!ParseArg(args, 2, "accel", out.accel, error) || + !ParseArg(args, 3, "max_velocity", out.max_velocity, error) || + !ParseArg(args, 4, "relative_position", out.relative_position, error)) { + return false; + } + + if (out.accel <= 0) { + error = "acceleration must be positive"; + return false; + } + if (out.max_velocity <= 0) { + error = "max_velocity must be positive"; + return false; + } + if (out.relative_position == 0) { + error = "relative_position cannot be zero"; + return false; + } + return true; +} + +const fastcat::GoldActuatorState* FindGoldActuatorState( + const std::vector& states, const std::string& name) +{ + for (const auto& state : states) { + if (state.name == name && state.type == fastcat::GOLD_ACTUATOR_STATE) { + return &state.gold_actuator_state; + } + } + return nullptr; +} + +bool RunResetPhase(fastcat::Manager& mgr, const std::string& actuator_name, + double loop_rate_hz) +{ + const auto period = std::chrono::duration(1.0 / loop_rate_hz); + + std::cout << "Resetting actuator '" << actuator_name << "'..." << std::endl; + + fastcat::DeviceCmd reset_cmd; + reset_cmd.name = actuator_name; + reset_cmd.type = fastcat::ACTUATOR_RESET_CMD; + mgr.QueueCommand(reset_cmd); + + auto reset_start = std::chrono::steady_clock::now(); + uint32_t last_elmo_state = kNoStateLogged; + for (int i = 0; i < kResetTicks; ++i) { + if (!mgr.Process()) { + std::cerr << "Error: Manager process failed during reset phase at " + "iteration " + << i << std::endl; + std::cerr << "This likely indicates a hardware/communication issue with " + "the drive." + << std::endl; + std::cerr << "Check: 1) Motor is connected 2) Drive has power 3) " + "EtherCAT cable is secure" + << std::endl; + return false; + } + + auto states = mgr.GetDeviceStates(); + auto act_state = FindGoldActuatorState(states, actuator_name); + if (act_state && act_state->elmo_state_machine_state != last_elmo_state) { + std::cout << " [warmup tick " << i << "] elmo_sms=" + << DescribeElmoState(act_state->elmo_state_machine_state) + << " act_sms=0x" << std::hex + << act_state->actuator_state_machine_state << std::dec + << " servo=" << (int)act_state->servo_enabled + << " motor_on=" << (int)act_state->motor_on + << " jsd_fault=" << act_state->jsd_fault_code << std::endl; + last_elmo_state = act_state->elmo_state_machine_state; + } + std::this_thread::sleep_for(period); + } + + double reset_duration = std::chrono::duration( + std::chrono::steady_clock::now() - reset_start) + .count(); + std::cout << "Reset phase completed in " << std::fixed << std::setprecision(1) + << reset_duration << "s, proceeding to warmup..." << std::endl; + return true; +} + +void HaltAndSettle(fastcat::Manager& mgr, const std::string& actuator_name, + double loop_rate_hz) +{ + const auto period = std::chrono::duration(1.0 / loop_rate_hz); + const int max_settle_ticks = + TicksFromSeconds(kHaltSettleTimeoutS, loop_rate_hz); + + std::cout << "\nGraceful halt: waiting for drive to reach safe state..." + << std::endl; + { + fastcat::DeviceCmd halt_cmd; + halt_cmd.name = actuator_name; + halt_cmd.type = fastcat::ACTUATOR_HALT_CMD; + mgr.QueueCommand(halt_cmd); + } + + uint32_t last_logged_sms = kNoStateLogged; + bool reached_safe = false; + uint32_t final_sms = 0; + for (int i = 0; i < max_settle_ticks; ++i) { + // Ignore Process() return - the drive may transit through fault states + // (e.g. QUICK_STOP_ACTIVE) on the way down, and jsd_egd will auto-issue + // FAULT_RESET to walk it back to SWITCH_ON_DISABLED. + mgr.Process(); + + auto states = mgr.GetDeviceStates(); + auto act_state = FindGoldActuatorState(states, actuator_name); + if (act_state) { + final_sms = act_state->elmo_state_machine_state; + if (final_sms != last_logged_sms) { + std::cout << " shutdown tick " << i << ": elmo_sms=" + << DescribeElmoState(final_sms) << std::endl; + last_logged_sms = final_sms; + } + reached_safe = IsSafeElmoState(final_sms); + } + if (reached_safe) break; + std::this_thread::sleep_for(period); + } + + if (reached_safe) { + std::cout << "Drive reached safe state (elmo_sms=" + << DescribeElmoState(final_sms) + << "); next run can start cleanly." << std::endl; + } else { + std::cerr << "Warning: drive did not reach a safe state within " + << kHaltSettleTimeoutS << "s (last elmo_sms=" + << DescribeElmoState(final_sms) << ")." << std::endl; + std::cerr << " If next run reports 'off nominal', power-cycle the drive " + "first." + << std::endl; + } +} + +ProfileResult RunProfileLoop(fastcat::Manager& mgr, + const std::string& actuator_name, + double loop_rate_hz, const ProfileHooks& hooks) +{ + enum Phase { ISSUE, BRAKE_DISENGAGE, RUN, RECOVER, DONE }; + + const auto period = std::chrono::duration(1.0 / loop_rate_hz); + const int brake_disengage_ticks = + TicksFromSeconds(kBrakeDisengageTimeoutS, loop_rate_hz); + const int recover_settle_ticks = + TicksFromSeconds(kRecoverSettleS, loop_rate_hz); + + Phase phase = ISSUE; + int tick_count = 0; + int run_tick_count = 0; + int retry_count = 0; + int recover_tick_count = 0; + bool motor_ready = false; + + ProfileResult result; + + auto start_time = std::chrono::steady_clock::now(); + auto next_tick = start_time + std::chrono::duration_cast< + std::chrono::steady_clock::duration>(period); + + while (phase != DONE && !g_shutdown) { + // Process the EtherCAT cycle. If a fault occurred and retries remain, drop + // into RECOVER. + if (!mgr.Process()) { + if (phase == RUN) { + std::cerr << "Error: Bus faulted during RUN phase" << std::endl; + phase = DONE; + } else if (phase != RECOVER && retry_count < kMaxRecoverRetries) { + std::cerr << "Bus faulted during " + << (phase == ISSUE ? "ISSUE" : "BRAKE_DISENGAGE") + << " (attempt " << (retry_count + 1) << "/" + << kMaxRecoverRetries << "), entering RECOVER..." + << std::endl; + phase = RECOVER; + recover_tick_count = 0; + } else { + std::cerr << "Error: Manager process failed and retries exhausted" + << std::endl; + break; + } + } + + auto states = mgr.GetDeviceStates(); + auto act_state = FindGoldActuatorState(states, actuator_name); + if (!act_state) { + std::cerr << "Error: Could not find actuator state for '" << actuator_name + << "'" << std::endl; + break; + } + + double elapsed_time = std::chrono::duration( + std::chrono::steady_clock::now() - start_time) + .count(); + double unix_time_s = + std::chrono::duration( + std::chrono::system_clock::now().time_since_epoch()) + .count(); + + hooks.write_csv_row(*act_state, unix_time_s, elapsed_time); + + if (std::abs(act_state->actual_velocity) > result.peak_actual_velocity) { + result.peak_actual_velocity = std::abs(act_state->actual_velocity); + } + if (std::abs(act_state->cmd_velocity) > result.peak_cmd_velocity) { + result.peak_cmd_velocity = std::abs(act_state->cmd_velocity); + } + + switch (phase) { + case ISSUE: { + fastcat::DeviceCmd cmd = hooks.issue(*act_state); + cmd.name = actuator_name; + mgr.QueueCommand(cmd); + + std::cout << "Waiting for brake disengagement and motor enable..." + << std::endl; + phase = BRAKE_DISENGAGE; + tick_count = 0; + break; + } + + case BRAKE_DISENGAGE: + if (!motor_ready && act_state->servo_enabled && act_state->motor_on) { + std::cout << "Motor enabled (servo_enabled=1, motor_on=1)" + << std::endl; + motor_ready = true; + std::cout << "Executing motion profile..." << std::endl; + phase = RUN; + run_tick_count = 0; + break; + } + + if (tick_count % kBrakeStatusPrintInterval == 0 && tick_count > 0) { + std::cout << " Waiting for motor enable: servo=" + << (int)act_state->servo_enabled + << " motor_on=" << (int)act_state->motor_on << " state=0x" + << std::hex << act_state->actuator_state_machine_state + << std::dec << std::endl; + } + + // Check for faults - the top-of-loop branch handles bus-level failures + if (mgr.IsFaulted()) { + std::cerr << "Manager faulted during brake disengagement" + << std::endl; + std::cerr << " Fault code: " << act_state->fastcat_fault_code + << " EMCY: 0x" << std::hex << act_state->emcy_error_code + << std::dec << " elmo_sms=" + << DescribeElmoState(act_state->elmo_state_machine_state) + << std::endl; + if (retry_count < kMaxRecoverRetries) { + phase = RECOVER; + recover_tick_count = 0; + } else { + std::cerr << "Retries exhausted." << std::endl; + phase = DONE; + } + break; + } + + tick_count++; + if (tick_count >= brake_disengage_ticks) { + std::cerr << "Error: Motor did not enable within " + << kBrakeDisengageTimeoutS << "s" << std::endl; + std::cerr << "servo_enabled=" << (int)act_state->servo_enabled + << " motor_on=" << (int)act_state->motor_on << " state=0x" + << std::hex << act_state->actuator_state_machine_state + << std::dec << std::endl; + phase = DONE; + } + break; + + case RUN: + // The trapezoid is self-driving once issued - do not re-queue the + // motion command or we restart the profile every tick. + if (run_tick_count % kRunStatusPrintInterval == 0) { + hooks.log_run_tick(*act_state, elapsed_time); + } + + run_tick_count++; + if (run_tick_count >= hooks.run_ticks) { + std::cout << "Motion profile complete" << std::endl; + if (hooks.on_run_complete) { + hooks.on_run_complete(*act_state); + } + phase = DONE; + } + + if (mgr.IsFaulted()) { + std::cerr << "Error: Manager reported fault" << std::endl; + phase = DONE; + } + break; + + case RECOVER: + if (recover_tick_count == 0) { + std::cout << "RECOVER: calling ExecuteAllDeviceResets (retry " + << (retry_count + 1) << "/" << kMaxRecoverRetries << ")" + << std::endl; + mgr.ExecuteAllDeviceResets(); + } + recover_tick_count++; + if (recover_tick_count >= recover_settle_ticks) { + retry_count++; + std::cout << "RECOVER: settle complete, reissuing motion command" + << " elmo_sms=" + << DescribeElmoState(act_state->elmo_state_machine_state) + << " servo=" << (int)act_state->servo_enabled + << " motor_on=" << (int)act_state->motor_on << std::endl; + motor_ready = false; + tick_count = 0; + phase = ISSUE; + } + break; + + case DONE: + break; + } + + std::this_thread::sleep_until(next_tick); + next_tick += std::chrono::duration_cast< + std::chrono::steady_clock::duration>(period); + } + + return result; +} + +} // namespace profile_utils diff --git a/utils/profile_utils.h b/utils/profile_utils.h new file mode 100644 index 0000000..b0bf69d --- /dev/null +++ b/utils/profile_utils.h @@ -0,0 +1,176 @@ +#ifndef FASTCAT_UTILS_PROFILE_UTILS_H_ +#define FASTCAT_UTILS_PROFILE_UTILS_H_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "fastcat/fastcat.h" +#include "jsd/jsd_elmo_common_types.h" + +// Logic shared by the elmo_vel_profile and elmo_pos_profile utilities. The +// argument parsing and profile math here are free of hardware dependencies so +// they can be exercised by utils/test_profile_utils.cc; the Run*() helpers +// drive a fastcat::Manager and require a live bus. +namespace profile_utils +{ +// Elmo CiA-402 states that IsMotionFaultConditionMet() treats as nominal, i.e. +// states a subsequent run can issue a motion command from without tripping the +// off-nominal check. +constexpr jsd_elmo_state_machine_state_t kSafeElmoStates[] = { + JSD_ELMO_STATE_MACHINE_STATE_SWITCH_ON_DISABLED, + JSD_ELMO_STATE_MACHINE_STATE_READY_TO_SWITCH_ON, + JSD_ELMO_STATE_MACHINE_STATE_SWITCHED_ON, +}; + +// Loop rate below which encoder quantization noise on actual_velocity is large +// enough to trip the drive's internal speed tracking limit. See +// doc/utilities.md, "Loop rate guidance". +constexpr double kMinRecommendedLoopRateHz = 64.0; + +// Bus cycles pumped after ACTUATOR_RESET_CMD to let the reset take effect. +constexpr int kResetTicks = 30; + +// Time allowed for the brake to disengage and the servo to enable. +constexpr double kBrakeDisengageTimeoutS = 2.0; + +// Settle time between ExecuteAllDeviceResets() and reissuing the motion command. +constexpr double kRecoverSettleS = 0.5; + +// Attempts to recover from the empirically observed SWITCHED_ON race before +// giving up. +constexpr int kMaxRecoverRetries = 5; + +// Time allowed after ACTUATOR_HALT_CMD for the drive to walk itself down to one +// of kSafeElmoStates. +constexpr double kHaltSettleTimeoutS = 3.0; + +// Tick intervals for the periodic console prints. +constexpr int kBrakeStatusPrintInterval = 50; +constexpr int kRunStatusPrintInterval = 10; + +// Sentinel meaning "no state logged yet", so the first observed state always +// prints. No real elmo_state_machine_state takes this value. +constexpr uint32_t kNoStateLogged = 0xFFFFFFFF; + +// Set by the SIGINT/SIGTERM handler installed by InstallSignalHandlers(). +extern std::atomic g_shutdown; + +void InstallSignalHandlers(); + +// True if the drive has settled somewhere a subsequent run can start from. +bool IsSafeElmoState(uint32_t elmo_state_machine_state); + +// Bus cycles spanning `seconds` at `loop_rate_hz`. +int TicksFromSeconds(double seconds, double loop_rate_hz); + +// Every device `name` declared under buses[].devices[] in a fastcat config. +std::vector CollectDeviceNames(const YAML::Node& node); + +// True if `actuator_name` names a device in `node`. On failure the available +// names are reported on stderr. +bool ValidateActuatorName(const YAML::Node& node, + const std::string& actuator_name); + +// "___telem.csv" in local time. +std::string MakeTelemetryFilename(const std::string& tag, std::time_t when); +std::string MakeTelemetryFilename(const std::string& tag); + +// Duration of a trapezoidal position move, collapsing to a triangular profile +// when `distance` is too short to reach `max_velocity`. +double TrapezoidalMoveDuration(double distance, double accel, + double max_velocity); + +// Duration of a velocity profile: ramp up, hold, ramp back down to rest. +double VelocityProfileDuration(double accel, double cruise_speed, + double cruise_duration); + +// Parses `text` as a double, rejecting trailing garbage and empty input. +bool ParseDouble(const std::string& text, double& out); + +struct VelProfileArgs { + std::string config_path; + std::string actuator_name; + double accel = 0.0; + double cruise_speed = 0.0; + double cruise_duration = 0.0; +}; + +struct PosProfileArgs { + std::string config_path; + std::string actuator_name; + double accel = 0.0; + double max_velocity = 0.0; + double relative_position = 0.0; +}; + +// Parse and range-check the command line. `args` excludes the program name. On +// failure `error` is set to a message describing the first problem found. +bool ParseVelProfileArgs(const std::vector& args, + VelProfileArgs& out, std::string& error); +bool ParsePosProfileArgs(const std::vector& args, + PosProfileArgs& out, std::string& error); + +// The named actuator's state, or nullptr if it is not a gold actuator or not +// present in `states`. +const fastcat::GoldActuatorState* FindGoldActuatorState( + const std::vector& states, const std::string& name); + +// Queues ACTUATOR_RESET_CMD and pumps kResetTicks cycles to clear stale faults, +// logging Elmo state transitions. False if the bus faulted, in which case the +// caller should shut down rather than proceed to the profile. +bool RunResetPhase(fastcat::Manager& mgr, const std::string& actuator_name, + double loop_rate_hz); + +// Queues ACTUATOR_HALT_CMD and pumps cycles until the Elmo state machine +// settles in one of kSafeElmoStates, so the next run starts cleanly. Warns on +// stderr if it does not settle within kHaltSettleTimeoutS. +void HaltAndSettle(fastcat::Manager& mgr, const std::string& actuator_name, + double loop_rate_hz); + +// The per-tool parts of the control loop run by RunProfileLoop(). +struct ProfileHooks { + // Bus cycles to stay in the RUN phase before declaring the profile complete. + int run_ticks = 0; + + // Motion command to queue on entering ISSUE, given the actuator's state at + // that moment. Called again on each recovery retry. + std::function issue; + + // Appends one telemetry row. + std::function + write_csv_row; + + // Periodic RUN phase console print, every kRunStatusPrintInterval ticks. + std::function + log_run_tick; + + // Optional extra console output once the RUN phase finishes. + std::function on_run_complete; +}; + +struct ProfileResult { + // actual_velocity is heavily quantized on low-resolution encoders (42 + // counts/rev x 64 Hz ~ 9.6 rad/s/count); the commanded value is the cleaner + // indicator that the trapezoid actually ran. + double peak_cmd_velocity = 0.0; + double peak_actual_velocity = 0.0; +}; + +// Runs the ISSUE -> BRAKE_DISENGAGE -> RUN phase machine at `loop_rate_hz`, +// recovering from faults up to kMaxRecoverRetries times, until the profile +// completes, the bus gives up, or g_shutdown is set. +ProfileResult RunProfileLoop(fastcat::Manager& mgr, + const std::string& actuator_name, + double loop_rate_hz, const ProfileHooks& hooks); + +} // namespace profile_utils + +#endif diff --git a/utils/test_profile_utils.cc b/utils/test_profile_utils.cc new file mode 100644 index 0000000..32d2f65 --- /dev/null +++ b/utils/test_profile_utils.cc @@ -0,0 +1,305 @@ +#include + +#include +#include +#include + +#include "profile_utils.h" + +namespace +{ +using profile_utils::PosProfileArgs; +using profile_utils::VelProfileArgs; + +TEST(IsSafeElmoState, AcceptsEveryStateInTheSafeList) +{ + for (auto safe_state : profile_utils::kSafeElmoStates) { + EXPECT_TRUE(profile_utils::IsSafeElmoState( + static_cast(safe_state))) + << "0x" << std::hex << safe_state; + } +} + +TEST(IsSafeElmoState, RejectsFaultAndQuickStopStates) +{ + EXPECT_FALSE(profile_utils::IsSafeElmoState( + JSD_ELMO_STATE_MACHINE_STATE_NOT_READY_TO_SWITCH_ON)); + EXPECT_FALSE(profile_utils::IsSafeElmoState( + JSD_ELMO_STATE_MACHINE_STATE_OPERATION_ENABLED)); + EXPECT_FALSE(profile_utils::IsSafeElmoState( + JSD_ELMO_STATE_MACHINE_STATE_QUICK_STOP_ACTIVE)); + EXPECT_FALSE(profile_utils::IsSafeElmoState( + JSD_ELMO_STATE_MACHINE_STATE_FAULT_REACTION_ACTIVE)); + EXPECT_FALSE( + profile_utils::IsSafeElmoState(JSD_ELMO_STATE_MACHINE_STATE_FAULT)); + EXPECT_FALSE(profile_utils::IsSafeElmoState(profile_utils::kNoStateLogged)); +} + +TEST(TicksFromSeconds, TruncatesTowardsZero) +{ + EXPECT_EQ(128, profile_utils::TicksFromSeconds(2.0, 64.0)); + EXPECT_EQ(32, profile_utils::TicksFromSeconds(0.5, 64.0)); + // 0.3 * 64 = 19.2 ticks + EXPECT_EQ(19, profile_utils::TicksFromSeconds(0.3, 64.0)); + EXPECT_EQ(0, profile_utils::TicksFromSeconds(0.0, 64.0)); +} + +const char* kTwoBusConfig = R"( +target_loop_rate_hz: 100 +buses: + - type: jsd_bus + ifname: eth_ecat + devices: + - device_class: GoldActuator + name: gold_act_1 + - device_class: GoldActuator + name: gold_act_2 + - type: fastcat_bus + devices: + - device_class: SignalGenerator + name: sig_gen_1 +)"; + +TEST(CollectDeviceNames, ReturnsNamesFromEveryBusInOrder) +{ + std::vector names = + profile_utils::CollectDeviceNames(YAML::Load(kTwoBusConfig)); + ASSERT_EQ(3u, names.size()); + EXPECT_EQ("gold_act_1", names[0]); + EXPECT_EQ("gold_act_2", names[1]); + EXPECT_EQ("sig_gen_1", names[2]); +} + +TEST(CollectDeviceNames, ToleratesMissingBusesAndDevicesKeys) +{ + EXPECT_TRUE( + profile_utils::CollectDeviceNames(YAML::Load("target_loop_rate_hz: 100")) + .empty()); + EXPECT_TRUE(profile_utils::CollectDeviceNames( + YAML::Load("buses:\n - type: jsd_bus\n")) + .empty()); +} + +TEST(ValidateActuatorName, AcceptsNamePresentOnAnyBus) +{ + YAML::Node node = YAML::Load(kTwoBusConfig); + EXPECT_TRUE(profile_utils::ValidateActuatorName(node, "gold_act_1")); + EXPECT_TRUE(profile_utils::ValidateActuatorName(node, "sig_gen_1")); +} + +TEST(ValidateActuatorName, RejectsUnknownName) +{ + YAML::Node node = YAML::Load(kTwoBusConfig); + EXPECT_FALSE(profile_utils::ValidateActuatorName(node, "gold_act_3")); + EXPECT_FALSE(profile_utils::ValidateActuatorName(node, "")); +} + +TEST(MakeTelemetryFilename, EmbedsLocalTimestampAndTag) +{ + std::tm tm_local = {}; + tm_local.tm_year = 2026 - 1900; + tm_local.tm_mon = 8; // September + tm_local.tm_mday = 2; + tm_local.tm_hour = 14; + tm_local.tm_min = 5; + tm_local.tm_sec = 6; + tm_local.tm_isdst = -1; + std::time_t when = std::mktime(&tm_local); + + EXPECT_EQ("20260902_140506_pos_prof_telem.csv", + profile_utils::MakeTelemetryFilename("pos_prof", when)); + EXPECT_EQ("20260902_140506_vel_prof_telem.csv", + profile_utils::MakeTelemetryFilename("vel_prof", when)); +} + +TEST(TrapezoidalMoveDuration, TrapezoidalWhenMaxVelocityIsReached) +{ + // accel=2, max_vel=4 -> 2s ramps covering 4 rad each. A 20 rad move leaves + // 12 rad of cruise at 4 rad/s = 3s, so 2 + 3 + 2 = 7s. + EXPECT_NEAR(7.0, profile_utils::TrapezoidalMoveDuration(20.0, 2.0, 4.0), + 1e-9); +} + +TEST(TrapezoidalMoveDuration, TriangularWhenMoveIsTooShortToReachMaxVelocity) +{ + // Same ramps need 8 rad to reach max_vel; a 4 rad move peaks early at + // 2*sqrt(4/2) = 2.828s. + EXPECT_NEAR(2.0 * std::sqrt(4.0 / 2.0), + profile_utils::TrapezoidalMoveDuration(4.0, 2.0, 4.0), 1e-9); +} + +TEST(TrapezoidalMoveDuration, IsContinuousAcrossTheProfileShapeBoundary) +{ + // At exactly 2 * accel_distance the two branches must agree. + const double accel = 2.0, max_velocity = 4.0; + const double boundary_distance = + max_velocity * max_velocity / accel; // 2 * 0.5 * v^2 / a + EXPECT_NEAR(profile_utils::TrapezoidalMoveDuration( + boundary_distance - 1e-9, accel, max_velocity), + profile_utils::TrapezoidalMoveDuration(boundary_distance, accel, + max_velocity), + 1e-6); +} + +TEST(TrapezoidalMoveDuration, GrowsMonotonicallyWithDistance) +{ + double previous = 0.0; + for (double distance = 0.5; distance < 30.0; distance += 0.5) { + double duration = profile_utils::TrapezoidalMoveDuration(distance, 2.0, 4.0); + EXPECT_GT(duration, previous) << "distance=" << distance; + previous = duration; + } +} + +TEST(VelocityProfileDuration, SumsBothRampsAndTheCruiseHold) +{ + // accel=2, cruise=6 -> 3s ramp up + 5s hold + 3s ramp down + EXPECT_NEAR(11.0, profile_utils::VelocityProfileDuration(2.0, 6.0, 5.0), + 1e-9); +} + +TEST(VelocityProfileDuration, IsUnaffectedByCruiseSpeedSign) +{ + EXPECT_NEAR(profile_utils::VelocityProfileDuration(2.0, 6.0, 5.0), + profile_utils::VelocityProfileDuration(2.0, -6.0, 5.0), 1e-9); +} + +TEST(ParseDouble, AcceptsWellFormedNumbers) +{ + double value = 0.0; + EXPECT_TRUE(profile_utils::ParseDouble("2.5", value)); + EXPECT_DOUBLE_EQ(2.5, value); + EXPECT_TRUE(profile_utils::ParseDouble("-3", value)); + EXPECT_DOUBLE_EQ(-3.0, value); + EXPECT_TRUE(profile_utils::ParseDouble("1e-3", value)); + EXPECT_DOUBLE_EQ(0.001, value); +} + +TEST(ParseDouble, RejectsGarbageInsteadOfSilentlyYieldingZero) +{ + double value = 0.0; + EXPECT_FALSE(profile_utils::ParseDouble("", value)); + EXPECT_FALSE(profile_utils::ParseDouble("abc", value)); + EXPECT_FALSE(profile_utils::ParseDouble("2.5rad", value)); + EXPECT_FALSE(profile_utils::ParseDouble("--2", value)); +} + +TEST(ParseVelProfileArgs, AcceptsNominalArguments) +{ + VelProfileArgs args; + std::string error; + ASSERT_TRUE(profile_utils::ParseVelProfileArgs( + {"cfg.yaml", "gold_act_1", "2.0", "-5.0", "3.0"}, args, error)) + << error; + EXPECT_EQ("cfg.yaml", args.config_path); + EXPECT_EQ("gold_act_1", args.actuator_name); + EXPECT_DOUBLE_EQ(2.0, args.accel); + EXPECT_DOUBLE_EQ(-5.0, args.cruise_speed); + EXPECT_DOUBLE_EQ(3.0, args.cruise_duration); +} + +TEST(ParseVelProfileArgs, RejectsWrongArgumentCount) +{ + VelProfileArgs args; + std::string error; + EXPECT_FALSE(profile_utils::ParseVelProfileArgs({}, args, error)); + EXPECT_FALSE(error.empty()); + EXPECT_FALSE(profile_utils::ParseVelProfileArgs( + {"cfg.yaml", "gold_act_1", "2.0", "5.0"}, args, error)); + EXPECT_FALSE(profile_utils::ParseVelProfileArgs( + {"cfg.yaml", "gold_act_1", "2.0", "5.0", "3.0", "extra"}, args, error)); +} + +TEST(ParseVelProfileArgs, RejectsOutOfRangeValues) +{ + VelProfileArgs args; + std::string error; + EXPECT_FALSE(profile_utils::ParseVelProfileArgs( + {"cfg.yaml", "a", "0", "5.0", "3.0"}, args, error)); + EXPECT_EQ("acceleration must be positive", error); + EXPECT_FALSE(profile_utils::ParseVelProfileArgs( + {"cfg.yaml", "a", "-1", "5.0", "3.0"}, args, error)); + EXPECT_EQ("acceleration must be positive", error); + EXPECT_FALSE(profile_utils::ParseVelProfileArgs( + {"cfg.yaml", "a", "2.0", "0", "3.0"}, args, error)); + EXPECT_EQ("cruise_speed must be non-zero", error); + EXPECT_FALSE(profile_utils::ParseVelProfileArgs( + {"cfg.yaml", "a", "2.0", "5.0", "0"}, args, error)); + EXPECT_EQ("cruise_duration must be positive", error); +} + +TEST(ParseVelProfileArgs, ReportsWhichFieldWasNotANumber) +{ + VelProfileArgs args; + std::string error; + EXPECT_FALSE(profile_utils::ParseVelProfileArgs( + {"cfg.yaml", "a", "2.0", "fast", "3.0"}, args, error)); + EXPECT_NE(std::string::npos, error.find("cruise_speed")) << error; +} + +TEST(ParsePosProfileArgs, AcceptsNominalArguments) +{ + PosProfileArgs args; + std::string error; + ASSERT_TRUE(profile_utils::ParsePosProfileArgs( + {"cfg.yaml", "gold_act_1", "2.0", "5.0", "-3.0"}, args, error)) + << error; + EXPECT_EQ("cfg.yaml", args.config_path); + EXPECT_EQ("gold_act_1", args.actuator_name); + EXPECT_DOUBLE_EQ(2.0, args.accel); + EXPECT_DOUBLE_EQ(5.0, args.max_velocity); + EXPECT_DOUBLE_EQ(-3.0, args.relative_position); +} + +fastcat::DeviceState MakeDeviceState(const std::string& name, + fastcat::DeviceStateType type) +{ + fastcat::DeviceState state; + state.name = name; + state.type = type; + return state; +} + +TEST(FindGoldActuatorState, ReturnsTheMatchingGoldActuator) +{ + std::vector states = { + MakeDeviceState("sig_gen_1", fastcat::SIGNAL_GENERATOR_STATE), + MakeDeviceState("gold_act_1", fastcat::GOLD_ACTUATOR_STATE), + MakeDeviceState("gold_act_2", fastcat::GOLD_ACTUATOR_STATE), + }; + states[1].gold_actuator_state.actual_position = 1.25; + + const fastcat::GoldActuatorState* found = + profile_utils::FindGoldActuatorState(states, "gold_act_1"); + ASSERT_NE(nullptr, found); + EXPECT_DOUBLE_EQ(1.25, found->actual_position); +} + +TEST(FindGoldActuatorState, ReturnsNullptrOnNameOrTypeMismatch) +{ + std::vector states = { + MakeDeviceState("sig_gen_1", fastcat::SIGNAL_GENERATOR_STATE), + MakeDeviceState("gold_act_1", fastcat::GOLD_ACTUATOR_STATE), + }; + // Right name, wrong device class + EXPECT_EQ(nullptr, profile_utils::FindGoldActuatorState(states, "sig_gen_1")); + EXPECT_EQ(nullptr, profile_utils::FindGoldActuatorState(states, "no_such")); + EXPECT_EQ(nullptr, profile_utils::FindGoldActuatorState({}, "gold_act_1")); +} + +TEST(ParsePosProfileArgs, RejectsOutOfRangeValues) +{ + PosProfileArgs args; + std::string error; + EXPECT_FALSE(profile_utils::ParsePosProfileArgs( + {"cfg.yaml", "a", "0", "5.0", "3.0"}, args, error)); + EXPECT_EQ("acceleration must be positive", error); + EXPECT_FALSE(profile_utils::ParsePosProfileArgs( + {"cfg.yaml", "a", "2.0", "-5.0", "3.0"}, args, error)); + EXPECT_EQ("max_velocity must be positive", error); + EXPECT_FALSE(profile_utils::ParsePosProfileArgs( + {"cfg.yaml", "a", "2.0", "5.0", "0"}, args, error)); + EXPECT_EQ("relative_position cannot be zero", error); +} + +} // namespace