c2_robot_2link.c

Serial 2 bar robot PD control in simulink

      /* matrix G */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,43);
      c2_e_x = c2_thita[0] + c2_thita[1];
      c2_em_d5 = cos(c2_e_x);
      c2_f_x = c2_thita[0];
      c2_em_d6 = cos(c2_f_x);
      c2_g1 = c2_alpha * c2_iita * c2_em_d6 + c2_nita * c2_iita * c2_em_d5;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,44);
      c2_g_x = c2_thita[0] + c2_thita[1];
      c2_em_d7 = cos(c2_g_x);
      c2_g2 = c2_nita * c2_iita * c2_em_d7;
      /* matrix N */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,46);
      c2_n1 = c2_v1 + c2_g1;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,47);
      c2_n2 = c2_v2 + c2_g2;
      /* error */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,49);
      c2_e1 = c2_thita1D - c2_thita[0];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,50);
      c2_e2 = c2_thita2D - c2_thita[1];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,51);
      c2_e4 = c2_thita1Ddot - c2_thita[2];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,52);
      c2_e5 = c2_thita2Ddot - c2_thita[3];
      /* calculation of T */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,55);
      c2_s1 = (c2_thita1Dddot + c2_kd * c2_e4) + c2_kp * c2_e1;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,56);
      c2_s2 = (c2_thita2Dddot + c2_kd * c2_e5) + c2_kp * c2_e2;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,57);
      c2_b_Tau1 = c2_n1 + (c2_M11 * c2_s1 + c2_M12 * c2_s2);
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,58);
      c2_b_Tau2 = c2_n2 + (c2_M12 * c2_s1 + c2_M22 * c2_s2);
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,-58);
      sf_debug_pop_symbol_scope();
    }
    *c2_Tau2() = c2_b_Tau2;
    *c2_Tau1() = c2_b_Tau1;
    _SFD_CC_CALL(EXIT_OUT_OF_FUNCTION_TAG,0);
  }
  _sfEvent_ = c2_previousEvent;
}

static real_T c2_mpower(real_T c2_x1, real_T c2_x2)
{
  real_T c2_em_d9;
  real_T c2_b_x1;
  real_T c2_b_x2;
  c2_b_x1 = c2_x1;
  c2_b_x2 = c2_x2;
  {
    real_T c2_x1expand;
    real_T c2_x2expand;
    real_T c2_i;
    real_T c2_b_i;
    real_T c2_em_d10;
    real_T c2_c_x1;
    real_T c2_c_x2;
    c2_x1expand = c2_b_x1;
    c2_x2expand = c2_b_x2;
    c2_b_i = 1.0;
    c2_i = c2_b_i;
    _SFD_EML_ARRAY_BOUNDS_CHECK("x1expand", (int32_T)_SFD_INTEGER_CHECK("i",
      c2_i), 1, 1, 1);
    _SFD_EML_ARRAY_BOUNDS_CHECK("x2expand", (int32_T)_SFD_INTEGER_CHECK("i",
      c2_i), 1, 1, 1);
    _SFD_EML_ARRAY_BOUNDS_CHECK("y", (int32_T)_SFD_INTEGER_CHECK("i", c2_i), 1,
     1, 1);
    c2_c_x1 = c2_x1expand;
    c2_c_x2 = c2_x2expand;
    c2_em_d10 = pow(c2_c_x1, c2_c_x2);
    c2_em_d9 = c2_em_d10;
  }
  return c2_em_d9;
}

static real_T *c2_u(void)
{
  return (real_T *)ssGetInputPortSignal(chartInstance.S, 0);
}

static real_T *c2_Tau1(void)
{
  return (real_T *)ssGetOutputPortSignal(chartInstance.S, 1);
}

static real_T *c2_Tau2(void)
{
  return (real_T *)ssGetOutputPortSignal(chartInstance.S, 2);
}

/* SFunction Glue Code */
void sf_c2_robot_2link_get_check_sum(mxArray *plhs[])
{
  ((real_T *)mxGetPr((plhs[0])))[0] = (real_T)(2537247275U);
  ((real_T *)mxGetPr((plhs[0])))[1] = (real_T)(618470303U);
  ((real_T *)mxGetPr((plhs[0])))[2] = (real_T)(227147716U);
  ((real_T *)mxGetPr((plhs[0])))[3] = (real_T)(2146379267U);
}

mxArray *sf_c2_robot_2link_get_autoinheritance_info(void)
{
  const char *autoinheritanceInfoStructFieldNames[] =
  {"checksum","inputTypes","outputSizes","outputTypes"};
  mxArray *mxAutoinheritanceInfo = NULL;
  mxArray *mxChecksum = NULL;
  mxArray *mxInputTypes = NULL;
  mxArray *mxOutputSizes = NULL;
  mxArray *mxOutputTypes = NULL;

  mxAutoinheritanceInfo = mxCreateStructMatrix(1,1,
   sizeof(autoinheritanceInfoStructFieldNames)/sizeof(char *),
   autoinheritanceInfoStructFieldNames);

  mxChecksum = mxCreateDoubleMatrix(4,1,mxREAL);
  ((real_T *)mxGetPr((mxChecksum)))[0] = (real_T)(3080195939U);
  ((real_T *)mxGetPr((mxChecksum)))[1] = (real_T)(2643215801U);
  ((real_T *)mxGetPr((mxChecksum)))[2] = (real_T)(1929780124U);
  ((real_T *)mxGetPr((mxChecksum)))[3] = (real_T)(37664598U);

  mxInputTypes = mxCreateDoubleMatrix(1,1,mxREAL);
  ((real_T *)mxGetPr((mxInputTypes)))[0] = (real_T)(10U);

  mxOutputSizes = mxCreateDoubleMatrix(2,2,mxREAL);
  mxOutputTypes = mxCreateDoubleMatrix(1,2,mxREAL);
  ((real_T *)mxGetPr((mxOutputSizes)))[0] = (real_T)(1U);
  ((real_T *)mxGetPr((mxOutputSizes)))[1] = (real_T)(1U);
  ((real_T *)mxGetPr((mxOutputTypes)))[0] = (real_T)(10U);
  ((real_T *)mxGetPr((mxOutputSizes)))[2] = (real_T)(1U);
  ((real_T *)mxGetPr((mxOutputSizes)))[3] = (real_T)(1U);
  ((real_T *)mxGetPr((mxOutputTypes)))[1] = (real_T)(10U);

  mxSetFieldByNumber(mxAutoinheritanceInfo,0,0,mxChecksum);
  mxSetFieldByNumber(mxAutoinheritanceInfo,0,1,mxInputTypes);
  mxSetFieldByNumber(mxAutoinheritanceInfo,0,2,mxOutputSizes);
  mxSetFieldByNumber(mxAutoinheritanceInfo,0,3,mxOutputTypes);

  return(mxAutoinheritanceInfo);
}

static void chart_debug_initialization(SimStruct *S)
{
  if(ssIsFirstInitCond(S)) {
    /* do this only if simulation is starting */
    if(!sim_mode_is_rtw_gen(S)) {
      {
        unsigned int chartAlreadyPresent;
        chartAlreadyPresent =
          sf_debug_initialize_chart(_robot_2linkMachineNumber_,
         2,
         1,
         1,
         3,
         0,
         0,
         0,
         0,
         &(chartInstance.chartNumber),
         &(chartInstance.instanceNumber),
         ssGetPath(S),
         (void *)S);
        if(chartAlreadyPresent==0) {
          /* this is the first instance */
          sf_debug_set_chart_disable_implicit_casting(_robot_2linkMachineNumber_,chartInstance.chartNumber,1);
          sf_debug_set_chart_event_thresholds(_robot_2linkMachineNumber_,
           chartInstance.chartNumber,
           0,
           0,
           0);

          {
            unsigned int dimVector[1];
            dimVector[0]= 10;
            _SFD_SET_DATA_PROPS(2,1,1,0,SF_DOUBLE,1,&(dimVector[0]),0,0.0,1.0,0,"u",0);
          }
          _SFD_SET_DATA_PROPS(0,2,0,1,SF_DOUBLE,0,NULL,0,0.0,1.0,0,"Tau1",0);
          _SFD_SET_DATA_PROPS(1,2,0,1,SF_DOUBLE,0,NULL,0,0.0,1.0,0,"Tau2",0);
          _SFD_STATE_INFO(0,0,2);
          _SFD_CH_SUBSTATE_COUNT(0);
          _SFD_CH_SUBSTATE_DECOMP(0);
        }
        _SFD_CV_INIT_CHART(0,0,0,0);
        {
          _SFD_CV_INIT_STATE(0,0,0,0,0,0,NULL,NULL);
        }

        _SFD_CV_INIT_TRANS(0,0,NULL,NULL,0,NULL);

        /* Initialization of EML Model Coverage */
        _SFD_CV_INIT_EML(0,1,0,0,0,0,0,0);
        _SFD_CV_INIT_EML_FCN(0,0,"eML_blk_kernel",0,-1,1192);
        _SFD_TRANS_COV_WTS(0,0,0,1,0);
        if(chartAlreadyPresent==0)
        {
          _SFD_TRANS_COV_MAPS(0,
           0,NULL,NULL,
           0,NULL,NULL,
           1,NULL,NULL,
           0,NULL,NULL);
        }
        _SFD_SET_DATA_VALUE_PTR(2U, c2_u());
        _SFD_SET_DATA_VALUE_PTR(0U, c2_Tau1());
        _SFD_SET_DATA_VALUE_PTR(1U, c2_Tau2());
      }
    }
  } else {
    sf_debug_reset_current_state_configuration(_robot_2linkMachineNumber_,chartInstance.chartNumber,chartInstance.instanceNumber);
  }
}

static void sf_opaque_initialize_c2_robot_2link(void *chartInstanceVar)
{
  chart_debug_initialization(chartInstance.S);
  initialize_c2_robot_2link();
}

static void sf_opaque_enable_c2_robot_2link(void *chartInstanceVar)
{
  enable_c2_robot_2link();
}

static void sf_opaque_disable_c2_robot_2link(void *chartInstanceVar)
{
  disable_c2_robot_2link();
}

static void sf_opaque_gateway_c2_robot_2link(void *chartInstanceVar)
{
  sf_c2_robot_2link();
}

static void sf_opaque_terminate_c2_robot_2link(void *chartInstanceVar)
{
  finalize_c2_robot_2link();
}

static void mdlSetWorkWidths_c2_robot_2link(SimStruct *S)
{
  if(sim_mode_is_rtw_gen(S)) {
    int_T chartIsInlinable =
      (int_T)sf_is_chart_inlinable("robot_2link",2);
    int_T chartIsMultiInstanced =
      (int_T)sf_is_chart_multi_instanced("robot_2link",2);
    ssSetStateflowIsInlinable(S,chartIsInlinable);
    ssSetEnableFcnIsTrivial(S,1);
    ssSetDisableFcnIsTrivial(S,1);
    if(chartIsInlinable) {
      ssSetInputPortOptimOpts(S, 0, SS_REUSABLE_AND_LOCAL);
      sf_mark_chart_expressionable_inputs(S,"robot_2link",2,1);
      sf_mark_chart_reusable_outputs(S,"robot_2link",2,2);
    }
    if (!sf_is_chart_instance_optimized_out("robot_2link",2)) {
      int dtId;
      char *chartInstanceTypedefName =
        sf_chart_instance_typedef_name("robot_2link",2);
      dtId = ssRegisterDataType(S, chartInstanceTypedefName);
      if (dtId == INVALID_DTYPE_ID ) return;
      /* Register the size of the udt */
      if (!ssSetDataTypeSize(S, dtId, 8)) return;
      if(!ssSetNumDWork(S,1)) return;
      ssSetDWorkDataType(S, 0, dtId);
      ssSetDWorkWidth(S, 0, 1);
      ssSetDWorkName(S, 0, "ChartInstance"); /*optional name, less than 16 chars*/
      sf_set_rtw_identifier(S);
    }
    ssSetHasSubFunctions(S,!(chartIsInlinable));
    ssSetOptions(S,ssGetOptions(S)|SS_OPTION_WORKS_WITH_CODE_REUSE);
  }

  ssSetChecksum0(S,(2537247275U));
  ssSetChecksum1(S,(618470303U));
  ssSetChecksum2(S,(227147716U));
  ssSetChecksum3(S,(2146379267U));

  ssSetExplicitFCSSCtrl(S,1);
}

static void mdlRTW_c2_robot_2link(SimStruct *S)
{
  sf_write_symbol_mapping(S, "robot_2link", 2);
  ssWriteRTWStrParam(S, "StateflowChartType", "Embedded MATLAB");
}

static void mdlStart_c2_robot_2link(SimStruct *S)
{
  chartInstance.chartInfo.chartInstance = NULL;
  chartInstance.chartInfo.isEMLChart = 1;
  chartInstance.chartInfo.chartInitialized = 0;
  chartInstance.chartInfo.sFunctionGateway = sf_opaque_gateway_c2_robot_2link;
  chartInstance.chartInfo.initializeChart = sf_opaque_initialize_c2_robot_2link;
  chartInstance.chartInfo.terminateChart = sf_opaque_terminate_c2_robot_2link;
  chartInstance.chartInfo.enableChart = sf_opaque_enable_c2_robot_2link;
  chartInstance.chartInfo.disableChart = sf_opaque_disable_c2_robot_2link;
  chartInstance.chartInfo.mdlRTW = mdlRTW_c2_robot_2link;
  chartInstance.chartInfo.mdlStart = mdlStart_c2_robot_2link;
  chartInstance.chartInfo.mdlSetWorkWidths = mdlSetWorkWidths_c2_robot_2link;
  chartInstance.chartInfo.restoreLastMajorStepConfiguration = NULL;
  chartInstance.chartInfo.restoreBeforeLastMajorStepConfiguration = NULL;
  chartInstance.chartInfo.storeCurrentConfiguration = NULL;
  chartInstance.S = S;
  ssSetUserData(S,(void *)(&(chartInstance.chartInfo))); /* register the chart instance with simstruct */
}

void c2_robot_2link_method_dispatcher(SimStruct *S, int_T method, void *data)
{
  switch (method) {
   case SS_CALL_MDL_START:
    mdlStart_c2_robot_2link(S);
    break;
   case SS_CALL_MDL_SET_WORK_WIDTHS:
    mdlSetWorkWidths_c2_robot_2link(S);
    break;
   default:
    /* Unhandled method */
    sf_mex_error_message("Stateflow Internal Error:\n"
     "Error calling c2_robot_2link_method_dispatcher.\n"
     "Can't handle method %d.\n", method);
    break;
  }
}