Merge pull request #27 from nasa-jpl/add-actuator-power-dev

Add actuator power dev

Author: JosephBowkett

doc/fastcat_device_config_parameters.md

@@ -188,6 +188,9 @@ Engineering Units (EU) are radians for revolute actuators and meters for linear
 | `egd_crc`                       | CRC of the flashed Elmo parameter set                        |
 | `egd_drive_max_current_limit`   | The Maximum drive current for the Elmo Gold Drive            |
 | `smooth_factor`                 | Affects controller smoothing, defaults to `0`                |
+| `winding_resistance`            | OPTIONAL: Winding resistance of motor for optional power calculation |
+| `torque_constant`               | OPTIONAL: Torque constant of motor for optional power calculation  |
+| `motor_encoder_gear_ratio`      | OPTIONAL: Capture any gear ratio between the motor and the encoder, i.e. an output encoder  |
 
 ### Implementation Notes
 

src/fcgen/fastcat_types.yaml

@@ -118,6 +118,8 @@ states:
       type: int32_t
     - name: actuator_state_machine_state
       type: uint8_t
+    - name: power
+      type: double
 
   - name: faulter
     fields:

src/jsd/actuator.cc

@@ -132,6 +132,25 @@ bool fastcat::Actuator::ConfigFromYaml(YAML::Node node)
   if (!ParseValCheckRange(node, "smooth_factor", smooth_factor_, -1, 64)) {
     return false;
   }
+  
+  if (ParseOptValCheckRange(node, "torque_constant", torque_constant_, 0.0, 999999.0) &&
+      ParseOptValCheckRange(node, "winding_resistance", winding_resistance_, 0.0, 999999.0) ) {
+    
+    // Only compute power if we received both torque_constant and winding_resistance parameters
+    compute_power_ = true;
+    
+    // Read in brake power (if provided) to add to actuator power
+    if (!ParseOptValCheckRange(node, "brake_power", brake_power_, 0.0, 9999.0)) {
+      // If not found then set to zero
+      brake_power_ = 0.0;
+    }
+    
+    // Read in any gear ratio between the motor and encoder for power calculation
+    if (!ParseOptValCheckRange(node, "motor_encoder_gear_ratio", motor_encoder_gear_ratio_, 0.0, 9999.0)) {
+      // If not found then set to 1.0
+      motor_encoder_gear_ratio_ = 1.0;
+    }
+  }
 
   // overall_reduction must be set before using EuToCnts/CntsToEu
   if (actuator_type_ == ACTUATOR_TYPE_REVOLUTE) {
@@ -221,6 +240,22 @@ bool fastcat::Actuator::Read()
   state_->actuator_state.actuator_state_machine_state =
       static_cast<int>(actuator_sms_);
 
+  if (compute_power_) {
+    double motor_velocity = fabs(state_->actuator_state.actual_velocity) * motor_encoder_gear_ratio_;
+    double current = fabs(jsd_egd_state_.actual_current);
+
+    // P = R I^2 + K_T * I * \omega
+    state_->actuator_state.power = current *
+      (winding_resistance_ * current +
+       torque_constant_ * motor_velocity);
+
+    // Should checking, but assuming motor_on > 0 means brakes powered/disengaged
+    if (state_->actuator_state.motor_on)
+      state_->actuator_state.power += brake_power_;
+  }
+  // else
+  // state_->actuator_state.power = 0;
+
   return true;
 }
 

src/jsd/actuator.h

@@ -119,6 +119,12 @@ class Actuator : public DeviceBase
   double egd_crc_                       = 0;
   double egd_drive_max_cur_limit_amps_  = 0;
   double smooth_factor_                 = 0;
+  double torque_constant_               = 0;
+  double winding_resistance_            = 0;
+  double brake_power_                   = 0;
+  double motor_encoder_gear_ratio_      = 0;
+
+  bool compute_power_ = false;
 
   jsd_slave_config_t jsd_slave_config_;
   jsd_egd_state_t    jsd_egd_state_;

src/jsd/actuator_offline.cc

@@ -61,13 +61,17 @@ void fastcat::ActuatorOffline::EgdCSP(jsd_egd_motion_command_csp_t jsd_csp_cmd)
   jsd_egd_state_.cmd_ff_velocity = jsd_csp_cmd.velocity_offset;
   jsd_egd_state_.cmd_ff_current  = jsd_csp_cmd.torque_offset_amps;
 
+  // Differentiate position to get actual_velocity
+  double vel = (jsd_egd_state_.cmd_position + jsd_egd_state_.cmd_ff_position
+                - jsd_egd_state_.actual_position) / loop_period_;
+
   // simulate actuals
   jsd_egd_state_.actual_position =
       jsd_egd_state_.cmd_position + jsd_egd_state_.cmd_ff_position;
-  jsd_egd_state_.actual_velocity =
-      jsd_egd_state_.cmd_velocity + jsd_egd_state_.cmd_ff_velocity;
   jsd_egd_state_.actual_current =
       jsd_egd_state_.cmd_current + jsd_egd_state_.cmd_ff_current;
+  
+  jsd_egd_state_.actual_velocity = vel; // "sure, why not"
 }
 
 void fastcat::ActuatorOffline::EgdCSV(jsd_egd_motion_command_csv_t jsd_csv_cmd)

src/yaml_parser.cc

@@ -262,3 +262,31 @@ bool fastcat::ParseValCheckRange(YAML::Node node, std::string field,
         upper);
   return false;
 }
+
+// Optional value parsing
+
+bool fastcat::ParseOptVal(YAML::Node node, std::string field, double& val)
+{
+  if (!node[field]) {
+    MSG_DEBUG("Did not find optional double field: %s", field.c_str());
+    return false;
+  }
+  val = node[field].as<double>();
+  MSG_DEBUG("Parsed double field %s: %lf", field.c_str(), val);
+  return true;
+}
+
+bool fastcat::ParseOptValCheckRange(YAML::Node node, std::string field,
+                                    double& val, double lower, double upper)
+{
+  if (!ParseOptVal(node, field, val)) {
+    return false;
+  }
+
+  if (lower <= val && val <= upper) {
+    return true;
+  }
+  ERROR("%s failed range check %lf <= %lf <= %lf", field.c_str(), lower, val,
+        upper);
+  return false;
+}

src/yaml_parser.h

@@ -50,6 +50,12 @@ bool ParseValCheckRange(YAML::Node node, std::string field, int8_t& val,
 bool ParseValCheckRange(YAML::Node node, std::string field, uint8_t& val,
                         uint8_t lower, uint8_t upper);
 
+// Optional double values
+bool ParseOptVal(YAML::Node node, std::string field, double& val);
+
+bool ParseOptValCheckRange(YAML::Node node, std::string field, double& val,
+                           double lower, double upper);
+
 }  // namespace fastcat
 
 #endif