From 6464ff66d9cb8debe645ec28f2b57a2cfd48002b Mon Sep 17 00:00:00 2001 From: Alex Brinkman Date: Thu, 22 Dec 2022 13:53:12 -0800 Subject: [PATCH 1/7] fix yaml parsing test input yaml typo --- test/test_unit/test_yaml_parser_yamls/yaml_parser_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/test_unit/test_yaml_parser_yamls/yaml_parser_config.yaml b/test/test_unit/test_yaml_parser_yamls/yaml_parser_config.yaml index b8e4c13..fd85811 100644 --- a/test/test_unit/test_yaml_parser_yamls/yaml_parser_config.yaml +++ b/test/test_unit/test_yaml_parser_yamls/yaml_parser_config.yaml @@ -6,6 +6,6 @@ double_ex: 3.14159 float_ex: 2.71828 string_ex: "fastcat" int32_ex: -25 -uint8_ex: q +uint8_ex: 1 uint32_ex: 256 -bool_ex: true \ No newline at end of file +bool_ex: true From 3854d4d5572f3957601eab43e4951ad4a5d6c0ac Mon Sep 17 00:00:00 2001 From: Alex Brinkman Date: Thu, 22 Dec 2022 15:46:56 -0800 Subject: [PATCH 2/7] fastcat manager no longer flushes the command queue when faulted --- src/jsd/actuator.cc | 115 +++++++++++++++++++++++++------------------- src/manager.cc | 7 --- 2 files changed, 66 insertions(+), 56 deletions(-) diff --git a/src/jsd/actuator.cc b/src/jsd/actuator.cc index 700268e..d4a83f9 100644 --- a/src/jsd/actuator.cc +++ b/src/jsd/actuator.cc @@ -304,51 +304,11 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) return (sdoResult == SDO_RET_VAL_SUCCESS); } + // Honor these Non-motion Commands even when faulted switch (cmd.type) { - case ACTUATOR_CSP_CMD: - if (!HandleNewCSPCmd(cmd)) { - ERROR("Failed to handle Actuator CSP command"); - return false; - } - break; - - case ACTUATOR_CSV_CMD: - if (!HandleNewCSVCmd(cmd)) { - ERROR("Failed to handle Actuator CSV command"); - return false; - } - break; - - case ACTUATOR_CST_CMD: - if (!HandleNewCSTCmd(cmd)) { - ERROR("Failed to handle Actuator CST command"); - return false; - } - break; - - case ACTUATOR_PROF_POS_CMD: - if (!HandleNewProfPosCmd(cmd)) { - ERROR("Failed to setup Actuator Profiled Pos command"); - return false; - } - break; - - case ACTUATOR_PROF_VEL_CMD: - if (!HandleNewProfVelCmd(cmd)) { - ERROR("Failed to setup Actuator Profiled Pos command"); - return false; - } - break; - - case ACTUATOR_PROF_TORQUE_CMD: - if (!HandleNewProfTorqueCmd(cmd)) { - ERROR("Failed to setup Actuator Profiled Pos command"); - return false; - } - break; - case ACTUATOR_RESET_CMD: Reset(); + return true; break; case ACTUATOR_HALT_CMD: @@ -365,19 +325,13 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) } break; - case ACTUATOR_CALIBRATE_CMD: - if (!HandleNewCalibrationCmd(cmd)) { - ERROR("Failed to handle Calibrate Command"); - return false; - } - break; - case ACTUATOR_SET_MAX_CURRENT_CMD: // This application may choose to set this during motions // in order to boost current during accelration/decel // phases so don't check the state machine peak_current_limit_amps_ = cmd.actuator_set_max_current_cmd.current; EgdSetPeakCurrent(peak_current_limit_amps_); + return true; break; case ACTUATOR_SDO_SET_UNIT_MODE_CMD: @@ -395,6 +349,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) EgdSetGainSchedulingMode( JSD_EGD_GAIN_SCHEDULING_MODE_DISABLED, cmd.actuator_sdo_disable_gain_scheduling_cmd.app_id); + return true; break; } @@ -406,6 +361,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) EgdSetGainSchedulingMode( JSD_EGD_GAIN_SCHEDULING_MODE_SPEED, cmd.actuator_sdo_enable_speed_gain_scheduling_cmd.app_id); + return true; break; } @@ -417,6 +373,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) EgdSetGainSchedulingMode( JSD_EGD_GAIN_SCHEDULING_MODE_POSITION, cmd.actuator_sdo_enable_position_gain_scheduling_cmd.app_id); + return true; break; } @@ -428,6 +385,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) EgdSetGainSchedulingMode( JSD_EGD_GAIN_SCHEDULING_MODE_MANUAL_LOW, cmd.actuator_sdo_enable_manual_gain_scheduling_cmd.app_id); + return true; break; } @@ -438,8 +396,67 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) } EgdSetGainSchedulingIndex( cmd.actuator_set_gain_scheduling_index_cmd.gain_scheduling_index); + return true; break; } + } + + + // Return early if a fault is active + // So as to not honor these motion commands when faulted + if(device_fault_active_){ + return false; + } + + switch (cmd.type) { + case ACTUATOR_CSP_CMD: + if (!HandleNewCSPCmd(cmd)) { + ERROR("Failed to handle Actuator CSP command"); + return false; + } + break; + + case ACTUATOR_CSV_CMD: + if (!HandleNewCSVCmd(cmd)) { + ERROR("Failed to handle Actuator CSV command"); + return false; + } + break; + + case ACTUATOR_CST_CMD: + if (!HandleNewCSTCmd(cmd)) { + ERROR("Failed to handle Actuator CST command"); + return false; + } + break; + + case ACTUATOR_PROF_POS_CMD: + if (!HandleNewProfPosCmd(cmd)) { + ERROR("Failed to setup Actuator Profiled Pos command"); + return false; + } + break; + + case ACTUATOR_PROF_VEL_CMD: + if (!HandleNewProfVelCmd(cmd)) { + ERROR("Failed to setup Actuator Profiled Pos command"); + return false; + } + break; + + case ACTUATOR_PROF_TORQUE_CMD: + if (!HandleNewProfTorqueCmd(cmd)) { + ERROR("Failed to setup Actuator Profiled Pos command"); + return false; + } + break; + + case ACTUATOR_CALIBRATE_CMD: + if (!HandleNewCalibrationCmd(cmd)) { + ERROR("Failed to handle Calibrate Command"); + return false; + } + break; default: WARNING("That command type is not supported in this mode!"); diff --git a/src/manager.cc b/src/manager.cc index d27f4c6..41cc938 100644 --- a/src/manager.cc +++ b/src/manager.cc @@ -655,13 +655,6 @@ bool fastcat::Manager::ConfigOfflineBusFromYaml(YAML::Node node) bool fastcat::Manager::WriteCommands() { - if (faulted_) { - // Clear device command queue - while (!cmd_queue_->empty()) { - cmd_queue_->pop(); - } - return true; - } while (!cmd_queue_->empty()) { DeviceCmd cmd = cmd_queue_->front(); From 768adcefbd2715e4fc5c777cb5e77448c9788a0a Mon Sep 17 00:00:00 2001 From: Alex Brinkman Date: Thu, 22 Dec 2022 17:01:54 -0800 Subject: [PATCH 3/7] preventing fts taring and a few other commands when in faulted --- src/fastcat_devices/fts.cc | 13 ++++++++++--- src/fastcat_devices/pid.cc | 5 +++++ src/fcgen/commander.cc.cog | 6 ++++++ src/jsd/actuator.cc | 3 +++ src/jsd/ati_fts.cc | 21 ++++++++++++++------- 5 files changed, 38 insertions(+), 10 deletions(-) diff --git a/src/fastcat_devices/fts.cc b/src/fastcat_devices/fts.cc index 0f6a0bd..9d2dcd2 100644 --- a/src/fastcat_devices/fts.cc +++ b/src/fastcat_devices/fts.cc @@ -153,10 +153,17 @@ fastcat::FaultType fastcat::Fts::Process() bool fastcat::Fts::Write(DeviceCmd& cmd) { if (cmd.type == FTS_TARE_CMD) { - for (int ii = 0; ii < FC_FTS_N_DIMS; ii++) { - sig_offset_[ii] = -wrench_[ii]; + + // Do not permit taring if there is a fault + if(device_fault_active_){ + ERROR("Taring FTS is not permited with a active fault, reset first"); + return false; + }else{ + for (int ii = 0; ii < FC_FTS_N_DIMS; ii++) { + sig_offset_[ii] = -wrench_[ii]; + } + return true; } - return true; } else if (cmd.type == FTS_ENABLE_GUARD_FAULT_CMD) { enable_fts_guard_fault_ = cmd.fts_enable_guard_fault_cmd.enable; diff --git a/src/fastcat_devices/pid.cc b/src/fastcat_devices/pid.cc index ad8855d..b6f61de 100644 --- a/src/fastcat_devices/pid.cc +++ b/src/fastcat_devices/pid.cc @@ -114,6 +114,11 @@ bool fastcat::Pid::Write(DeviceCmd& cmd) return false; } + if(device_fault_active_){ + ERROR("Unable to Activate PID (%s) with an active fault, reset first", name_.c_str()); + return false; + } + state_->pid_state.active = true; pid_activate_cmd_ = cmd.pid_activate_cmd; activation_time_ = state_->time; diff --git a/src/fcgen/commander.cc.cog b/src/fcgen/commander.cc.cog index 65f5747..29a0644 100644 --- a/src/fcgen/commander.cc.cog +++ b/src/fcgen/commander.cc.cog @@ -179,6 +179,12 @@ bool fastcat::Commander::Read() bool fastcat::Commander::Write(DeviceCmd& cmd) { + + if(device_fault_active_){ + ERROR("Commander (%s) cannot be commanded with an active fault, reset first", name_.c_str()); + return false; + } + if (cmd.type == COMMANDER_ENABLE_CMD) { state_->commander_state.enable = true; enable_duration_ = cmd.commander_enable_cmd.duration; diff --git a/src/jsd/actuator.cc b/src/jsd/actuator.cc index d4a83f9..c0e6974 100644 --- a/src/jsd/actuator.cc +++ b/src/jsd/actuator.cc @@ -399,6 +399,9 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) return true; break; } + default: + // no-op + break; } diff --git a/src/jsd/ati_fts.cc b/src/jsd/ati_fts.cc index 4488dc6..2050115 100644 --- a/src/jsd/ati_fts.cc +++ b/src/jsd/ati_fts.cc @@ -141,13 +141,20 @@ bool fastcat::AtiFts::Write(DeviceCmd& cmd) } if (cmd.type == FTS_TARE_CMD) { - bias_[0] = -state_->fts_state.raw_fx; - bias_[1] = -state_->fts_state.raw_fy; - bias_[2] = -state_->fts_state.raw_fz; - bias_[3] = -state_->fts_state.raw_tx; - bias_[4] = -state_->fts_state.raw_ty; - bias_[5] = -state_->fts_state.raw_tz; - return true; + + // Do not permit taring if there is a fault + if(device_fault_active_){ + ERROR("Taring (%s) FTS is not permited with an active fault, reset first", name_.c_str()); + return false; + }else{ + bias_[0] = -state_->fts_state.raw_fx; + bias_[1] = -state_->fts_state.raw_fy; + bias_[2] = -state_->fts_state.raw_fz; + bias_[3] = -state_->fts_state.raw_tx; + bias_[4] = -state_->fts_state.raw_ty; + bias_[5] = -state_->fts_state.raw_tz; + return true; + } } else if (cmd.type == FTS_ENABLE_GUARD_FAULT_CMD) { enable_fts_guard_fault_ = cmd.fts_enable_guard_fault_cmd.enable; From f934c3de1557a3d7efd9f2db8433a22ae3708a82 Mon Sep 17 00:00:00 2001 From: Alex Brinkman Date: Fri, 23 Dec 2022 10:57:26 -0800 Subject: [PATCH 4/7] reenabled commander uts --- test/CMakeLists.txt | 3 - test/test_filter.cc | 49 ---------------- test/test_unit/CMakeLists.txt | 3 +- test/test_unit/test_commander.cc | 58 +++++++++++-------- .../test_commander_yamls/c1_config.yaml | 1 + .../test_commander_yamls/c3_config.yaml | 1 + 6 files changed, 36 insertions(+), 79 deletions(-) delete mode 100644 test/test_filter.cc diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index d8fd78a..41cb7ab 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -21,9 +21,6 @@ target_link_libraries(test_config ${fastcat_test_libs}) add_executable(test_cli test_cli.cc) target_link_libraries(test_cli ${fastcat_test_libs}) -add_executable(test_filter test_filter.cc) -target_link_libraries(test_filter ${fastcat_test_libs}) - find_package(GTest) if(GTEST_FOUND) add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/test_unit) diff --git a/test/test_filter.cc b/test/test_filter.cc deleted file mode 100644 index f0fd53d..0000000 --- a/test/test_filter.cc +++ /dev/null @@ -1,49 +0,0 @@ -#include "fastcat/fastcat.h" -#include "fastcat/fastcat_devices/filter.h" - -int main(int argc, char* argv[]) -{ - (void)argc; - (void)argv; - - // 2nd order Butterworth, Wn=0.5 - // std::vector A = {1.0, 0.0, 0.1715729}; - // std::vector B = {0.2928932, 0.5857864, 0.2928932}; - - // 2nd order Butterworth, Wn=0.05 - // std::vector A = {1.0, -1.778631, 0.800803}; - // std::vector B = {0.005542717, 0.0110854434, 0.0055427172}; - - // Moving Average - std::vector A = {0}; - std::vector B = {0.25, 0.25, 0.25, 0.25}; - fastcat::DigitalABFilter filter(A, B); - - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - filter.ApplyFilter(1); - - fastcat::MovingAverageFilter mafilt(4); - - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - MSG_DEBUG("ma = %lf", mafilt.ApplyFilter(1)); - - return 0; -} diff --git a/test/test_unit/CMakeLists.txt b/test/test_unit/CMakeLists.txt index f72a410..048b34f 100644 --- a/test/test_unit/CMakeLists.txt +++ b/test/test_unit/CMakeLists.txt @@ -1,6 +1,7 @@ find_package(Threads REQUIRED) set(TEST_SOURCES + test_commander.cc test_conditional.cc test_function.cc test_schmitt_trigger.cc @@ -8,9 +9,7 @@ set(TEST_SOURCES test_transform_utils.cc test_yaml_parser.cc test_linear_interpolation.cc - test_jsd_device_base.cc - test_trap.cc ) diff --git a/test/test_unit/test_commander.cc b/test/test_unit/test_commander.cc index 7d41101..9be71aa 100644 --- a/test/test_unit/test_commander.cc +++ b/test/test_unit/test_commander.cc @@ -30,26 +30,17 @@ class CommanderTest : public ::testing::Test TEST_F(CommanderTest, ConfigFromYamlSuccess) { - // ConfigFromYaml() returns true if all necessary fields are present in the - // YAML file + // ConfigFromYaml() returns true if all necessary fields are present EXPECT_TRUE(c1_.ConfigFromYaml(config_node)); } TEST_F(CommanderTest, ConfigFromYamlBadCommandType) { - // ConfigFromYaml() returns false if device_cmd_type is not POLYNOMIAL + // ConfigFromYaml() returns false if device_cmd_type is invalid config_node["device_cmd_type"] = "NOT_GOOD"; EXPECT_FALSE(c1_.ConfigFromYaml(config_node)); } -TEST_F(CommanderTest, ConfigFromYamlIncorrectFieldType) -{ - // ConfigFromYaml() throws an exception if a necessary field contains data - // that cannot be converted to the appropriate type - config_node["start_enabled"] = "JPL"; - EXPECT_ANY_THROW(c1_.ConfigFromYaml(config_node)); -} - TEST_F(CommanderTest, ConfigFromYamlMissingField) { // ConfigFromYaml() returns false if a necessary field is missing from the @@ -58,19 +49,6 @@ TEST_F(CommanderTest, ConfigFromYamlMissingField) EXPECT_FALSE(c1_.ConfigFromYaml(config_node)); } -TEST_F(CommanderTest, ReadResponseRealSignals) -{ - // Read() returns false when signal cannot be updated - EXPECT_FALSE(c2_.Read()); - - // Read() returns true when signals have updated successfully - double dummy_sig_out = 5.0; - c2_.signals_[0].data_loc = (void*)&dummy_sig_out; - c2_.signals_[0].data_type = fastcat::DATA_TYPE_DOUBLE; - EXPECT_TRUE(c2_.Read()); -} - -// TODO: Test case where all signals are FIXED_VALUE TEST_F(CommanderTest, ReadResponseFixedSignals) { // Read() returns true when all signals are FIXED_VALUE @@ -84,7 +62,7 @@ TEST_F(CommanderTest, WriteValidCommand) cmd.type = fastcat::COMMANDER_ENABLE_CMD; cmd.commander_enable_cmd.duration = 30; EXPECT_TRUE(c2_.Write(cmd)); - EXPECT_EQ(c2_.GetState()->commander_state.enable, false); + EXPECT_TRUE(c2_.GetState()->commander_state.enable); } TEST_F(CommanderTest, WriteInvalidCommand) @@ -92,6 +70,7 @@ TEST_F(CommanderTest, WriteInvalidCommand) // Write() returns false if the command sent is not valid for Commander // devices fastcat::DeviceCmd cmd; + cmd.type = fastcat::BAD_DEVICE_CMD; EXPECT_FALSE(c2_.Write(cmd)); } @@ -100,4 +79,33 @@ TEST_F(CommanderTest, ProcessResponse) // Process() always returns NO_FAULT EXPECT_EQ(c2_.Process(), fastcat::NO_FAULT); } + +TEST_F(CommanderTest, CmdsRejectedWhenFaulted) +{ + // Put in faulted state + c2_.Fault(); + + // Reject Disable + fastcat::DeviceCmd cmd; + cmd.type = fastcat::COMMANDER_DISABLE_CMD; + + EXPECT_FALSE(c2_.Write(cmd)); + EXPECT_FALSE(c2_.GetState()->commander_state.enable); + + // Reject Enable + cmd.type = fastcat::COMMANDER_ENABLE_CMD; + cmd.commander_enable_cmd.duration = 30; + + EXPECT_FALSE(c2_.Write(cmd)); + EXPECT_FALSE(c2_.GetState()->commander_state.enable); + + // Reset, then accept Enable + c2_.Reset(); + EXPECT_TRUE(c2_.Write(cmd)); + EXPECT_TRUE(c2_.GetState()->commander_state.enable); + + +} + + } // namespace diff --git a/test/test_unit/test_commander_yamls/c1_config.yaml b/test/test_unit/test_commander_yamls/c1_config.yaml index e95ed0e..3d94342 100644 --- a/test/test_unit/test_commander_yamls/c1_config.yaml +++ b/test/test_unit/test_commander_yamls/c1_config.yaml @@ -3,6 +3,7 @@ name: c1 start_enabled: true device_cmd_name: m1 device_cmd_type: EGD_CSP_CMD +skip_n_loops: 0 signals: - observed_device_name: FIXED_VALUE fixed_value: 0 diff --git a/test/test_unit/test_commander_yamls/c3_config.yaml b/test/test_unit/test_commander_yamls/c3_config.yaml index 6246660..2a2f967 100644 --- a/test/test_unit/test_commander_yamls/c3_config.yaml +++ b/test/test_unit/test_commander_yamls/c3_config.yaml @@ -3,6 +3,7 @@ name: c3 start_enabled: true device_cmd_name: m2 device_cmd_type: EGD_CSP_CMD +skip_n_loops: 0 signals: - observed_device_name: FIXED_VALUE fixed_value: 0 From a7547d12a5ce4db0562b0098e87bdaef0989e6d0 Mon Sep 17 00:00:00 2001 From: Alex Brinkman Date: Fri, 23 Dec 2022 18:20:11 -0800 Subject: [PATCH 5/7] added UTs to ensure only motion commands are prevented in faulted --- src/jsd/actuator.cc | 18 +- test/test_unit/CMakeLists.txt | 7 +- test/test_unit/test_actuator.cc | 160 ++++++++++++++++++ test/test_unit/test_actuator_yamls/valid.yaml | 24 +++ .../test_actuator_yamls/valid_with_opts.yaml | 30 ++++ .../test_actuator_yamls/valid_with_power.yaml | 31 ++++ 6 files changed, 259 insertions(+), 11 deletions(-) create mode 100644 test/test_unit/test_actuator.cc create mode 100644 test/test_unit/test_actuator_yamls/valid.yaml create mode 100644 test/test_unit/test_actuator_yamls/valid_with_opts.yaml create mode 100644 test/test_unit/test_actuator_yamls/valid_with_power.yaml diff --git a/src/jsd/actuator.cc b/src/jsd/actuator.cc index c0e6974..26d1d08 100644 --- a/src/jsd/actuator.cc +++ b/src/jsd/actuator.cc @@ -311,18 +311,12 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) return true; break; - case ACTUATOR_HALT_CMD: - if (!HandleNewHaltCmd()) { - ERROR("Failed to handle Halt Command"); - return false; - } - break; - case ACTUATOR_SET_OUTPUT_POSITION_CMD: if (!HandleNewSetOutputPositionCmd(cmd)) { ERROR("Failed to handle Set Output Position Command"); return false; } + return true; break; case ACTUATOR_SET_MAX_CURRENT_CMD: @@ -338,7 +332,8 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) if (!HandleNewSetUnitModeCmd(cmd)) { ERROR("Failed to handle Set Unit Mode Command"); return false; - } + } + return true; break; case ACTUATOR_SDO_DISABLE_GAIN_SCHEDULING_CMD: { @@ -461,6 +456,13 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd) } break; + case ACTUATOR_HALT_CMD: + if (!HandleNewHaltCmd()) { + ERROR("Failed to handle Halt Command"); + return false; + } + break; + default: WARNING("That command type is not supported in this mode!"); return false; diff --git a/test/test_unit/CMakeLists.txt b/test/test_unit/CMakeLists.txt index 048b34f..adc2374 100644 --- a/test/test_unit/CMakeLists.txt +++ b/test/test_unit/CMakeLists.txt @@ -1,16 +1,17 @@ find_package(Threads REQUIRED) set(TEST_SOURCES + test_actuator.cc test_commander.cc test_conditional.cc test_function.cc + test_jsd_device_base.cc + test_linear_interpolation.cc test_schmitt_trigger.cc test_signal_generator.cc test_transform_utils.cc - test_yaml_parser.cc - test_linear_interpolation.cc - test_jsd_device_base.cc test_trap.cc + test_yaml_parser.cc ) foreach(TEST_SOURCE ${TEST_SOURCES}) diff --git a/test/test_unit/test_actuator.cc b/test/test_unit/test_actuator.cc new file mode 100644 index 0000000..d01e047 --- /dev/null +++ b/test/test_unit/test_actuator.cc @@ -0,0 +1,160 @@ +#include +#include + +#include "fastcat/config.h" +#include "fastcat/jsd/actuator_offline.h" +#include "fastcat/signal_handling.h" + +#include "jsd/jsd_print.h" + +namespace +{ +class ActuatorTest : public ::testing::Test +{ + protected: + void SetUp() override + { + jsd_context_ = jsd_alloc(); + + // FASTCAT_UNIT_TEST_DIR contains path to . + base_dir_ = FASTCAT_UNIT_TEST_DIR; + base_dir_ += "test_actuator_yamls/"; + + device_.SetSlaveId(0); + device_.SetContext(jsd_context_); + device_.SetOffline(true); + } + + void TearDown() override + { + jsd_free(jsd_context_); + } + + jsd_t* jsd_context_; + std::string base_dir_; + YAML::Node node_; + fastcat::ActuatorOffline device_; +}; + + TEST_F(ActuatorTest, ParseValidWithPower) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"valid_with_power.yaml"))); + } + + TEST_F(ActuatorTest, ParseValidWithOptional) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"valid_with_opts.yaml"))); + } + + TEST_F(ActuatorTest, ParseValid) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"valid.yaml"))); + } + + TEST_F(ActuatorTest, RejectMotionCommandsWhenFaulted) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"valid.yaml"))); + + device_.Fault(); + + // Fail Motion commands + { // CSP + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_CSP_CMD; + cmd.actuator_csp_cmd.target_position = 0; + cmd.actuator_csp_cmd.position_offset = 0; + cmd.actuator_csp_cmd.velocity_offset = 0; + cmd.actuator_csp_cmd.torque_offset_amps = 0; + EXPECT_FALSE(device_.Write(cmd)); + } + { // CSV + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_CSV_CMD; + cmd.actuator_csv_cmd.target_velocity = 0; + cmd.actuator_csv_cmd.velocity_offset = 0; + cmd.actuator_csv_cmd.torque_offset_amps = 0; + EXPECT_FALSE(device_.Write(cmd)); + } + { // CST + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_CST_CMD; + cmd.actuator_cst_cmd.target_torque_amps = 0; + cmd.actuator_cst_cmd.torque_offset_amps = 0; + EXPECT_FALSE(device_.Write(cmd)); + } + { // Prof Pos + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_PROF_POS_CMD; + cmd.actuator_prof_pos_cmd.target_position = 0; + cmd.actuator_prof_pos_cmd.profile_velocity = 0.1; + cmd.actuator_prof_pos_cmd.end_velocity = 0; + cmd.actuator_prof_pos_cmd.profile_accel = 0.1; + cmd.actuator_prof_pos_cmd.relative = 0; + EXPECT_FALSE(device_.Write(cmd)); + } + { // Prof Vel + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_PROF_VEL_CMD; + cmd.actuator_prof_vel_cmd.target_velocity = 0.1; + cmd.actuator_prof_vel_cmd.profile_accel = 0.1; + cmd.actuator_prof_vel_cmd.max_duration = 30; + EXPECT_FALSE(device_.Write(cmd)); + } + { // Prof Torque + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_PROF_TORQUE_CMD; + cmd.actuator_prof_torque_cmd.target_torque_amps = 0; + cmd.actuator_prof_torque_cmd.max_duration = 30; + EXPECT_FALSE(device_.Write(cmd)); + } + { // Calibrate + fastcat::DeviceCmd cmd; + cmd.type = fastcat::ACTUATOR_CALIBRATE_CMD; + cmd.actuator_calibrate_cmd.velocity = 0.02; + cmd.actuator_calibrate_cmd.accel = 0.2; + cmd.actuator_calibrate_cmd.max_current = 0.2; + EXPECT_FALSE(device_.Write(cmd)); + } + { // Halt + fastcat::DeviceCmd cmd; + + cmd.type = fastcat::ACTUATOR_HALT_CMD; + EXPECT_FALSE(device_.Write(cmd)); + } + + { // Confirm non-motion commands are still accepted + fastcat::DeviceCmd cmd; + + cmd.type = fastcat::ACTUATOR_SET_GAIN_SCHEDULING_INDEX_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SET_GAIN_SCHEDULING_INDEX_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SET_OUTPUT_POSITION_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SET_MAX_CURRENT_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SDO_SET_UNIT_MODE_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SDO_SET_UNIT_MODE_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SDO_DISABLE_GAIN_SCHEDULING_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SDO_ENABLE_SPEED_GAIN_SCHEDULING_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SDO_ENABLE_POSITION_GAIN_SCHEDULING_CMD; + EXPECT_TRUE(device_.Write(cmd)); + + cmd.type = fastcat::ACTUATOR_SDO_ENABLE_MANUAL_GAIN_SCHEDULING_CMD; + EXPECT_TRUE(device_.Write(cmd)); + } + + + } + + + +} // namespace diff --git a/test/test_unit/test_actuator_yamls/valid.yaml b/test/test_unit/test_actuator_yamls/valid.yaml new file mode 100644 index 0000000..d8adb32 --- /dev/null +++ b/test/test_unit/test_actuator_yamls/valid.yaml @@ -0,0 +1,24 @@ +device_class: Actuator +name: act_1 +actuator_type: revolute # revolute: eu = radians +gear_ratio: 100 +counts_per_rev: 500 +max_speed_eu_per_sec: 10 #rad/sec +max_accel_eu_per_sec2: 30 #rad/sec/sec +over_speed_multiplier: 2 +vel_tracking_error_eu_per_sec: 1000 # chosen arbitrarily +pos_tracking_error_eu: 1000 # chosen arbitrarily +peak_current_limit_amps: 10 +peak_current_time_sec: 3.0 # chosen arbitrarily +continuous_current_limit_amps: 5 +torque_slope_amps_per_sec: 0.5 # amps/sec; chosen arbitrarily +low_pos_cal_limit_eu: -3.2 +low_pos_cmd_limit_eu: -3.14159 +high_pos_cmd_limit_eu: 3.14159 +high_pos_cal_limit_eu: 3.2 +holding_duration_sec: 3.0 # chosen arbitrarily +egd_brake_engage_msec: 500 +egd_brake_disengage_msec: 1000 +egd_crc: 12345 +egd_drive_max_current_limit: 10 # amps +smooth_factor: 0 # default value is 0 diff --git a/test/test_unit/test_actuator_yamls/valid_with_opts.yaml b/test/test_unit/test_actuator_yamls/valid_with_opts.yaml new file mode 100644 index 0000000..67a9522 --- /dev/null +++ b/test/test_unit/test_actuator_yamls/valid_with_opts.yaml @@ -0,0 +1,30 @@ +device_class: Actuator +name: act_1 +actuator_type: revolute # revolute: eu = radians +gear_ratio: 100 +counts_per_rev: 500 +max_speed_eu_per_sec: 10 #rad/sec +max_accel_eu_per_sec2: 30 #rad/sec/sec +over_speed_multiplier: 2 +vel_tracking_error_eu_per_sec: 1000 # chosen arbitrarily +pos_tracking_error_eu: 1000 # chosen arbitrarily +peak_current_limit_amps: 10 +peak_current_time_sec: 3.0 # chosen arbitrarily +continuous_current_limit_amps: 5 +torque_slope_amps_per_sec: 0.5 # amps/sec; chosen arbitrarily +low_pos_cal_limit_eu: -3.2 +low_pos_cmd_limit_eu: -3.14159 +high_pos_cmd_limit_eu: 3.14159 +high_pos_cal_limit_eu: 3.2 +holding_duration_sec: 3.0 # chosen arbitrarily +egd_brake_engage_msec: 500 +egd_brake_disengage_msec: 1000 +egd_crc: 12345 +egd_drive_max_current_limit: 10 # amps +smooth_factor: 0 # default value is 0 + +# Optional Params follow: +ctrl_gain_scheduling_mode: DISABLED +absolute_encoder: True +prof_pos_hold: True + diff --git a/test/test_unit/test_actuator_yamls/valid_with_power.yaml b/test/test_unit/test_actuator_yamls/valid_with_power.yaml new file mode 100644 index 0000000..0d22f67 --- /dev/null +++ b/test/test_unit/test_actuator_yamls/valid_with_power.yaml @@ -0,0 +1,31 @@ +device_class: Actuator +name: act_1 +actuator_type: revolute # revolute: eu = radians +gear_ratio: 100 +counts_per_rev: 500 +max_speed_eu_per_sec: 10 #rad/sec +max_accel_eu_per_sec2: 30 #rad/sec/sec +over_speed_multiplier: 2 +vel_tracking_error_eu_per_sec: 1000 # chosen arbitrarily +pos_tracking_error_eu: 1000 # chosen arbitrarily +peak_current_limit_amps: 10 +peak_current_time_sec: 3.0 # chosen arbitrarily +continuous_current_limit_amps: 5 +torque_slope_amps_per_sec: 0.5 # amps/sec; chosen arbitrarily +low_pos_cal_limit_eu: -3.2 +low_pos_cmd_limit_eu: -3.14159 +high_pos_cmd_limit_eu: 3.14159 +high_pos_cal_limit_eu: 3.2 +holding_duration_sec: 3.0 # chosen arbitrarily +egd_brake_engage_msec: 500 +egd_brake_disengage_msec: 1000 +egd_crc: 12345 +egd_drive_max_current_limit: 10 # amps +smooth_factor: 0 # default value is 0 + +# Optional Power Params + +torque_constant: 0.5 +winding_resistance: 20.0 +brake_power: 0.1 +motor_encoder_gear_ratio: 2000 From 278566c582a46a0efd17cc82fe92a069d2d58ee5 Mon Sep 17 00:00:00 2001 From: Alex Brinkman Date: Thu, 5 Jan 2023 23:08:31 -0800 Subject: [PATCH 6/7] set up tests for 3 fts devices --- test/test_unit/CMakeLists.txt | 3 ++ test/test_unit/test_ati_fts.cc | 44 +++++++++++++++++++ .../test_unit/test_ati_fts_yamls/nominal.yaml | 9 ++++ test/test_unit/test_fts.cc | 27 ++++++++++++ test/test_unit/test_fts_yamls/nominal.yaml | 27 ++++++++++++ test/test_unit/test_virtual_fts.cc | 27 ++++++++++++ .../test_virtual_fts_yamls/nominal.yaml | 23 ++++++++++ 7 files changed, 160 insertions(+) create mode 100644 test/test_unit/test_ati_fts.cc create mode 100644 test/test_unit/test_ati_fts_yamls/nominal.yaml create mode 100644 test/test_unit/test_fts.cc create mode 100644 test/test_unit/test_fts_yamls/nominal.yaml create mode 100644 test/test_unit/test_virtual_fts.cc create mode 100644 test/test_unit/test_virtual_fts_yamls/nominal.yaml diff --git a/test/test_unit/CMakeLists.txt b/test/test_unit/CMakeLists.txt index adc2374..7cec7bc 100644 --- a/test/test_unit/CMakeLists.txt +++ b/test/test_unit/CMakeLists.txt @@ -12,6 +12,9 @@ set(TEST_SOURCES test_transform_utils.cc test_trap.cc test_yaml_parser.cc + test_fts.cc + test_ati_fts.cc + test_virtual_fts.cc ) foreach(TEST_SOURCE ${TEST_SOURCES}) diff --git a/test/test_unit/test_ati_fts.cc b/test/test_unit/test_ati_fts.cc new file mode 100644 index 0000000..553c7dd --- /dev/null +++ b/test/test_unit/test_ati_fts.cc @@ -0,0 +1,44 @@ +#include +#include + +#include "fastcat/config.h" +#include "fastcat/jsd/ati_fts.h" +#include "fastcat/signal_handling.h" + +#include "jsd/jsd.h" +#include "jsd/jsd_print.h" + +namespace +{ +class AtiFtsTest : public ::testing::Test +{ + protected: + void SetUp() override + { + jsd_context_ = jsd_alloc(); + + // FASTCAT_UNIT_TEST_DIR contains path to . + base_dir_ = FASTCAT_UNIT_TEST_DIR; + base_dir_ += "test_ati_fts_yamls/"; + + device_.SetSlaveId(0); + device_.SetContext(jsd_context_); + device_.SetOffline(true); + } + + void TearDown() override + { + jsd_free(jsd_context_); + } + + jsd_t* jsd_context_; + std::string base_dir_; + YAML::Node node_; + fastcat::AtiFts device_; +}; + +TEST_F(AtiFtsTest, ParseNominalConfig) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); +} + +} // namespace diff --git a/test/test_unit/test_ati_fts_yamls/nominal.yaml b/test/test_unit/test_ati_fts_yamls/nominal.yaml new file mode 100644 index 0000000..74c2de4 --- /dev/null +++ b/test/test_unit/test_ati_fts_yamls/nominal.yaml @@ -0,0 +1,9 @@ +device_class: AtiFts +name: ati_fts_1 +calibration: 0 +max_force_x: 100 +max_force_y: 200 +max_force_z: 300 +max_torque_x: 10 +max_torque_y: 20 +max_torque_z: 30 diff --git a/test/test_unit/test_fts.cc b/test/test_unit/test_fts.cc new file mode 100644 index 0000000..d9f9221 --- /dev/null +++ b/test/test_unit/test_fts.cc @@ -0,0 +1,27 @@ +#include +#include + +#include "fastcat/config.h" +#include "fastcat/fastcat_devices/fts.h" +#include "fastcat/signal_handling.h" + +#include "jsd/jsd_print.h" + +class FtsTest : public ::testing::Test +{ + protected: + void SetUp() override + { + // FASTCAT_UNIT_TEST_DIR contains path to . + base_dir_ = FASTCAT_UNIT_TEST_DIR; + base_dir_ += "test_fts_yamls/"; + } + + std::string base_dir_; + YAML::Node node_; + fastcat::Fts device_; +}; + +TEST_F(FtsTest, ParseNominalConfig) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); +} diff --git a/test/test_unit/test_fts_yamls/nominal.yaml b/test/test_unit/test_fts_yamls/nominal.yaml new file mode 100644 index 0000000..9ddcae3 --- /dev/null +++ b/test/test_unit/test_fts_yamls/nominal.yaml @@ -0,0 +1,27 @@ +device_class: Fts +name: fts_1 +calibration_matrix: [1,0,0, 0,0,0, + 0,1,0, 0,0,0, + 0,0,1, 0,0,0, + 0,0,0, 1,0,0, + 0,0,0, 0,1,0, + 0,0,0, 0,0,1] +max_force_x: 100 +max_force_y: 200 +max_force_z: 300 +max_torque_x: 10 +max_torque_y: 20 +max_torque_z: 30 +signals: +- observed_device_name: FIXED_VALUE + fixed_value: 10 +- observed_device_name: FIXED_VALUE + fixed_value: 20 +- observed_device_name: FIXED_VALUE + fixed_value: 30 +- observed_device_name: FIXED_VALUE + fixed_value: 1 +- observed_device_name: FIXED_VALUE + fixed_value: 2 +- observed_device_name: FIXED_VALUE + fixed_value: 3 diff --git a/test/test_unit/test_virtual_fts.cc b/test/test_unit/test_virtual_fts.cc new file mode 100644 index 0000000..1ea6f2a --- /dev/null +++ b/test/test_unit/test_virtual_fts.cc @@ -0,0 +1,27 @@ +#include +#include + +#include "fastcat/config.h" +#include "fastcat/fastcat_devices/virtual_fts.h" +#include "fastcat/signal_handling.h" + +#include "jsd/jsd_print.h" + +class VirtualFtsTest : public ::testing::Test +{ + protected: + void SetUp() override + { + // FASTCAT_UNIT_TEST_DIR contains path to . + base_dir_ = FASTCAT_UNIT_TEST_DIR; + base_dir_ += "test_virtual_fts_yamls/"; + } + + std::string base_dir_; + YAML::Node node_; + fastcat::VirtualFts device_; +}; + +TEST_F(VirtualFtsTest, ParseNominalConfig) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); +} diff --git a/test/test_unit/test_virtual_fts_yamls/nominal.yaml b/test/test_unit/test_virtual_fts_yamls/nominal.yaml new file mode 100644 index 0000000..0e35c56 --- /dev/null +++ b/test/test_unit/test_virtual_fts_yamls/nominal.yaml @@ -0,0 +1,23 @@ +device_class: VirtualFts +name: virtual_fts_1 +position: [0, 0, 0] +quaternion: [0.7071, 0.35355, 0, 0.35355] +max_force_x: 100 +max_force_y: 200 +max_force_z: 300 +max_torque_x: 10 +max_torque_y: 20 +max_torque_z: 30 +signals: +- observed_device_name: FIXED_VALUE + fixed_value: 10 +- observed_device_name: FIXED_VALUE + fixed_value: 20 +- observed_device_name: FIXED_VALUE + fixed_value: 30 +- observed_device_name: FIXED_VALUE + fixed_value: 1 +- observed_device_name: FIXED_VALUE + fixed_value: 2 +- observed_device_name: FIXED_VALUE + fixed_value: 3 From 1dcdbee4fede35ff437208589d783b2999e350d7 Mon Sep 17 00:00:00 2001 From: Alex Brinkman Date: Tue, 24 Jan 2023 10:37:24 -0800 Subject: [PATCH 7/7] add UTs for preventing taring in faulted state --- test/test_unit/test_ati_fts.cc | 16 ++++++++++++++++ test/test_unit/test_fts.cc | 16 ++++++++++++++++ test/test_unit/test_virtual_fts.cc | 16 ++++++++++++++++ 3 files changed, 48 insertions(+) diff --git a/test/test_unit/test_ati_fts.cc b/test/test_unit/test_ati_fts.cc index 553c7dd..6b8220a 100644 --- a/test/test_unit/test_ati_fts.cc +++ b/test/test_unit/test_ati_fts.cc @@ -41,4 +41,20 @@ TEST_F(AtiFtsTest, ParseNominalConfig) { EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); } +// Currently the nominal behavior to faults is to prevent taring when faulted +// This may change with optional YAML parameters in the future perhaps. +TEST_F(AtiFtsTest, RejectTareWhenFaulted) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); + + fastcat::DeviceCmd cmd; + cmd.type = fastcat::FTS_TARE_CMD; + EXPECT_TRUE(device_.Write(cmd)); + MSG("Accepted tare when in nominal state"); + + device_.Fault(); + + cmd.type = fastcat::FTS_TARE_CMD; + EXPECT_FALSE(device_.Write(cmd)); +} + } // namespace diff --git a/test/test_unit/test_fts.cc b/test/test_unit/test_fts.cc index d9f9221..2f3cfeb 100644 --- a/test/test_unit/test_fts.cc +++ b/test/test_unit/test_fts.cc @@ -25,3 +25,19 @@ class FtsTest : public ::testing::Test TEST_F(FtsTest, ParseNominalConfig) { EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); } + +// Currently the nominal behavior to faults is to prevent taring when faulted +// This may change with optional YAML parameters in the future perhaps. +TEST_F(FtsTest, RejectTareWhenFaulted) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); + + fastcat::DeviceCmd cmd; + cmd.type = fastcat::FTS_TARE_CMD; + EXPECT_TRUE(device_.Write(cmd)); + MSG("Accepted tare when in nominal state"); + + device_.Fault(); + + cmd.type = fastcat::FTS_TARE_CMD; + EXPECT_FALSE(device_.Write(cmd)); +} diff --git a/test/test_unit/test_virtual_fts.cc b/test/test_unit/test_virtual_fts.cc index 1ea6f2a..7273d29 100644 --- a/test/test_unit/test_virtual_fts.cc +++ b/test/test_unit/test_virtual_fts.cc @@ -25,3 +25,19 @@ class VirtualFtsTest : public ::testing::Test TEST_F(VirtualFtsTest, ParseNominalConfig) { EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); } + +// Currently the nominal behavior to faults is to prevent taring when faulted +// This may change with optional YAML parameters in the future perhaps. +TEST_F(VirtualFtsTest, RejectTareWhenFaulted) { + EXPECT_TRUE(device_.ConfigFromYaml(YAML::LoadFile(base_dir_+"nominal.yaml"))); + + fastcat::DeviceCmd cmd; + cmd.type = fastcat::FTS_TARE_CMD; + EXPECT_TRUE(device_.Write(cmd)); + MSG("Accepted tare when in nominal state"); + + device_.Fault(); + + cmd.type = fastcat::FTS_TARE_CMD; + EXPECT_FALSE(device_.Write(cmd)); +}