Motor Control Workbench Position Control: Proper way to reset theta.

[M_Schmidt]

What is the proper way with position control to reset the current angle by for example 50 rotations towards a smaller value?
If the theta keeps increasing forever, with the float number representation, there will be an error during continuous operations in the same direction. (Smallest increment of a float that is not able to represent 1 as smallest number:

mantissa_bits = 23
precision_bits = mantissa_bits + 1 // include implicit leading 1
N_max_exact = 2 ^ precision_bits
= 2 ^ 24

So if theta reaches 16.777.216 or 2.6 million rotations, there will be a significant error relative to the motor angle. Is that handled in the MCWB position control somehow? What is the intended usage for this problem?

Below is the intended code-example with increasing position by 180°*of the output shaft after a gear ratio of 50 each cycle:

/*
 * MotorControl_Interface.c
 *
 *  Created on: 26.03.2026
 */
#include "MotorControl_Interface/Inc/MotorControl_Interface.h"
#include "main.h"
#include <cmsis_os.h>
#include <math.h>

#define TWO_PI (2.0f * M_PI)
#define GEAR_RATIO 50.0f
#define FULL_ROT_RAD (TWO_PI * GEAR_RATIO)
#define POS_TO_MECH (FULL_ROT_RAD / 360.0f)

static float targetPos[2] = {0.0f, 0.0f};
static float maxSpeed[2] = {50.0f, 50.0f}; // rad/s

// Utility: wrap angle into [-FULL_ROT_RAD/2 .. +FULL_ROT_RAD/2]
static inline float wrapAngle(const float angle)
{
    float a = fmodf(angle, FULL_ROT_RAD);
    if (a > FULL_ROT_RAD/2.0f) a -= FULL_ROT_RAD;
    if (a < -FULL_ROT_RAD/2.0f) a += FULL_ROT_RAD;
    return a;
}

// Get current position from MCWB
static float getCurrentPos(const uint8_t idx)
{
    return (idx == 0) ? MC_GetCurrentPosition1() : MC_GetCurrentPosition2();
}

// Program move to MCWB
static void programMove(const uint8_t idx, const float target, const float duration)
{
    if (idx == 0)
        MC_ProgramPositionCommandMotor1(target, duration);
    else
        MC_ProgramPositionCommandMotor2(target, duration);
}


ReturnType_MotorControl_Interface StartMotor(const MotorNB_MotorControl_Interface motor)
{
    if (motor & Motor1) MC_StartMotor1();
    if (motor & Motor2) MC_StartMotor2();

    while (((motor & Motor1) && MC_GetAlignmentStatusMotor1() != TC_ALIGNMENT_COMPLETED) ||
           ((motor & Motor2) && MC_GetAlignmentStatusMotor2() != TC_ALIGNMENT_COMPLETED))
    {
        osDelay(10);
    }
    return MC_I_OK;
}

ReturnType_MotorControl_Interface SetMotorMaxSpeed(const MotorNB_MotorControl_Interface motor, const float rpm)
{
    if (rpm <= 0.0f) return MC_I_ERROR_PARAM;
    const float rad_s = rpm * TWO_PI / 60.0f;
    if (motor & Motor1) maxSpeed[0] = rad_s;
    if (motor & Motor2) maxSpeed[1] = rad_s;
    return MC_I_OK;
}

ReturnType_MotorControl_Interface MoveMotorToPosition(
    const MotorNB_MotorControl_Interface motor,
    const MotorDirection_MotorControl_Interface direction,
    const int32_t position_deg)
{
    if ((motor & (Motor1|Motor2)) == 0) return MC_I_ERROR_PARAM;
    if (direction != positive_turning_direction && direction != negative_turning_direction) return MC_I_ERROR_PARAM;

    const float requested = position_deg * POS_TO_MECH;

    for (uint8_t i = 0; i < 2; i++)
    {
        if (!(motor & (i == 0 ? Motor1 : Motor2))) continue;
        float requested_corrected = requested;
        MotorDirection_MotorControl_Interface direction_corrected = direction;
        if(i == 1)
		{
			requested_corrected = POS_TO_MECH * 360.0f - requested;
			direction_corrected = !direction;
		}

        const float curr = getCurrentPos(i);
        const float wrappedCurr = wrapAngle(curr);

        // Compute raw delta in intended direction
        float delta = wrapAngle(requested_corrected) - wrappedCurr;
        if (direction_corrected == positive_turning_direction && delta < 0.0f) delta += FULL_ROT_RAD;
        if (direction_corrected == negative_turning_direction && delta > 0.0f) delta -= FULL_ROT_RAD;

        const float finalTarget = curr + delta;
        targetPos[i] = finalTarget;

        // Read max speed and max acceleration for this motor
        const float vmax = maxSpeed[i];
        const float amax = 40000;//TODO: when use that?(float)pPosCtrl[i]->Acceleration; // rad/s^2

        // Trapezoidal or triangular profile duration
        float d_acc = (vmax * vmax) / (2.0f * amax);
        float duration;
        if (fabsf(delta) <= 2.0f * d_acc)
        {
            // Triangular profile: never reach max speed
            duration = 2.0f * sqrtf(fabsf(delta) / amax);
        }
        else
        {
            // Trapezoidal profile: accelerate, cruise, decelerate
            duration = (fabsf(delta) - d_acc) / vmax + vmax / amax;
        }

        if (duration < 0.001f) duration = 0.001f;

        // Program the target
        programMove(i, finalTarget, duration);
    }

    return MC_I_OK;
}


// Check if motor reached target position (within tolerance)
bool CheckMotorStableAtTargetPosition(const MotorNB_MotorControl_Interface motor)
{
    bool ok = true;

    if (motor & Motor1)
    {
        if (MC_GetControlPositionStatusMotor1() == TC_MOVEMENT_ON_GOING ||
            fabsf(wrapAngle(MC_GetCurrentPosition1() - targetPos[0])) > 0.2f)
            ok = false;
    }

    if (motor & Motor2)
    {
        if (MC_GetControlPositionStatusMotor2() == TC_MOVEMENT_ON_GOING ||
            fabsf(wrapAngle(MC_GetCurrentPosition2() - targetPos[1])) > 0.2f)
            ok = false;
    }

    return ok;
}

// Read torque from MCWB
float ReadMotorTorque(const MotorNB_MotorControl_Interface motor)
{
    if (motor & Motor1) return MC_GetTerefMotor1_F();
    if (motor & Motor2) return MC_GetTerefMotor2_F();
    return 0.0f;
}


float test_torque, test_speed, test_angle;

void startTestingTask(const void * argument)
{
    const MotorNB_MotorControl_Interface motors = MotorBoth;

    osDelay(100);//Wait until Watchdog is started and Motors have Supply Voltage
    //osDelay(osWaitForever);
    pMCI[0]->pPosCtrl->PIDPosRegulator->hKpGain = 400;
    pMCI[0]->pPosCtrl->PIDPosRegulator->hKiGain = 250;
    pMCI[0]->pPosCtrl->PIDPosRegulator->hKdGain = 50;

    pMCI[1]->pPosCtrl->PIDPosRegulator->hKpGain = 400;
    pMCI[1]->pPosCtrl->PIDPosRegulator->hKiGain = 250;
    pMCI[1]->pPosCtrl->PIDPosRegulator->hKdGain = 50;

    MC_AcknowledgeFaultMotor1();
    MC_AcknowledgeFaultMotor2();

    osDelay(100);

    StartMotor(motors);

    MotorDirection_MotorControl_Interface dir = positive_turning_direction;
    static const uint32_t POS = 180;

    while(true)
    {
        while(!CheckMotorStableAtTargetPosition(motors))
        {
            test_torque = ReadMotorTorque(Motor1);
            test_speed = pMCI[0]->pSTC->SPD->hAvrMecSpeedUnit;
            test_angle = MC_GetCurrentPosition1();
            osDelay(10);
        }


        osDelay(500);

        MoveMotorToPosition(MotorBoth, positive_turning_direction, (dir == positive_turning_direction ? POS : 0));
        dir = (dir == positive_turning_direction) ? negative_turning_direction : positive_turning_direction;
    }
}

I tried following code to reset to a smaller value when stationary:

// Idle-only re-zero: call when motor stopped
static void tryReZero(uint8_t idx)
{
    bool isIdle =
                    (MC_GetControlPositionStatusMotor1() != TC_MOVEMENT_ON_GOING) &&
                    (MC_GetControlPositionStatusMotor2() != TC_MOVEMENT_ON_GOING);
    if (!isIdle) return;

    //osDelay(2000);
    idx = 0;
    float pos = getCurrentPos(0);
    if (fabsf(pos) > REZERO_THRESHOLD)
    {
    	{
			const float wrapped = pos - FULL_ROT_RAD;      // exact wrapped angle

			// --- Copy from TC_EncoderReset ---
			PosCtrl_Handle_t* const pHandle = pMCI[0]->pPosCtrl;
			pHandle->MecAngleOffset = pHandle->pENC->_Super.hMecAngle;
			pHandle->pENC->_Super.wMecAngle = wrapped;
			//pHandle->EncoderAbsoluteAligned = true;
			//pHandle->AlignmentStatus = TC_ALIGNMENT_COMPLETED;
			//pHandle->PositionCtrlStatus = TC_READY_FOR_COMMAND;
			pHandle->Theta = wrapped;

			ENC_SetMecAngle(pHandle->pENC, pHandle->MecAngleOffset);

			// Shift your application-level target to stay consistent
			targetPos[0] -= FULL_ROT_RAD;

    	}
        {
            pos = getCurrentPos(1);
			const float wrapped = pos + FULL_ROT_RAD;      // exact wrapped angle

			// --- Copy from TC_EncoderReset ---
			PosCtrl_Handle_t* const pHandle = pMCI[1]->pPosCtrl;
			pHandle->MecAngleOffset = pHandle->pENC->_Super.hMecAngle;
			pHandle->pENC->_Super.wMecAngle = wrapped;
			//pHandle->EncoderAbsoluteAligned = true;
			//pHandle->AlignmentStatus = TC_ALIGNMENT_COMPLETED;
			//pHandle->PositionCtrlStatus = TC_READY_FOR_COMMAND;
			pHandle->Theta = wrapped;

			ENC_SetMecAngle(pHandle->pENC, pHandle->MecAngleOffset);

			// Shift your application-level target to stay consistent
			targetPos[1] += FULL_ROT_RAD;
        }
	}
}

This had some unintended behaviours, that the motor starts twitching, sometimes even theta jumping by 25 motor rotations, but not every time it is reset, just sometimes.
 
Is there some way to cleanly change theta without the motor moving?

[GMA]

Hello @M_Schmidt,

Position control management occurs at the medium-frequency task level.
Did you place the tryReZero() function at this level using the following entry point?

      /* Applicative hook at end of Medium Frequency for Motor 1 */
      MC_APP_PostMediumFrequencyHook_M1();

[M_Schmidt]

Dear @GMA 
The tryReZero() function is called from within the CheckMotorStableAtTargetPosition() function. After decreasing the threshhold inside that function to

if (motor & Motor1)
    {
        if (MC_GetControlPositionStatusMotor1() == TC_MOVEMENT_ON_GOING ||
            fabsf(wrapAngle(MC_GetCurrentPosition1() - targetPos[0])) > 0.01f)
            ok = false;
        else
            tryReZero(Motor1);
    }

and removing the call to

ENC_SetMecAngle(pHandle->pENC, pHandle->MecAngleOffset);

no more twitching of the motor happens, that seemed to be a bug in the code, that now works as expected. The tryReZero even works with an active motor (MCI_GetSTMState == RUN) with active position control.

Please excuse the confusion from my end, as the symptom of my buggy code and the underlying issue are not the same problem.
Attached is a working code snippet of the current task interfacing the MCWB functions and variables, with the limitation of resetting both motors only if motor2 is correctly positioned. I will provide a working example for both motors once fully tested.
But: Over the runtime there is a small noticeable (seemingly) addative error at the output shaft after the gearbox. I will keep it running for a longer time, to see if it actually is increasing.

Therefor my original question: What would the intended reset/change to the current mechanical angle theta be? How should it be changed? 
Is the problem with floating point representation handled by the MCWB generated code somehow?

[GMA · Accepted solution]

Hello @M_Schmidt,

Your firmware should be called at Medium Frequency task level as "Position Control" controller is called at this level (controller manage the position even if feature status is TC_TARGET_POSITION_REACHED).

There is no additional processing of the floating-point format for the absolute position in MCSDK.