From d2246d2f7ba751a688b182b3f520480a36316bdf Mon Sep 17 00:00:00 2001 From: avanrossum <36618231+ator1811@users.noreply.github.com> Date: Tue, 30 Jun 2026 17:24:46 +0200 Subject: [PATCH] Add MotionControlType::angle_nocascade support to DCMotor::move() Direct-voltage angle control: P_angle output goes straight to voltage.q without an inner velocity loop, mirroring the FOCMotor base class implementation. LPF_angle is applied to shaft_angle before the error calculation (configurable via commander AAF; passthrough when Tf=0). Output is clamped to voltage_limit via _constrain. Previously this mode hit the default branch, which called disable() and set motor_status = motor_error, silently stopping the motor. Co-Authored-By: Claude Sonnet 4.6 --- src/DCMotor.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/DCMotor.cpp b/src/DCMotor.cpp index e5491ce..ea9e614 100644 --- a/src/DCMotor.cpp +++ b/src/DCMotor.cpp @@ -110,6 +110,13 @@ void DCMotor::move(float new_target) { else voltage.q = 0.0f; // TODO implement the other torque modes, and current control loop break; + case MotionControlType::angle_nocascade: + // non-cascaded angle control: P_angle output goes directly to voltage + // (no inner velocity loop — mirrors FOCMotor implementation) + shaft_angle_sp = target; + voltage.q = P_angle(shaft_angle_sp - LPF_angle(shaft_angle)); + voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit); + break; case MotionControlType::angle: // angle set point // include angle loop