From: Carlos Jenkins Date: Fri, 22 Feb 2013 16:47:54 +0000 (+0100) Subject: Implemented most of the motor controller inside an S-Function. X-Git-Url: http://rtime.felk.cvut.cz/gitweb/jenkicar/testing.git/commitdiff_plain/e3947d8c5fbc98ad9617a8a70abd39e2b21ad285?hp=5c41621b85ba238478e78b94357ee8e6bd339ad9 Implemented most of the motor controller inside an S-Function. Phase shifted, don't know why yet. --- diff --git a/Motor controller/decoder.c b/Motor controller/decoder.c new file mode 100644 index 0000000..e6fb48c --- /dev/null +++ b/Motor controller/decoder.c @@ -0,0 +1,205 @@ +/* + * Copyright (C) 2013 Czech Technical University in Prague + * + * Authors: + * - Carlos Jenkins + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * File : decoder.c + * Abstract: + * Motor position and rotation decoder algorithm S-Function for Simulink. + * + * I A Ap B Bp | O Err + * ---------------------------- + * 00) 0 0 0 0 | 0 f + * 01) 0 0 0 1 | -1 f + * 02) 0 0 1 0 | +1 f + * 03) 0 0 1 1 | 0 f + * 04) 0 1 0 0 | +1 f + * 05) 0 1 0 1 | -2 t + * 06) 0 1 1 0 | -2 t + * 07) 0 1 1 1 | -1 f + * 08) 1 0 0 0 | -1 f + * 09) 1 0 0 1 | -2 t + * 10) 1 0 1 0 | -2 t + * 11) 1 0 1 1 | +1 f + * 12) 1 1 0 0 | 0 f + * 13) 1 1 0 1 | +1 f + * 14) 1 1 1 0 | -1 f + * 15) 1 1 1 1 | 0 f + * + */ + +#define S_FUNCTION_NAME decoder +#define S_FUNCTION_LEVEL 2 + +#include "simstruc.h" + +/* Static data */ +static int8_T DEC_TABLE[16] = { + 0, -1, +1, 0, +1, -2, -2, -1, -1, -2, -2, +1, 0, +1, -1, 0 + }; + +/* Global variables */ +boolean_T a; +boolean_T b; +boolean_T ap = false; +boolean_T bp = false; + +boolean_T err = false; +real_T acc = 0.0; + + +/* Function: mdlInitializeSizes =============================================== + * Abstract: + * Setup sizes of the various vectors. + */ +static void mdlInitializeSizes(SimStruct *S) +{ + /* Configure parameters */ + ssSetNumSFcnParams(S, 0); + if(ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) { + return; + } + + /* Configure input ports */ + if(!ssSetNumInputPorts(S, 2)) { + return; + } + ssSetInputPortWidth(S, 0, 1); /* Sensor A */ + ssSetInputPortDataType(S, 0, SS_BOOLEAN); + ssSetInputPortDirectFeedThrough(S, 0, 1); + ssSetInputPortRequiredContiguous(S, 0, 1); + + ssSetInputPortWidth(S, 1, 1); /* Sensor B */ + ssSetInputPortDataType(S, 1, SS_BOOLEAN); + ssSetInputPortDirectFeedThrough(S, 1, 1); + ssSetInputPortRequiredContiguous(S, 1, 1); + /* Contiguous is required so port can be + * accessed with ssGetInputPortSignal + */ + + /* Configure output ports */ + if(!ssSetNumOutputPorts(S, 2)) { + return; + } + ssSetOutputPortWidth(S, 0, 1); /* Output */ + ssSetOutputPortDataType(S, 1, SS_DOUBLE); + ssSetOutputPortWidth(S, 1, 1); /* Error flag */ + ssSetOutputPortDataType(S, 1, SS_BOOLEAN); + + /* Configure sample time */ + ssSetNumSampleTimes(S, 1); + + /* FIXME: Check the following lines, not sure what they does. */ + /* specify the sim state compliance to be same as a built-in block */ + ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE); + + /* Take care when specifying exception free code - see sfuntmpl_doc.c */ + ssSetOptions(S, + SS_OPTION_WORKS_WITH_CODE_REUSE | + /*SS_OPTION_EXCEPTION_FREE_CODE |*/ + SS_OPTION_USE_TLC_WITH_ACCELERATOR); +} + + +/* Function: mdlInitializeSampleTimes ========================================= + * Abstract: + * Specifiy that we inherit our sample time from the driving block. + */ +static void mdlInitializeSampleTimes(SimStruct *S) +{ + ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME); + ssSetOffsetTime(S, 0, 0.0); + ssSetModelReferenceSampleTimeDefaultInheritance(S); +} + +/* Function: mdlOutputs ======================================================= + * Abstract: + * o = table[i] + */ +static void mdlOutputs(SimStruct *S, int_T tid) +{ + /* IO ports pointers */ + boolean_T* inaprt = (boolean_T*) ssGetInputPortSignal(S, 0); + boolean_T* inbprt = (boolean_T*) ssGetInputPortSignal(S, 1); + real_T* outprt = (real_T*) ssGetOutputPortSignal(S, 0); + boolean_T* errprt = (boolean_T*) ssGetOutputPortSignal(S, 1); + + /* Reset variables */ + err = false; + a = inaprt[0]; + b = inbprt[0]; + + /* Assemble new input */ + int8_T input = 0b0000; + if(a) { + input |= 0b1000; + } + if(ap) { + input |= 0b0100; + } + if(b) { + input |= 0b0010; + } + if(bp) { + input |= 0b0001; + } + ssPrintf("Input computed: %d %d %d %d = %d\n", a, ap, b, bp, input); + + /* Check input out of bounds */ + if((input < 0) || (input >= 16)) { + ssPrintf("Out of bounds inputs in decoder: %d\n", input); + input = 0; + err = true; + } + + /* Check decoder valid entry */ + int8_T value = DEC_TABLE[input]; + if(value == -2) { + ssPrintf("Invalid inputs in decoder: %d %d %d %d\n", a, ap, b, bp); + err = true; + } + + /* Output accumulated value */ + acc += value; + *outprt = (real_T) acc; + ssPrintf("New accumulated value was computed: %6.2f.\n", acc); + + /* Set or clear error flag */ + *errprt = err; + + /* Save values */ + ap = a; + bp = b; +} + + +/* Function: mdlTerminate ===================================================== + * Abstract: + * No termination needed, but we are required to have this routine. + */ +static void mdlTerminate(SimStruct *S) +{ +} + + + +#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 + diff --git a/Motor controller/motor_controller.slx b/Motor controller/motor_controller.slx index 1857bc0..9bca76a 100644 Binary files a/Motor controller/motor_controller.slx and b/Motor controller/motor_controller.slx differ