sfun_trajgen_pos.tlc

%% Abstract:
%%      Tlc file for S-function sfun_trahgen.cpp

%implements "sfun_trajgen_pos" "C"

%% Function: BlockTypeSetup ====================================================
%function BlockTypeSetup(block, system) void
  %%
  %% The Target Language must be C++
  %if ::GenCPP==0 && !IsModelReferenceSimTarget()
    %<LibReportFatalError("This S-Function generated by the Legacy Code Tool must be only used with the C++ Target Language")>
  %endif

  %<LibAddToCommonIncludes("<ReflexxesAPI.h>")>
  %<LibAddToCommonIncludes("<RMLPositionFlags.h>")>
  %<LibAddToCommonIncludes("<RMLPositionInputParameters.h>")>
  %<LibAddToCommonIncludes("<RMLPositionOutputParameters.h>")>
  
  %<LibAddToModelSources("ReflexxesAPI")>
  %<LibAddToModelSources("TypeIIRMLCalculatePositionalExtrems")>
  %<LibAddToModelSources("TypeIIRMLDecisions")>
  %<LibAddToModelSources("TypeIIRMLDecisionTree1A")>
  %<LibAddToModelSources("TypeIIRMLDecisionTree1B")>
  %<LibAddToModelSources("TypeIIRMLDecisionTree1C")>
  %<LibAddToModelSources("TypeIIRMLDecisionTree2")>
  %<LibAddToModelSources("TypeIIRMLFallBackStrategy")>
  %<LibAddToModelSources("TypeIIRMLIsPhaseSynchronizationPossible")>
  %<LibAddToModelSources("TypeIIRMLPolynomial")>
  %<LibAddToModelSources("TypeIIRMLPosition")>
  %<LibAddToModelSources("TypeIIRMLQuicksort")>
  %<LibAddToModelSources("TypeIIRMLSetupModifiedSelectionVector")>
  %<LibAddToModelSources("TypeIIRMLStep1")>
  %<LibAddToModelSources("TypeIIRMLStep1IntermediateProfiles")>
  %<LibAddToModelSources("TypeIIRMLStep1Profiles")>
  %<LibAddToModelSources("TypeIIRMLStep2")>
  %<LibAddToModelSources("TypeIIRMLStep2IntermediateProfiles")>
  %<LibAddToModelSources("TypeIIRMLStep2PhaseSynchronization")>
  %<LibAddToModelSources("TypeIIRMLStep2Profiles")>
  %<LibAddToModelSources("TypeIIRMLStep2WithoutSynchronization")>
  %<LibAddToModelSources("TypeIIRMLStep3")>
  %<LibAddToModelSources("TypeIIRMLVelocity")>
  %<LibAddToModelSources("TypeIIRMLVelocityCalculatePositionalExtrems")>
  %<LibAddToModelSources("TypeIIRMLVelocityFallBackStrategy")>
  %<LibAddToModelSources("TypeIIRMLVelocityIsPhaseSynchronizationPossible")>
  %<LibAddToModelSources("TypeIIRMLVelocityMethods")>
  %<LibAddToModelSources("TypeIIRMLVelocitySetupPhaseSyncSelectionVector")>
%endfunction


%% Function: mdlInitializeConditions ========================================
%% Abstract:
%%    In this function, you should initialize the continuous and discrete
%%    states for your S-function block.  The initial states are placed
%%    in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
%%    You can also perform any other initialization activities that your
%%    S-function may require. Note, this routine will be called at the
%%    start of simulation and if it is present in an enabled subsystem
%%    configured to reset states, it will be call when the enabled
%%    subsystem restarts execution to reset the states.
%function InitializeConditions(block, system) Output
  /* %<Type> Block: %<Name> */
  {
    %% The mdlInitializeConditions method is called when the simulation
    %% start and every time an enabled subsystem is re-enabled.

    %% Reset the IWork flag to 1 when values need to be reinitialized.
    %<LibBlockIWork(BlockInit, "", "", 0)> = 1;
  }
%endfunction


%% Function: mdlStart =======================================================
%% Abstract:
%%   This function is called once at start of model execution. If you have
%%   states that should be initialized once, this is the place to do it.
%function Start(block, system) Output
  /* %<Type> Block: %<Name> */
  {
    %% Creating all relevant objects of the Type II Reflexxes Motion Library
    const unsigned int ndofs = static_cast<unsigned int>(%<SFcnParamSettings.NumberOfDoFs>);
    ReflexxesAPI                *RML   = new ReflexxesAPI( ndofs, %<SFcnParamSettings.SampleTime> );
    RMLPositionInputParameters  *IP    = new RMLPositionInputParameters( ndofs );
    RMLPositionOutputParameters *OP    = new RMLPositionOutputParameters( ndofs );
    RMLPositionFlags            *FLAGS = new RMLPositionFlags();
    
    FLAGS->SynchronizationBehavior = RMLFlags::%<SFcnParamSettings.SyncBehavior>;
    FLAGS->BehaviorAfterFinalStateOfMotionIsReached = RMLPositionFlags::%<SFcnParamSettings.AfterFinalStateBehavior>;
    
    %% Store new C++ object in the pointers vector
    %<LibBlockPWork(ReflexxesAPI, "", "", 0)> = RML;
    %<LibBlockPWork(RMLPositionInputParameters, "", "", 0)> = IP;
    %<LibBlockPWork(RMLPositionOutputParameters, "", "", 0)> = OP;
    %<LibBlockPWork(RMLPositionFlags, "", "", 0)> = FLAGS;
  }
%endfunction


%% Function: mdlOutputs =======================================================
%% Abstract:
%%   In this function, you compute the outputs of your S-function block.
%function Outputs(block, system) Output
  /* %<Type> Block: %<Name> */
  {
    %% Initialize values if the IWork vector flag is true. 
    if (%<LibBlockIWork(BlockInit, "", "", 0)> == 1) {
        const int_T ndofs = static_cast<int_T>(%<SFcnParamSettings.NumberOfDoFs>);
        for (int_T dof=0; dof < ndofs; dof++)
        {
            (%<LibBlockOutputSignalAddr(0, "", "", 0)>)[dof] = (%<LibBlockInputSignalAddr(2, "", "", 0)>)[dof];
            (%<LibBlockOutputSignalAddr(1, "", "", 0)>)[dof] = (%<LibBlockInputSignalAddr(3, "", "", 0)>)[dof];
            (%<LibBlockOutputSignalAddr(2, "", "", 0)>)[dof] = (%<LibBlockInputSignalAddr(4, "", "", 0)>)[dof];
        }

        %<LibBlockIWork(BlockInit, "", "", 0)> = 0;
    }
    else {
        %% Retrieve C++ object from the pointers vector
        RMLPositionOutputParameters *OP = static_cast<RMLPositionOutputParameters *>(%<LibBlockPWork(RMLPositionOutputParameters, "", "", 0)>);
    
        const int_T ndofs = static_cast<int_T>(%<SFcnParamSettings.NumberOfDoFs>);
        for (int_T dof=0; dof < ndofs; dof++)
        {
            (%<LibBlockOutputSignalAddr(0, "", "", 0)>)[dof] = OP->NewPositionVector->VecData[dof];
            (%<LibBlockOutputSignalAddr(1, "", "", 0)>)[dof] = OP->NewVelocityVector->VecData[dof];
            (%<LibBlockOutputSignalAddr(2, "", "", 0)>)[dof] = OP->NewAccelerationVector->VecData[dof];
        }
    }
  }
%endfunction


%% Function: mdlUpdate ======================================================
%% Abstract:
%%    This function is called once for every major integration time step.
%%    Discrete states are typically updated here, but this function is
%%    useful for performing any tasks that should only take place once per
%%    integration step.
%function Update(block, system) Output
  /* %<Type> Block: %<Name> */
  {
    %% Retrieve C++ object from the pointers vector
    ReflexxesAPI                *RML   = static_cast<ReflexxesAPI *>(%<LibBlockPWork(ReflexxesAPI, "", "", 0)>);
    RMLPositionInputParameters  *IP    = static_cast<RMLPositionInputParameters *>(%<LibBlockPWork(RMLPositionInputParameters, "", "", 0)>);
    RMLPositionOutputParameters *OP    = static_cast<RMLPositionOutputParameters *>(%<LibBlockPWork(RMLPositionOutputParameters, "", "", 0)>);
    RMLPositionFlags            *FLAGS = static_cast<RMLPositionFlags *>(%<LibBlockPWork(RMLPositionFlags, "", "", 0)>);
    
    %% Set-up the input parameters
    const int_T ndofs = static_cast<int_T>(%<SFcnParamSettings.NumberOfDoFs>);
    for (int_T dof=0; dof < ndofs; dof++)
    {
        IP->CurrentPositionVector->VecData[dof]     = (%<LibBlockInputSignalAddr(2, "", "", 0)>)[dof];
        IP->CurrentVelocityVector->VecData[dof]     = (%<LibBlockInputSignalAddr(3, "", "", 0)>)[dof];
        IP->CurrentAccelerationVector->VecData[dof] = (%<LibBlockInputSignalAddr(4, "", "", 0)>)[dof];
        IP->MaxVelocityVector->VecData[dof]         = (%<LibBlockParameterAddr(VelLimit, "", "", 0)>)[dof];
        IP->MaxAccelerationVector->VecData[dof]     = (%<LibBlockParameterAddr(AccLimit, "", "", 0)>)[dof];
        IP->MaxJerkVector->VecData[dof]             = (%<LibBlockParameterAddr(JrkLimit, "", "", 0)>)[dof];
        IP->TargetPositionVector->VecData[dof]      = (%<LibBlockInputSignalAddr(0, "", "", 0)>)[dof];
        IP->TargetVelocityVector->VecData[dof]      = (%<LibBlockInputSignalAddr(1, "", "", 0)>)[dof];
        IP->SelectionVector->VecData[dof]           = true;
    }
    
    %% Calling the Reflexxes OTG algorithm
    int_T ResultValue = RML->RMLPosition( *IP, OP, *FLAGS );
  }
%endfunction


%% Function: mdlTerminate =====================================================
%% Abstract:
%%   In this function, you should perform any actions that are necessary
%%   at the termination of a simulation.  For example, if memory was
%%   allocated in mdlStart, this is the place to free it.
%function Terminate(block, system) Output
  /* %<Type> Block: %<Name> */
  {
    %% Retrieve and destroy C++ objects
    ReflexxesAPI                *RML   = static_cast<ReflexxesAPI *>(%<LibBlockPWork(ReflexxesAPI, "", "", 0)>);
    RMLPositionInputParameters  *IP    = static_cast<RMLPositionInputParameters *>(%<LibBlockPWork(RMLPositionInputParameters, "", "", 0)>);
    RMLPositionOutputParameters *OP    = static_cast<RMLPositionOutputParameters *>(%<LibBlockPWork(RMLPositionOutputParameters, "", "", 0)>);
    RMLPositionFlags            *FLAGS = static_cast<RMLPositionFlags *>(%<LibBlockPWork(RMLPositionFlags, "", "", 0)>);
    
    %% Deleting the objects of the Reflexxes Motion Library end terminating
    %% the process
    delete  RML;
    delete  IP;
    delete  OP;
    delete  FLAGS;
  }
%endfunction