Halting Position Control via the Motor Control Protocol

[Vasalic]

Is there a way to halt a position control move? It seems that issuing the stop and then reissuing the start will not clear the previous position control command and the controller will still attempt to achieve the position after the stop. Issuing another position move request does not seem to stop the process either as the code seems to reject move requests until they are complete.

Here’s the code in question from the MCSDK 6.4.2

case MC_REG_POSITION_RAMP:
          {
            FloatToU32 Position;
            FloatToU32 Duration;
            /* 32 bits access are split into 2x16 bits access */
            Position.U32_Val = ((int32_t)(*(int16_t *)&rawData[2]))<<16 | *(uint16_t *)rawData;
            /* 32 bits access are split into 2x16 bits access */
            Duration.U32_Val = ((int32_t)(*(int16_t *)&rawData[6]))<<16 | *(uint16_t *)&rawData[4];
            MCI_ExecPositionCommand(pMCIN, Position.Float_Val, Duration.Float_Val);
            break;
          }

__weak void MCI_ExecPositionCommand(MCI_Handle_t *pHandle, float_t FinalPosition, float_t Duration)
{
#ifdef NULL_PTR_CHECK_MC_INT
  if (MC_NULL == pHandle)
  {
    /* Nothing to do */
  }
  else
  {
#endif
    pHandle->pFOCVars->bDriveInput = INTERNAL;
    float_t currentPositionRad = (float_t)(SPD_GetMecAngle(STC_GetSpeedSensor(pHandle->pSTC))) / RADTOS16;
    if (Duration > 0)
    {
      TC_MoveCommand(pHandle->pPosCtrl, currentPositionRad, FinalPosition - currentPositionRad, Duration);
    }
    else
    {
      TC_FollowCommand(pHandle->pPosCtrl, FinalPosition);
    }

    pHandle->LastModalitySetByUser = MCM_TORQUE_MODE;
#ifdef NULL_PTR_CHECK_MC_INT
  }
#endif
}


bool TC_MoveCommand(PosCtrl_Handle_t *pHandle, float startingAngle, float angleStep, float movementDuration)
{

  bool RetConfigStatus = false;
  float fMinimumStepDuration;

  if ((pHandle->PositionCtrlStatus == TC_FOLLOWING_ON_GOING) && (movementDuration > 0))
  {
    /* Back to Move command as the movement duration is different from 0 */
    pHandle->PositionCtrlStatus = TC_READY_FOR_COMMAND;
  }
  else
  {
    /* Nothing to do */
  }

  if ((pHandle->PositionCtrlStatus == TC_READY_FOR_COMMAND) && (movementDuration > 0))
  {
    pHandle->PositionControlRegulation = ENABLE;

    fMinimumStepDuration = (9.0f * pHandle->SamplingTime);

    /* WARNING: Movement duration value is rounded to the nearest valid value
       [(DeltaT/9) / SamplingTime]:  shall be an integer value */
    pHandle->MovementDuration = (float)((int)(movementDuration / fMinimumStepDuration)) * fMinimumStepDuration;

    pHandle->StartingAngle = startingAngle;
    pHandle->AngleStep = angleStep;
    pHandle->FinalAngle = startingAngle + angleStep;

    /* SubStep duration = DeltaT/9  (DeltaT represents the total duration of the programmed movement) */
    pHandle->SubStepDuration = pHandle->MovementDuration / 9.0f;

    /* Sub step of acceleration phase */
    pHandle->SubStep[0] = 1 * pHandle->SubStepDuration;   /* Sub-step 1 of acceleration phase */
    pHandle->SubStep[1] = 2 * pHandle->SubStepDuration;   /* Sub-step 2 of acceleration phase */
    pHandle->SubStep[2] = 3 * pHandle->SubStepDuration;   /* Sub-step 3 of acceleration phase */

    /* Sub step of  deceleration Phase */
    pHandle->SubStep[3] = 6 * pHandle->SubStepDuration;   /* Sub-step 1 of deceleration phase */
    pHandle->SubStep[4] = 7 * pHandle->SubStepDuration;   /* Sub-step 2 of deceleration phase */
    pHandle->SubStep[5] = 8 * pHandle->SubStepDuration;   /* Sub-step 3 of deceleration phase */

    /* Jerk (J) to be used by the trajectory calculator to integrate (step by step) the target position.
       J = DeltaTheta/(12 * A * A * A)  => DeltaTheta = final position and A = Sub-Step duration */
    pHandle->Jerk = pHandle->AngleStep / (12 * pHandle->SubStepDuration * pHandle->SubStepDuration * pHandle->SubStepDuration);

    /* Speed cruiser = 2*J*A*A) */
    pHandle->CruiseSpeed = 2 * pHandle->Jerk * pHandle->SubStepDuration * pHandle->SubStepDuration;

    pHandle->ElapseTime = 0.0f;

    pHandle->Omega = 0.0f;
    pHandle->Acceleration = 0.0f;
    pHandle->Theta = startingAngle;

    pHandle->PositionCtrlStatus = TC_MOVEMENT_ON_GOING;   /* new trajectory has been programmed */

    RetConfigStatus = true;

  }
  else
  {
    /* Nothing to do */
  }
  return (RetConfigStatus);
}

 

[GMA]

Hello ​@Vasalic,

MC_StopMotor1() acts as a pause in the current trajectory, and MC_StartMotor1() resumes it.
No API is available to stop the motor trajectory and keep the current position.
After MC_StopMotor1(), forcing the position control status to TC_READY_FOR_COMMAND aborts the current trajectory and keeps the current position.

MC_StopMotor1();  

HAL_Delay(500);
Mci->pPosCtrl->PositionCtrlStatus = TC_READY_FOR_COMMAND;

MC_StartMotor1();