diff --git a/docs/reference.md b/docs/reference.md index 7fa6e0b7..4a7fa4dd 100644 --- a/docs/reference.md +++ b/docs/reference.md @@ -1026,6 +1026,34 @@ external disturbances. When set, the `servo.pid_dq` configuration values no longer affect anything. +## `servo.fixed_voltage_mode` ## + +If non-zero, then no feedback based control of either position or +current is done. Instead, a fixed voltage is applied to the phase +terminals based on the current commanded position and the configured +number of motor poles. In this mode, the encoder and current sense +resistors are not used at all for control. + +This is a similar control mode to inexpensive brushless gimbal +controllers, and relies on burning a fixed amount of power in the +motor windings continuously. + +When this mode is active, the reported position and velocity will be 0 +when the drive is disabled, and exactly equal to the control position +when it is enabled. + +Various derating limits are inoperative in this mode: + * torque derating for temperature + * torque derating when outside position bounds + * the maximum current limit + * the commanded maximum torque + +A fault will still be triggered for over-temperature. + +## `servo.fixed_voltage_control_V` ## + +In the fixed voltage control mode, the voltage to apply to the output. + ## `servo.max_position_slip` ## diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc index a43afcd8..d424a7a3 100644 --- a/fw/bldc_servo.cc +++ b/fw/bldc_servo.cc @@ -697,6 +697,15 @@ class BldcServo::Impl { status_.cos = sin_cos.c; ISR_CalculateCurrentState(sin_cos); + + if (config_.fixed_voltage_mode) { + // Don't pretend we know where we are. + status_.unwrapped_position_raw = 0; + status_.unwrapped_position = 0.0f; + status_.velocity = 0.0f; + status_.torque_Nm = 0.0f; + } + #ifdef MOTEUS_PERFORMANCE_MEASURE status_.dwt.curstate = DWT->CYCCNT; #endif @@ -837,11 +846,16 @@ class BldcServo::Impl { const int16_t delta_position = static_cast(int_position - old_position); - if ((status_.mode != kStopped && status_.mode != kFault) && - std::abs(delta_position) > kMaxPositionDelta) { - // We probably had an error when reading the position. We must fault. - status_.mode = kFault; - status_.fault = errc::kEncoderFault; + if (!config_.fixed_voltage_mode) { + if ((status_.mode != kStopped && status_.mode != kFault) && + std::abs(delta_position) > kMaxPositionDelta) { + // We probably had an error when reading the position. We must fault. + status_.mode = kFault; + status_.fault = errc::kEncoderFault; + } + } else { + // In fixed voltage mode, we don't need the encoder at all, so + // don't fault if it isn't present. } // While we are in the first calibrating state, our unwrapped @@ -1756,6 +1770,32 @@ class BldcServo::Impl { velocity_command = 0.0f; } + // At this point, our control position and velocity are known. + + if (config_.fixed_voltage_mode) { + status_.unwrapped_position = + static_cast( + static_cast( + *status_.control_position >> 32)) / + 65536.0f; + status_.velocity = velocity_command; + + // For "fixed voltage" mode, we skip all position and current + // PID loops and all their associated calculations, including + // everything that uses the encoder. Instead we just burn power + // with a fixed voltage drive based on the desired position. + const float synthetic_electrical_theta = + WrapZeroToTwoPi( + status_.unwrapped_position + / motor_.unwrapped_position_scale + * position_constant_ + * k2Pi); + const SinCos synthetic_sin_cos = + cordic_(RadiansToQ31(synthetic_electrical_theta)); + ISR_DoVoltageDQ(synthetic_sin_cos, config_.fixed_voltage_control_V, 0.0f); + return; + } + const float measured_velocity = Threshold( status_.velocity, -config_.velocity_threshold, config_.velocity_threshold); diff --git a/fw/bldc_servo.h b/fw/bldc_servo.h index 3c4ee279..69d5edbc 100644 --- a/fw/bldc_servo.h +++ b/fw/bldc_servo.h @@ -181,6 +181,12 @@ class BldcServo { // configured for a low resistance motor. bool voltage_mode_control = false; + // If true, then the controller acts as a cheap gimbal controller + // and does not use the encoder at all. Instead, the desired + // position is used to command an open loop fixed voltage. + bool fixed_voltage_mode = false; + float fixed_voltage_control_V = 0.0f; + float max_position_slip = std::numeric_limits::quiet_NaN(); float default_timeout_s = 0.1f; @@ -256,6 +262,8 @@ class BldcServo { a->Visit(MJ_NVP(pid_dq)); a->Visit(MJ_NVP(pid_position)); a->Visit(MJ_NVP(voltage_mode_control)); + a->Visit(MJ_NVP(fixed_voltage_mode)); + a->Visit(MJ_NVP(fixed_voltage_control_V)); a->Visit(MJ_NVP(max_position_slip)); a->Visit(MJ_NVP(default_timeout_s)); a->Visit(MJ_NVP(timeout_max_torque_Nm));