sfunction_robot.cpp
Go to the documentation of this file.
00001 #define S_FUNCTION_NAME   sfunction_robot
00002 #define S_FUNCTION_LEVEL  2
00003 
00004 #define MDL_START
00005 
00006 
00007 #ifdef __cplusplus
00008 extern "C" {
00009 #endif
00010 
00011 #include "acado_common.h"
00012 #include "acado_auxiliary_functions.h"
00013 #include "simstruc.h"
00014 
00015 ACADOvariables acadoVariables;
00016 ACADOworkspace acadoWorkspace;
00017 
00018 
00019 static void mdlInitializeSizes (SimStruct *S)
00020 {
00021     /* Specify the number of continuous and discrete states */
00022     ssSetNumContStates(S, 0);
00023     ssSetNumDiscStates(S, 0);
00024 
00025     /* Specify the number of intput ports */
00026     if ( !ssSetNumInputPorts(S, 2) )
00027         return;
00028 
00029     /* Specify the number of output ports */
00030     if ( !ssSetNumOutputPorts(S, 2) )
00031         return;
00032 
00033     /* Specify the number of parameters */
00034     ssSetNumSFcnParams(S, 10);
00035     if ( ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S) )
00036         return;
00037 
00038     /* Specify dimension information for the input ports */
00039     ssSetInputPortVectorDimension(S, 0, ACADO_NX);
00040     ssSetInputPortVectorDimension(S, 1, ACADO_NY);
00041 
00042     /* Specify dimension information for the output ports */
00043     ssSetOutputPortVectorDimension(S, 0, ACADO_NU );
00044     ssSetOutputPortVectorDimension(S, 1, 1 );
00045 
00046     /* Specify the direct feedthrough status */
00047     ssSetInputPortDirectFeedThrough(S, 0, 1);
00048     ssSetInputPortDirectFeedThrough(S, 1, 1);
00049 
00050     /* One sample time */
00051     ssSetNumSampleTimes(S, 1);
00052     }
00053 
00054 
00055 #if defined(MATLAB_MEX_FILE)
00056 
00057 #define MDL_SET_INPUT_PORT_DIMENSION_INFO
00058 #define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
00059 
00060 static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
00061 {
00062     if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
00063          return;
00064 }
00065 
00066 static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
00067 {
00068     if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
00069          return;
00070 }
00071 
00072     #endif /* MATLAB_MEX_FILE */
00073 
00074 
00075 static void mdlInitializeSampleTimes(SimStruct *S)
00076 {
00077     double SAMPLINGTIME = mxGetScalar( ssGetSFcnParam(S, 0) );
00078     
00079     ssSetSampleTime(S, 0, SAMPLINGTIME);
00080     ssSetOffsetTime(S, 0, 0.0);
00081 }
00082 
00083 
00084 static void mdlStart(SimStruct *S)
00085 {
00086     int i, j, k;
00087 
00088     InputRealPtrsType in_ref;
00089     double *xInit, *uInit, *Smat, *SNmat, *numSteps, *lbV, *ubV, *lbAV, *ubAV;
00090 
00091     /* get inputs and perform feedback step */
00092     in_ref = ssGetInputPortRealSignalPtrs(S, 1);
00093 
00094     xInit = mxGetPr( ssGetSFcnParam(S, 1) );
00095     uInit = mxGetPr( ssGetSFcnParam(S, 2) );
00096 
00097     for( i=0; i < ACADO_N+1; ++i ) {
00098         for( j=0; j < ACADO_NX; ++j ) acadoVariables.x[i*ACADO_NX+j] = xInit[j];
00099     }
00100     for( i=0; i < ACADO_N; ++i ) {
00101         for( j=0; j < ACADO_NU; ++j ) acadoVariables.u[i*ACADO_NU+j] = uInit[j];
00102     }
00103 
00104     for( i=0; i < ACADO_N; ++i ) {
00105         for( j=0; j < ACADO_NY; ++j ) acadoVariables.y[i*ACADO_NY+j] = (double)(*in_ref[j]);
00106     }
00107     for( i=0; i < ACADO_NYN; ++i ) acadoVariables.yN[i] = (double)(*in_ref[i]);
00108     
00109     Smat = mxGetPr( ssGetSFcnParam(S, 3) );
00110     SNmat = mxGetPr( ssGetSFcnParam(S, 4) );
00111     numSteps = mxGetPr( ssGetSFcnParam(S, 5) );
00112     
00113     for( i = 0; i < (ACADO_NYN); ++i )  {
00114         for( j = 0; j < ACADO_NYN; ++j ) {
00115             acadoVariables.WN[i*ACADO_NYN+j] = SNmat[i*ACADO_NYN+j];
00116         }
00117     }
00118     for( i = 0; i < (ACADO_NY); ++i )  {
00119         for( j = 0; j < ACADO_NY; ++j ) {
00120             acadoVariables.W[i*ACADO_NY+j] = Smat[i*ACADO_NY+j];
00121         }
00122     }
00123     
00124     lbV = mxGetPr( ssGetSFcnParam(S, 6) );
00125     ubV = mxGetPr( ssGetSFcnParam(S, 7) );
00126     lbAV = mxGetPr( ssGetSFcnParam(S, 8) );
00127     ubAV = mxGetPr( ssGetSFcnParam(S, 9) );
00128     
00129     for( i=0; i < ACADO_N; ++i ) {
00130         for( j=0; j < ACADO_NU; ++j ) acadoVariables.lbValues[i*ACADO_NU+j] = lbV[j];
00131         for( j=0; j < ACADO_NU; ++j ) acadoVariables.ubValues[i*ACADO_NU+j] = ubV[j];
00132         
00133         for( j=0; j < ACADO_NU; ++j ) acadoVariables.lbAValues[i*ACADO_NU+j] = lbAV[j];
00134         for( j=0; j < ACADO_NU; ++j ) acadoVariables.ubAValues[i*ACADO_NU+j] = ubAV[j];
00135     }
00136 
00137     preparationStep( );
00138 }
00139 
00140 
00141 static void mdlOutputs(SimStruct *S, int_T tid)
00142 {
00143     int i, j, iter, error;
00144     double measurement[ACADO_NX];
00145     timer t;
00146     double timeFdb, timePrep;
00147 
00148     InputRealPtrsType in_x, in_ref;
00149     real_t *out_u0, *out_kktTol;
00150 
00151     /* get inputs and perform feedback step */
00152     in_x    = ssGetInputPortRealSignalPtrs(S, 0);
00153     in_ref = ssGetInputPortRealSignalPtrs(S, 1);
00154     
00155     for( i=0; i < ACADO_NX; ++i ) acadoVariables.x0[i] = (double)(*in_x[i]);
00156 
00157     for( i=0; i < ACADO_N-1; ++i ) {
00158         for( j=0; j < ACADO_NY; ++j ) acadoVariables.y[i*ACADO_NY+j] = acadoVariables.y[ACADO_NY+i*ACADO_NY+j];
00159     }
00160     for( i=0; i < ACADO_NYN; ++i ) acadoVariables.y[(ACADO_N-1)*ACADO_NY+i] = acadoVariables.yN[i];
00161     for( i=0; i < ACADO_NYN; ++i ) acadoVariables.yN[i] = (double)(*in_ref[i]);
00162     
00163         
00164     tic( &t );
00165     error = feedbackStep( );
00166     timeFdb = toc( &t );
00167     
00168     tic( &t );
00169     preparationStep( );
00170     timePrep = toc( &t );
00171     
00172     printf("NMPC step (error value: %d):\n", error);
00173     printf("Timing RTI iteration:   %.3g ms   +   %.3g ms   =   %.3g ms \n", timeFdb*1e3, timePrep*1e3, (timeFdb+timePrep)*1e3);
00174     printf("---------------------------------------------------------------\n");
00175 
00176     /* return outputs and prepare next iteration */
00177     out_u0     = ssGetOutputPortRealSignal(S, 0);
00178     out_kktTol = ssGetOutputPortRealSignal(S, 1);
00179 
00180     for( i=0; i < ACADO_NU; ++i ) out_u0[i] = acadoVariables.u[i];
00181     out_kktTol[0] = getKKT( );
00182 }
00183 
00184 
00185 static void mdlTerminate(SimStruct *S)
00186 {
00187 }
00188 
00189 
00190 #ifdef  MATLAB_MEX_FILE
00191 #include "simulink.c"
00192 #else
00193 #include "cg_sfun.h"
00194 #endif
00195 
00196 
00197 #ifdef __cplusplus
00198 }
00199 #endif


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:55