pid3con.c

自己写的pid控制的s函数

/* S函数
C代码格式
*/

#define S_FUNCTION_NAME pid3con
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"

#define U(element) (*uPtrs[element])  /* Pointer to Input Port0 */

/*参数设置*/
#define NUM_PARAMS     (4)
#define P (ssGetSFcnParam(S,0))
#define I (ssGetSFcnParam(S,1))
#define D (ssGetSFcnParam(S,2))
#define TT (ssGetSFcnParam(S,3))

#define Kp ((real_T)mxGetPr(P)[0])
#define Ki ((real_T)mxGetPr(I)[0])
#define Kd ((real_T)mxGetPr(D)[0])
#define tt ((real_T)mxGetPr(TT)[0])

/*====================*
 * S-function methods *
 *====================*/
 
 #define MDL_CHECK_PARAMETERS   /* Change to #undef to remove function */
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
/* Function: mdlCheckParameters ===============================================
 * only Can1UserBaudrate and Can2UserBaudrate are inputed by user
 */
static void mdlCheckParameters(SimStruct *S)
{
}
#endif /* MDL_CHECK_PARAMETERS */
 

/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 初始化
 */
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, 4);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        return; /* Parameter mismatch will be reported by Simulink */
    }

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 3);

    if (!ssSetNumInputPorts(S, 1)) return;
    ssSetInputPortWidth(S, 0, 1);
    ssSetInputPortDirectFeedThrough(S, 0, 1);

    if (!ssSetNumOutputPorts(S, 1)) return;
    ssSetOutputPortWidth(S, 0, 1);

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Take care when specifying exception free code - see sfuntmpl_doc.c */
    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
}



/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    Specify the sample time as 1.0
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{	
    ssSetSampleTime(S, 0, tt);
    ssSetOffsetTime(S, 0, 0.0);
   /* ssSetModelReferenceSampleTimeDefaultInheritance(S);  */    
}

#define MDL_INITIALIZE_CONDITIONS
/* Function: mdlInitializeConditions ========================================
 * Abstract:
 *    Initialize both discrete states to one.
 初始化设置
 */
static void mdlInitializeConditions(SimStruct *S)
{
    real_T *x0 = ssGetRealDiscStates(S);
    int_T  lp;

    for (lp=0;lp<3;lp++) { 
        *x0++=0.0; 
    }
}

#define MDL_START
static void mdlStart(SimStruct *S)
{
}
/*输出*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
    real_T            *y    = ssGetOutputPortRealSignal(S,0);
    real_T            *x    = ssGetRealDiscStates(S);
    InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
 
    UNUSED_ARG(tid); /* not used in single tasking mode */
    
    y[0]=x[0]-(Kp+2*Kd)*x[1]+Kd*x[2]+(Kp+Ki+Kd)*U(0);
    printf("/nHelloWorld!/n");

}

#define MDL_UPDATE

/*状态更新
*/
static void mdlUpdate(SimStruct *S, int_T tid)
{
    real_T            tempX[3] = {0.0, 0.0,0.0};
    real_T            *x       = ssGetRealDiscStates(S);
    InputRealPtrsType uPtrs    = ssGetInputPortRealSignalPtrs(S,0);

    UNUSED_ARG(tid); /* not used in single tasking mode */

    tempX[0]=x[0]-(Kp+2*Kd)*x[1]+Kd*x[2]+(Kp+Ki+Kd)*U(0);
    tempX[1]=U(0);
    tempX[2]=x[1];
 
    x[0]=tempX[0];
    x[1]=tempX[1];
    x[2]=tempX[2];
    printf("/nHelloWorld!/n");
}



/* Function: mdlTerminate =====================================================
 * Abstract:
 *    No termination needed, but we are required to have this routine.
 */
static void mdlTerminate(SimStruct *S)
{
    UNUSED_ARG(S); /* unused input argument */
}

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif