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 (file)
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 <carlos@jenkins.co.cr>
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ * 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 1857bc0ccc59c9bb5e8e2e5a7753e4a24aa6c2c1..9bca76a0474d10c33ab2c70bd4214922368f84b7 100644 (file)
Binary files a/Motor controller/motor_controller.slx and b/Motor controller/motor_controller.slx differ