diff --git a/docs/reference.md b/docs/reference.md index 0e664b01..9200d397 100644 --- a/docs/reference.md +++ b/docs/reference.md @@ -1162,6 +1162,15 @@ When in the "position timeout" mode the controller acts to damp the output. This parameter controls the maximum torque available for such damping. +## `servo.timeout_mode` ## + +Selects what behavior will take place in the position timeout mode. +The allowable values are a subset of the top level modes. + +* 0 - "stopped" - the driver is disengaged +* 12 - "zero velocity" +* 15 - "brake" + ## `servo.rezero_from_abs` ## If set to one, then shortly after startup, the value of the position diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc index ddf2b513..169f7892 100644 --- a/fw/bldc_servo.cc +++ b/fw/bldc_servo.cc @@ -1063,13 +1063,15 @@ class BldcServo::Impl { case kVoltageDq: case kCurrent: case kPosition: - case kPositionTimeout: case kZeroVelocity: case kStayWithinBounds: case kMeasureInductance: case kBrake: { return true; } + case kPositionTimeout: { + return config_.timeout_mode != 0; + } } return false; } @@ -1380,7 +1382,10 @@ class BldcServo::Impl { ISR_DoPosition(sin_cos, data); break; } - case kPositionTimeout: + case kPositionTimeout: { + ISR_DoPositionTimeout(sin_cos, data); + break; + } case kZeroVelocity: { ISR_DoZeroVelocity(sin_cos, data); break; @@ -1708,6 +1713,18 @@ class BldcServo::Impl { ISR_DoBalancedVoltageControl(ISR_CalculatePhaseVoltage(sin_cos, d_V, q_V)); } + void ISR_DoPositionTimeout(const SinCos& sin_cos, CommandData* data) MOTEUS_CCM_ATTRIBUTE { + if (config_.timeout_mode == kStopped) { + ISR_DoStopped(); + } else if (config_.timeout_mode == kZeroVelocity) { + ISR_DoZeroVelocity(sin_cos, data); + } else if (config_.timeout_mode == kBrake) { + ISR_DoBrake(); + } else { + ISR_DoStopped(); + } + } + void ISR_DoZeroVelocity(const SinCos& sin_cos, CommandData* data) MOTEUS_CCM_ATTRIBUTE { PID::ApplyOptions apply_options; apply_options.kp_scale = 0.0f; diff --git a/fw/bldc_servo.h b/fw/bldc_servo.h index 6ad311c1..d54f4031 100644 --- a/fw/bldc_servo.h +++ b/fw/bldc_servo.h @@ -196,6 +196,14 @@ class BldcServo { float default_timeout_s = 0.1f; float timeout_max_torque_Nm = 5.0f; + // Selects the behavior when in the timeout mode. The available + // options map to top level modes, although only the following are + // valid: + // 0 - "stopped" - motor driver disengaged + // 12 - "zero velocity" - derivative only position control + // 15 - "brake" - all motor phases shorted to ground + uint8_t timeout_mode = 12; + // Similar to 'max_voltage', the flux braking default voltage is // board rev dependent. float flux_brake_min_voltage = (g_measured_hw_rev <= 5) ? 34.5f : 43.5f; @@ -272,6 +280,7 @@ class BldcServo { a->Visit(MJ_NVP(max_position_slip)); a->Visit(MJ_NVP(default_timeout_s)); a->Visit(MJ_NVP(timeout_max_torque_Nm)); + a->Visit(MJ_NVP(timeout_mode)); a->Visit(MJ_NVP(flux_brake_min_voltage)); a->Visit(MJ_NVP(flux_brake_resistance_ohm)); a->Visit(MJ_NVP(max_current_A)); @@ -345,14 +354,14 @@ class BldcServo { // This state can be commanded directly, and will also be entered // automatically upon a watchdog timeout from kPosition. When in - // this state, the controller will apply a derivative only - // position control to slowly bring the servos to a resting - // position. + // this state, the controller will apply the selected fallback + // control mode. // // The only way to exit this state is through a stop command. kPositionTimeout = 11, - // This is just like kPositionTimeout, but is not latching. + // Control to zero velocity through a derivative only version of + // the position mode. kZeroVelocity = 12, // This applies the PID controller only to stay within a