Modified code to only check for SWITCHED ON as an error state once motion begins

Author: preston-rogers

src/jsd/actuator.cc

@@ -25,6 +25,9 @@ fastcat::Actuator::Actuator()
 bool fastcat::Actuator::ConfigFromYaml(const YAML::Node& node)
 {
   actuator_sms_ = ACTUATOR_SMS_HALTED;
+  elmo_state_machine_state_initialized_ = false;
+  last_elmo_state_machine_state_ =
+      JSD_ELMO_STATE_MACHINE_STATE_NOT_READY_TO_SWITCH_ON;
 
   // monotonic_initialization_time_sec_ set by base class method
   // SetInitializationTime, which must be called prior to ConfigFromYaml
@@ -228,6 +231,10 @@ bool fastcat::Actuator::Read()
 {
   ElmoRead();
   PopulateState();
+  if (!elmo_state_machine_state_initialized_) {
+    last_elmo_state_machine_state_ = GetElmoStateMachineState();
+    elmo_state_machine_state_initialized_ = true;
+  }
   return true;
 }
 
@@ -486,6 +493,10 @@ fastcat::FaultType fastcat::Actuator::Process()
       break;
   }
 
+  if (elmo_state_machine_state_initialized_) {
+    last_elmo_state_machine_state_ = GetElmoStateMachineState();
+  }
+
   ElmoProcess();
   return retval;
 }

src/jsd/actuator.h

@@ -262,6 +262,9 @@ class Actuator : public JsdDeviceBase
 
   bool    actuator_absolute_encoder_ = false;
   int32_t elmo_pos_offset_cnts_      = 1;
+  bool elmo_state_machine_state_initialized_ = false;
+  jsd_elmo_state_machine_state_t last_elmo_state_machine_state_ =
+      JSD_ELMO_STATE_MACHINE_STATE_NOT_READY_TO_SWITCH_ON;
   RingBuffer<DeviceCmd> last_device_cmd_ = RingBuffer<DeviceCmd>(50);
   double csp_interpolation_offset_time_ = 0.0;
   size_t csp_cycles_delay_ = 4;

src/jsd/actuator_fsm_helpers.cc

@@ -547,6 +547,17 @@ bool fastcat::Actuator::IsMotionFaultConditionMet()
     return true;
   }
   auto elmo_state_machine_state = GetElmoStateMachineState();
+  if (elmo_state_machine_state_initialized_ &&
+      last_elmo_state_machine_state_ ==
+          JSD_ELMO_STATE_MACHINE_STATE_OPERATION_ENABLED &&
+      elmo_state_machine_state == JSD_ELMO_STATE_MACHINE_STATE_SWITCHED_ON) {
+    ERROR("%s: Elmo drive state machine transitioned from OPERATION_ENABLED "
+          "to SWITCHED_ON during motion",
+          name_.c_str());
+    fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_INVALID_ELMO_SMS_DURING_MOTION;
+    return true;
+  }
+
   if (elmo_state_machine_state ==
           JSD_ELMO_STATE_MACHINE_STATE_QUICK_STOP_ACTIVE ||
       elmo_state_machine_state ==