/*
 * enkodery_new2.c
 *
 * Code generation for model "enkodery_new2".
 *
 * Model version              : 1.287
 * Simulink Coder version : 8.9 (R2015b) 13-Aug-2015
 * C source code generated on : Sat May 19 12:44:38 2018
 *
 * Target selection: sldrt.tlc
 * Note: GRT includes extra infrastructure and instrumentation for prototyping
 * Embedded hardware selection: Intel->x86-64
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "enkodery_new2.h"
#include "enkodery_new2_private.h"
#include "enkodery_new2_dt.h"

/* options for Simulink Desktop Real-Time board 0 */
static double RTWinBoardOptions0[] = {
  9.0,
};

/* list of Simulink Desktop Real-Time timers */
const int RTWinTimerCount = 1;
const double RTWinTimers[2] = {
  0.001, 0.0,
};

/* list of Simulink Desktop Real-Time boards */
const int RTWinBoardCount = 1;
RTWINBOARD RTWinBoards[1] = {
  { "Humusoft/MF624", 4294967295U, 1, RTWinBoardOptions0 },
};

/* Block signals (auto storage) */
B_enkodery_new2_T enkodery_new2_B;

/* Continuous states */
X_enkodery_new2_T enkodery_new2_X;

/* Block states (auto storage) */
DW_enkodery_new2_T enkodery_new2_DW;

/* Real-time model */
RT_MODEL_enkodery_new2_T enkodery_new2_M_;
RT_MODEL_enkodery_new2_T *const enkodery_new2_M = &enkodery_new2_M_;

/*
 * This function updates continuous states using the ODE3 fixed-step
 * solver algorithm
 */
static void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
  /* Solver Matrices */
  static const real_T rt_ODE3_A[3] = {
    1.0/2.0, 3.0/4.0, 1.0
  };

  static const real_T rt_ODE3_B[3][3] = {
    { 1.0/2.0, 0.0, 0.0 },

    { 0.0, 3.0/4.0, 0.0 },

    { 2.0/9.0, 1.0/3.0, 4.0/9.0 }
  };

  time_T t = rtsiGetT(si);
  time_T tnew = rtsiGetSolverStopTime(si);
  time_T h = rtsiGetStepSize(si);
  real_T *x = rtsiGetContStates(si);
  ODE3_IntgData *id = (ODE3_IntgData *)rtsiGetSolverData(si);
  real_T *y = id->y;
  real_T *f0 = id->f[0];
  real_T *f1 = id->f[1];
  real_T *f2 = id->f[2];
  real_T hB[3];
  int_T i;
  int_T nXc = 2;
  rtsiSetSimTimeStep(si,MINOR_TIME_STEP);

  /* Save the state values at time t in y, we'll use x as ynew. */
  (void) memcpy(y, x,
                (uint_T)nXc*sizeof(real_T));

  /* Assumes that rtsiSetT and ModelOutputs are up-to-date */
  /* f0 = f(t,y) */
  rtsiSetdX(si, f0);
  enkodery_new2_derivatives();

  /* f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*)); */
  hB[0] = h * rt_ODE3_B[0][0];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0]);
  }

  rtsiSetT(si, t + h*rt_ODE3_A[0]);
  rtsiSetdX(si, f1);
  enkodery_new2_output();
  enkodery_new2_derivatives();

  /* f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*)); */
  for (i = 0; i <= 1; i++) {
    hB[i] = h * rt_ODE3_B[1][i];
  }

  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
  }

  rtsiSetT(si, t + h*rt_ODE3_A[1]);
  rtsiSetdX(si, f2);
  enkodery_new2_output();
  enkodery_new2_derivatives();

  /* tnew = t + hA(3);
     ynew = y + f*hB(:,3); */
  for (i = 0; i <= 2; i++) {
    hB[i] = h * rt_ODE3_B[2][i];
  }

  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
  }

  rtsiSetT(si, tnew);
  rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}

/* Model output function */
void enkodery_new2_output(void)
{
  /* local block i/o variables */
  real_T rtb_Motor;
  real_T rtb_Kyvadlo1;
  real_T rtb_Subtract;
  real_T rtb_Diff;
  real_T rtb_Diff_f;
  boolean_T rtb_DigitalInput;
  boolean_T rtb_DigitalInput1;
  int32_T j;
  boolean_T exitg1;
  real_T rtb_Gain11;
  real_T rtb_xk;
  real_T rtb_ManualSwitch1;
  real_T rtb_u_exp2;
  if (rtmIsMajorTimeStep(enkodery_new2_M)) {
    /* set solver stop time */
    if (!(enkodery_new2_M->Timing.clockTick0+1)) {
      rtsiSetSolverStopTime(&enkodery_new2_M->solverInfo,
                            ((enkodery_new2_M->Timing.clockTickH0 + 1) *
        enkodery_new2_M->Timing.stepSize0 * 4294967296.0));
    } else {
      rtsiSetSolverStopTime(&enkodery_new2_M->solverInfo,
                            ((enkodery_new2_M->Timing.clockTick0 + 1) *
        enkodery_new2_M->Timing.stepSize0 + enkodery_new2_M->Timing.clockTickH0 *
        enkodery_new2_M->Timing.stepSize0 * 4294967296.0));
    }
  }                                    /* end MajorTimeStep */

  /* Update absolute time of base rate at minor time step */
  if (rtmIsMinorTimeStep(enkodery_new2_M)) {
    enkodery_new2_M->Timing.t[0] = rtsiGetT(&enkodery_new2_M->solverInfo);
  }

  if (rtmIsMajorTimeStep(enkodery_new2_M)) {
    /* S-Function Block: <Root>/Analog Input1 */
    {
      ANALOGIOPARM parm;
      parm.mode = (RANGEMODE) enkodery_new2_P.AnalogInput1_RangeMode;
      parm.rangeidx = enkodery_new2_P.AnalogInput1_VoltRange;
      RTBIO_DriverIO(0, ANALOGINPUT, IOREAD, 1,
                     &enkodery_new2_P.AnalogInput1_Channels, &rtb_Subtract,
                     &parm);
    }

    /* S-Function Block: <Root>/Motor */
    {
      ENCODERINPARM parm;
      parm.quad = (QUADMODE) 2;
      parm.index = (INDEXPULSE) 0;
      parm.infilter = enkodery_new2_P.Motor_InputFilter;
      RTBIO_DriverIO(0, ENCODERINPUT, IOREAD, 1, &enkodery_new2_P.Motor_Channels,
                     &rtb_Motor, &parm);
    }

    /* S-Function Block: <Root>/Kyvadlo1 */
    {
      ENCODERINPARM parm;
      parm.quad = (QUADMODE) 2;
      parm.index = (INDEXPULSE) 0;
      parm.infilter = enkodery_new2_P.Kyvadlo1_InputFilter;
      RTBIO_DriverIO(0, ENCODERINPUT, IOREAD, 1,
                     &enkodery_new2_P.Kyvadlo1_Channels, &rtb_Kyvadlo1, &parm);
    }

    /* S-Function Block: <Root>/Kyvadlo2 */
    {
      ENCODERINPARM parm;
      parm.quad = (QUADMODE) 2;
      parm.index = (INDEXPULSE) 0;
      parm.infilter = enkodery_new2_P.Kyvadlo2_InputFilter;
      RTBIO_DriverIO(0, ENCODERINPUT, IOREAD, 1,
                     &enkodery_new2_P.Kyvadlo2_Channels, &rtb_Diff, &parm);
    }

    /* S-Function Block: <Root>/Analog Input */
    {
      ANALOGIOPARM parm;
      parm.mode = (RANGEMODE) enkodery_new2_P.AnalogInput_RangeMode;
      parm.rangeidx = enkodery_new2_P.AnalogInput_VoltRange;
      RTBIO_DriverIO(0, ANALOGINPUT, IOREAD, 1,
                     &enkodery_new2_P.AnalogInput_Channels, &rtb_Diff_f, &parm);
    }

    /* S-Function Block: <Root>/Digital Input */
    {
      double inval[1];
      double* invalp = inval;
      RTBIO_DriverIO(0, DIGITALINPUT, IOREAD, 1,
                     &enkodery_new2_P.DigitalInput_Channels, inval,
                     &enkodery_new2_P.DigitalInput_BitMode);
      rtb_DigitalInput = (boolean_T) *invalp++;
    }

    /* S-Function Block: <Root>/Digital Input1 */
    {
      double inval[1];
      double* invalp = inval;
      RTBIO_DriverIO(0, DIGITALINPUT, IOREAD, 1,
                     &enkodery_new2_P.DigitalInput1_Channels, inval,
                     &enkodery_new2_P.DigitalInput1_BitMode);
      rtb_DigitalInput1 = (boolean_T) *invalp++;
    }

    /* Fcn: '<Root>/Fcn1' */
    enkodery_new2_B.Fcn1 = rtb_Subtract / 1.77;

    /* Gain: '<Root>/Gain11' */
    rtb_Gain11 = enkodery_new2_P.ckfi / enkodery_new2_P.dd *
      enkodery_new2_B.Fcn1;

    /* UnitDelay: '<S25>/Delay_x' */
    rtb_xk = enkodery_new2_DW.Delay_x_DSTATE;

    /* ManualSwitch: '<Root>/Manual Switch3' incorporates:
     *  Gain: '<S25>/C'
     *  Gain: '<S25>/D'
     *  Sum: '<S25>/sum1'
     *  UnitDelay: '<S25>/Delay_x'
     */
    if (enkodery_new2_P.ManualSwitch3_CurrentSetting == 1) {
      enkodery_new2_B.ManualSwitch3 = enkodery_new2_P.D_Gain * rtb_Gain11 +
        enkodery_new2_P.C_Gain * enkodery_new2_DW.Delay_x_DSTATE;
    } else {
      enkodery_new2_B.ManualSwitch3 = rtb_Gain11;
    }

    /* End of ManualSwitch: '<Root>/Manual Switch3' */

    /* Constant: '<Root>/Constant2' */
    enkodery_new2_B.Constant2 = enkodery_new2_P.Constant2_Value;

    /* Sum: '<Root>/Subtract' */
    rtb_Subtract = enkodery_new2_B.Constant2 - enkodery_new2_B.ManualSwitch3;

    /* Gain: '<S18>/Proportional Gain' */
    enkodery_new2_B.ProportionalGain = enkodery_new2_P.PIDController_P *
      rtb_Subtract;

    /* Gain: '<S18>/Derivative Gain' */
    enkodery_new2_B.DerivativeGain = enkodery_new2_P.PIDController_D *
      rtb_Subtract;
  }

  /* Gain: '<S18>/Filter Coefficient' incorporates:
   *  Integrator: '<S18>/Filter'
   *  Sum: '<S18>/SumD'
   */
  enkodery_new2_B.FilterCoefficient = (enkodery_new2_B.DerivativeGain -
    enkodery_new2_X.Filter_CSTATE) * enkodery_new2_P.PIDController_N;

  /* ManualSwitch: '<Root>/Manual Switch1' incorporates:
   *  Constant: '<Root>/Constant1'
   */
  if (enkodery_new2_P.ManualSwitch1_CurrentSetting == 1) {
    /* Sum: '<S18>/Sum' incorporates:
     *  Integrator: '<S18>/Integrator'
     */
    rtb_ManualSwitch1 = (enkodery_new2_B.ProportionalGain +
                         enkodery_new2_X.Integrator_CSTATE) +
      enkodery_new2_B.FilterCoefficient;

    /* Saturate: '<Root>/Saturation1' */
    if (rtb_ManualSwitch1 > enkodery_new2_P.Saturation1_UpperSat) {
      rtb_ManualSwitch1 = enkodery_new2_P.Saturation1_UpperSat;
    } else {
      if (rtb_ManualSwitch1 < enkodery_new2_P.Saturation1_LowerSat) {
        rtb_ManualSwitch1 = enkodery_new2_P.Saturation1_LowerSat;
      }
    }

    /* End of Saturate: '<Root>/Saturation1' */
  } else {
    rtb_ManualSwitch1 = enkodery_new2_P.Constant1_Value;
  }

  /* End of ManualSwitch: '<Root>/Manual Switch1' */
  if (rtmIsMajorTimeStep(enkodery_new2_M)) {
    /* Gain: '<Root>/Poloha' */
    enkodery_new2_B.Poloha = enkodery_new2_P.Poloha_Gain * rtb_Motor;

    /* RelationalOperator: '<S1>/Compare' incorporates:
     *  Abs: '<Root>/Abs1'
     *  Constant: '<S1>/Constant'
     */
    enkodery_new2_B.Compare = (fabs(enkodery_new2_B.Poloha) >
      enkodery_new2_P.CompareToConstant_const);
  }

  /* ManualSwitch: '<Root>/Manual Switch' incorporates:
   *  Switch: '<Root>/Switch'
   */
  if (enkodery_new2_P.ManualSwitch_CurrentSetting == 1) {
    enkodery_new2_B.ManualSwitch = rtb_ManualSwitch1;
  } else if (enkodery_new2_B.Compare) {
    /* Switch: '<Root>/Switch' */
    enkodery_new2_B.ManualSwitch = 0.0;
  } else {
    enkodery_new2_B.ManualSwitch = rtb_ManualSwitch1;
  }

  /* End of ManualSwitch: '<Root>/Manual Switch' */

  /* Saturate: '<Root>/Saturation2' */
  if (enkodery_new2_B.ManualSwitch > enkodery_new2_P.Saturation2_UpperSat) {
    rtb_ManualSwitch1 = enkodery_new2_P.Saturation2_UpperSat;
  } else if (enkodery_new2_B.ManualSwitch < enkodery_new2_P.Saturation2_LowerSat)
  {
    rtb_ManualSwitch1 = enkodery_new2_P.Saturation2_LowerSat;
  } else {
    rtb_ManualSwitch1 = enkodery_new2_B.ManualSwitch;
  }

  /* End of Saturate: '<Root>/Saturation2' */

  /* MATLAB Function: '<Root>/MATLAB Function4' incorporates:
   *  Constant: '<Root>/Constant'
   *  Constant: '<Root>/Constant10'
   */
  /* MATLAB Function 'MATLAB Function4': '<S17>:1' */
  /* '<S17>:1:3' */
  rtb_u_exp2 = 0.0;
  if ((rtb_ManualSwitch1 < 0.0) && (rtb_ManualSwitch1 > -0.0)) {
    /* '<S17>:1:5' */
    /* '<S17>:1:6' */
    rtb_u_exp2 = rtb_ManualSwitch1;
  } else {
    /* '<S17>:1:8' */
    j = 0;
    exitg1 = false;
    while ((!exitg1) && (j < 100)) {
      /* '<S17>:1:8' */
      if (enkodery_new2_P.h_norm[j] < -fabs(rtb_ManualSwitch1)) {
        /* '<S17>:1:9' */
        /* '<S17>:1:10' */
        if (rtb_ManualSwitch1 < 0.0) {
          rtb_u_exp2 = -1.0;
        } else if (rtb_ManualSwitch1 > 0.0) {
          rtb_u_exp2 = 1.0;
        } else if (rtb_ManualSwitch1 == 0.0) {
          rtb_u_exp2 = 0.0;
        } else {
          rtb_u_exp2 = rtb_ManualSwitch1;
        }

        rtb_u_exp2 = -(enkodery_new2_P.v_inv[j] * rtb_u_exp2);
        exitg1 = true;
      } else {
        /* '<S17>:1:8' */
        j++;
      }
    }
  }

  /* End of MATLAB Function: '<Root>/MATLAB Function4' */

  /* ManualSwitch: '<Root>/Manual Switch2' */
  if (enkodery_new2_P.ManualSwitch2_CurrentSetting == 1) {
    enkodery_new2_B.ManualSwitch2 = rtb_ManualSwitch1;
  } else {
    enkodery_new2_B.ManualSwitch2 = rtb_u_exp2;
  }

  /* End of ManualSwitch: '<Root>/Manual Switch2' */

  /* RelationalOperator: '<S3>/Compare' incorporates:
   *  Constant: '<S3>/Constant'
   */
  enkodery_new2_B.Compare_l = (enkodery_new2_B.ManualSwitch2 !=
    enkodery_new2_P.Constant_Value);

  /* RelationalOperator: '<S2>/Compare' incorporates:
   *  Constant: '<S2>/Constant'
   */
  enkodery_new2_B.Compare_n = (enkodery_new2_B.ManualSwitch2 <=
    enkodery_new2_P.Constant_Value_o);

  /* Abs: '<Root>/Abs' */
  enkodery_new2_B.Abs = fabs(enkodery_new2_B.ManualSwitch2);
  if (rtmIsMajorTimeStep(enkodery_new2_M)) {
    /* S-Function Block: <Root>/BRAKE */
    {
      double doval[1];
      double* dovalp = doval;
      *dovalp++ = (double) enkodery_new2_B.Compare_l;
      RTBIO_DriverIO(0, DIGITALOUTPUT, IOWRITE, 1,
                     &enkodery_new2_P.BRAKE_Channels, doval,
                     &enkodery_new2_P.BRAKE_BitMode);
    }

    /* S-Function Block: <Root>/DIR */
    {
      double doval[1];
      double* dovalp = doval;
      *dovalp++ = (double) enkodery_new2_B.Compare_n;
      RTBIO_DriverIO(0, DIGITALOUTPUT, IOWRITE, 1, &enkodery_new2_P.DIR_Channels,
                     doval, &enkodery_new2_P.DIR_BitMode);
    }

    /* S-Function Block: <Root>/PWM */
    {
      {
        RTBIO_DriverIO(0, FREQUENCYOUTPUT, IOWRITE, 1,
                       &enkodery_new2_P.PWM_Channels,
                       &enkodery_new2_P.PWM_Frequency, &enkodery_new2_B.Abs);
      }
    }

    /* Gain: '<Root>/Úhel vychýlení kyvadla' */
    enkodery_new2_B.helvychlenkyvadla = enkodery_new2_P.helvychlenkyvadla_Gain *
      rtb_Kyvadlo1;

    /* SampleTimeMath: '<S6>/TSamp'
     *
     * About '<S6>/TSamp':
     *  y = u * K where K = 1 / ( w * Ts )
     */
    enkodery_new2_B.TSamp = enkodery_new2_B.helvychlenkyvadla *
      enkodery_new2_P.TSamp_WtEt;

    /* Sum: '<S6>/Diff' incorporates:
     *  UnitDelay: '<S6>/UD'
     */
    rtb_ManualSwitch1 = enkodery_new2_B.TSamp - enkodery_new2_DW.UD_DSTATE;

    /* Sum: '<S22>/sum1' incorporates:
     *  Gain: '<S22>/C'
     *  Gain: '<S22>/D'
     *  UnitDelay: '<S22>/Delay_x'
     */
    enkodery_new2_B.yk = enkodery_new2_P.D_Gain_m * rtb_ManualSwitch1 +
      enkodery_new2_P.C_Gain_l * enkodery_new2_DW.Delay_x_DSTATE_k;

    /* Gain: '<Root>/Úhel vychýlení kyvadla1' */
    enkodery_new2_B.helvychlenkyvadla1 = enkodery_new2_P.helvychlenkyvadla1_Gain
      * rtb_Diff;

    /* SampleTimeMath: '<S8>/TSamp'
     *
     * About '<S8>/TSamp':
     *  y = u * K where K = 1 / ( w * Ts )
     */
    enkodery_new2_B.TSamp_k = enkodery_new2_B.helvychlenkyvadla1 *
      enkodery_new2_P.TSamp_WtEt_i;

    /* Sum: '<S8>/Diff' incorporates:
     *  UnitDelay: '<S8>/UD'
     */
    rtb_Diff = enkodery_new2_B.TSamp_k - enkodery_new2_DW.UD_DSTATE_i;

    /* Sum: '<S23>/sum1' incorporates:
     *  Gain: '<S23>/C'
     *  Gain: '<S23>/D'
     *  UnitDelay: '<S23>/Delay_x'
     */
    enkodery_new2_B.yk_c = enkodery_new2_P.D_Gain_ml * rtb_Diff +
      enkodery_new2_P.C_Gain_j * enkodery_new2_DW.Delay_x_DSTATE_o;

    /* Fcn: '<Root>/Fcn2' */
    enkodery_new2_B.Fcn2 = (rtb_Diff_f - 0.0682) / 1.77;

    /* SampleTimeMath: '<S7>/TSamp'
     *
     * About '<S7>/TSamp':
     *  y = u * K where K = 1 / ( w * Ts )
     */
    enkodery_new2_B.TSamp_i = enkodery_new2_B.Poloha *
      enkodery_new2_P.TSamp_WtEt_j;

    /* Sum: '<S7>/Diff' incorporates:
     *  UnitDelay: '<S7>/UD'
     */
    rtb_Diff_f = enkodery_new2_B.TSamp_i - enkodery_new2_DW.UD_DSTATE_g;

    /* Sum: '<S21>/sum1' incorporates:
     *  Gain: '<S21>/C'
     *  Gain: '<S21>/D'
     *  UnitDelay: '<S21>/Delay_x'
     */
    enkodery_new2_B.yk_e = enkodery_new2_P.D_Gain_o * rtb_Diff_f +
      enkodery_new2_P.C_Gain_a * enkodery_new2_DW.Delay_x_DSTATE_e;

    /* Gain: '<Root>/Gain1' */
    enkodery_new2_B.Gain1 = enkodery_new2_P.Gain1_Gain * rtb_Motor;

    /* SampleTimeMath: '<S9>/TSamp'
     *
     * About '<S9>/TSamp':
     *  y = u * K where K = 1 / ( w * Ts )
     */
    enkodery_new2_B.TSamp_ii = enkodery_new2_B.Gain1 *
      enkodery_new2_P.TSamp_WtEt_it;

    /* Sum: '<S9>/Diff' incorporates:
     *  UnitDelay: '<S9>/UD'
     */
    enkodery_new2_B.Diff = enkodery_new2_B.TSamp_ii -
      enkodery_new2_DW.UD_DSTATE_m;

    /* Gain: '<Root>/Poloha1' */
    enkodery_new2_B.Poloha1 = enkodery_new2_P.Poloha1_Gain * rtb_Motor;

    /* SampleTimeMath: '<S10>/TSamp'
     *
     * About '<S10>/TSamp':
     *  y = u * K where K = 1 / ( w * Ts )
     */
    enkodery_new2_B.TSamp_j = enkodery_new2_B.Poloha1 *
      enkodery_new2_P.TSamp_WtEt_p;

    /* Sum: '<S10>/Diff' incorporates:
     *  UnitDelay: '<S10>/UD'
     */
    enkodery_new2_B.Diff_i = enkodery_new2_B.TSamp_j -
      enkodery_new2_DW.UD_DSTATE_gm;

    /* Sum: '<S24>/sum1' incorporates:
     *  Gain: '<S24>/C'
     *  Gain: '<S24>/D'
     *  UnitDelay: '<S24>/Delay_x'
     */
    enkodery_new2_B.yk_k = enkodery_new2_P.D_Gain_h * enkodery_new2_B.Diff_i +
      enkodery_new2_P.C_Gain_d * enkodery_new2_DW.Delay_x_DSTATE_p;

    /* Sum: '<S20>/sum1' incorporates:
     *  Gain: '<S20>/C'
     *  Gain: '<S20>/D'
     *  UnitDelay: '<S20>/Delay_x'
     */
    enkodery_new2_B.yk_d = enkodery_new2_P.D_Gain_l * enkodery_new2_B.Diff +
      enkodery_new2_P.C_Gain_k * enkodery_new2_DW.Delay_x_DSTATE_n;

    /* Gain: '<Root>/Gain2' */
    enkodery_new2_B.Gain2 = enkodery_new2_P.Gain2_Gain * enkodery_new2_B.Gain1;

    /* Sum: '<S20>/A*x(k) + B*u(k)' incorporates:
     *  Gain: '<S20>/A'
     *  Gain: '<S20>/B'
     *  UnitDelay: '<S20>/Delay_x'
     */
    enkodery_new2_B.xk1 = enkodery_new2_P.A_Gain *
      enkodery_new2_DW.Delay_x_DSTATE_n + enkodery_new2_P.B_Gain *
      enkodery_new2_B.Diff;

    /* Sum: '<S21>/A*x(k) + B*u(k)' incorporates:
     *  Gain: '<S21>/A'
     *  Gain: '<S21>/B'
     *  UnitDelay: '<S21>/Delay_x'
     */
    enkodery_new2_B.xk1_b = enkodery_new2_P.A_Gain_c *
      enkodery_new2_DW.Delay_x_DSTATE_e + enkodery_new2_P.B_Gain_j * rtb_Diff_f;

    /* Sum: '<S22>/A*x(k) + B*u(k)' incorporates:
     *  Gain: '<S22>/A'
     *  Gain: '<S22>/B'
     *  UnitDelay: '<S22>/Delay_x'
     */
    enkodery_new2_B.xk1_n = enkodery_new2_P.A_Gain_f *
      enkodery_new2_DW.Delay_x_DSTATE_k + enkodery_new2_P.B_Gain_a *
      rtb_ManualSwitch1;

    /* Sum: '<S23>/A*x(k) + B*u(k)' incorporates:
     *  Gain: '<S23>/A'
     *  Gain: '<S23>/B'
     *  UnitDelay: '<S23>/Delay_x'
     */
    enkodery_new2_B.xk1_f = enkodery_new2_P.A_Gain_n *
      enkodery_new2_DW.Delay_x_DSTATE_o + enkodery_new2_P.B_Gain_i * rtb_Diff;

    /* Sum: '<S24>/A*x(k) + B*u(k)' incorporates:
     *  Gain: '<S24>/A'
     *  Gain: '<S24>/B'
     *  UnitDelay: '<S24>/Delay_x'
     */
    enkodery_new2_B.xk1_m = enkodery_new2_P.A_Gain_a *
      enkodery_new2_DW.Delay_x_DSTATE_p + enkodery_new2_P.B_Gain_n *
      enkodery_new2_B.Diff_i;

    /* Sum: '<S25>/A*x(k) + B*u(k)' incorporates:
     *  Gain: '<S25>/A'
     *  Gain: '<S25>/B'
     */
    enkodery_new2_B.xk1_l = enkodery_new2_P.A_Gain_ar * rtb_xk +
      enkodery_new2_P.B_Gain_o * rtb_Gain11;

    /* Gain: '<S18>/Integral Gain' */
    enkodery_new2_B.IntegralGain = enkodery_new2_P.PIDController_I *
      rtb_Subtract;

    /* RelationalOperator: '<S4>/Compare' incorporates:
     *  Constant: '<S4>/Constant'
     */
    enkodery_new2_B.Compare_k = (rtb_DigitalInput ==
      enkodery_new2_P.Constant_Value_e);

    /* RelationalOperator: '<S5>/Compare' incorporates:
     *  Constant: '<S5>/Constant'
     */
    enkodery_new2_B.Compare_a = (rtb_DigitalInput1 ==
      enkodery_new2_P.Constant_Value_c);
  }
}

/* Model update function */
void enkodery_new2_update(void)
{
  if (rtmIsMajorTimeStep(enkodery_new2_M)) {
    /* Update for UnitDelay: '<S25>/Delay_x' */
    enkodery_new2_DW.Delay_x_DSTATE = enkodery_new2_B.xk1_l;

    /* Update for UnitDelay: '<S6>/UD' */
    enkodery_new2_DW.UD_DSTATE = enkodery_new2_B.TSamp;

    /* Update for UnitDelay: '<S22>/Delay_x' */
    enkodery_new2_DW.Delay_x_DSTATE_k = enkodery_new2_B.xk1_n;

    /* Update for UnitDelay: '<S8>/UD' */
    enkodery_new2_DW.UD_DSTATE_i = enkodery_new2_B.TSamp_k;

    /* Update for UnitDelay: '<S23>/Delay_x' */
    enkodery_new2_DW.Delay_x_DSTATE_o = enkodery_new2_B.xk1_f;

    /* Update for UnitDelay: '<S7>/UD' */
    enkodery_new2_DW.UD_DSTATE_g = enkodery_new2_B.TSamp_i;

    /* Update for UnitDelay: '<S21>/Delay_x' */
    enkodery_new2_DW.Delay_x_DSTATE_e = enkodery_new2_B.xk1_b;

    /* Update for UnitDelay: '<S9>/UD' */
    enkodery_new2_DW.UD_DSTATE_m = enkodery_new2_B.TSamp_ii;

    /* Update for UnitDelay: '<S10>/UD' */
    enkodery_new2_DW.UD_DSTATE_gm = enkodery_new2_B.TSamp_j;

    /* Update for UnitDelay: '<S24>/Delay_x' */
    enkodery_new2_DW.Delay_x_DSTATE_p = enkodery_new2_B.xk1_m;

    /* Update for UnitDelay: '<S20>/Delay_x' */
    enkodery_new2_DW.Delay_x_DSTATE_n = enkodery_new2_B.xk1;
  }

  if (rtmIsMajorTimeStep(enkodery_new2_M)) {
    rt_ertODEUpdateContinuousStates(&enkodery_new2_M->solverInfo);
  }

  /* Update absolute time for base rate */
  /* The "clockTick0" counts the number of times the code of this task has
   * been executed. The absolute time is the multiplication of "clockTick0"
   * and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
   * overflow during the application lifespan selected.
   * Timer of this task consists of two 32 bit unsigned integers.
   * The two integers represent the low bits Timing.clockTick0 and the high bits
   * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
   */
  if (!(++enkodery_new2_M->Timing.clockTick0)) {
    ++enkodery_new2_M->Timing.clockTickH0;
  }

  enkodery_new2_M->Timing.t[0] = rtsiGetSolverStopTime
    (&enkodery_new2_M->solverInfo);

  {
    /* Update absolute timer for sample time: [0.001s, 0.0s] */
    /* The "clockTick1" counts the number of times the code of this task has
     * been executed. The absolute time is the multiplication of "clockTick1"
     * and "Timing.stepSize1". Size of "clockTick1" ensures timer will not
     * overflow during the application lifespan selected.
     * Timer of this task consists of two 32 bit unsigned integers.
     * The two integers represent the low bits Timing.clockTick1 and the high bits
     * Timing.clockTickH1. When the low bit overflows to 0, the high bits increment.
     */
    if (!(++enkodery_new2_M->Timing.clockTick1)) {
      ++enkodery_new2_M->Timing.clockTickH1;
    }

    enkodery_new2_M->Timing.t[1] = enkodery_new2_M->Timing.clockTick1 *
      enkodery_new2_M->Timing.stepSize1 + enkodery_new2_M->Timing.clockTickH1 *
      enkodery_new2_M->Timing.stepSize1 * 4294967296.0;
  }
}

/* Derivatives for root system: '<Root>' */
void enkodery_new2_derivatives(void)
{
  XDot_enkodery_new2_T *_rtXdot;
  _rtXdot = ((XDot_enkodery_new2_T *) enkodery_new2_M->ModelData.derivs);

  /* Derivatives for Integrator: '<S18>/Integrator' */
  _rtXdot->Integrator_CSTATE = enkodery_new2_B.IntegralGain;

  /* Derivatives for Integrator: '<S18>/Filter' */
  _rtXdot->Filter_CSTATE = enkodery_new2_B.FilterCoefficient;
}

/* Model initialize function */
void enkodery_new2_initialize(void)
{
  /* S-Function Block: <Root>/BRAKE */
  {
    RTBIO_DriverIO(0, DIGITALOUTPUT, IOWRITE, 1, &enkodery_new2_P.BRAKE_Channels,
                   &enkodery_new2_P.BRAKE_InitialValue,
                   &enkodery_new2_P.BRAKE_BitMode);
  }

  /* S-Function Block: <Root>/DIR */
  {
    RTBIO_DriverIO(0, DIGITALOUTPUT, IOWRITE, 1, &enkodery_new2_P.DIR_Channels,
                   &enkodery_new2_P.DIR_InitialValue,
                   &enkodery_new2_P.DIR_BitMode);
  }

  /* S-Function Block: <Root>/PWM */
  {
    {
      RTBIO_DriverIO(0, FREQUENCYOUTPUT, IOWRITE, 1,
                     &enkodery_new2_P.PWM_Channels,
                     &enkodery_new2_P.PWM_Frequency, &enkodery_new2_P.PWM_Duty);
    }
  }

  /* S-Function Block: <Root>/Motor */
  {
    ENCODERINPARM parm;
    parm.quad = (QUADMODE) 2;
    parm.index = (INDEXPULSE) 0;
    parm.infilter = enkodery_new2_P.Motor_InputFilter;
    RTBIO_DriverIO(0, ENCODERINPUT, IORESET, 1, &enkodery_new2_P.Motor_Channels,
                   NULL, &parm);
  }

  /* S-Function Block: <Root>/Kyvadlo1 */
  {
    ENCODERINPARM parm;
    parm.quad = (QUADMODE) 2;
    parm.index = (INDEXPULSE) 0;
    parm.infilter = enkodery_new2_P.Kyvadlo1_InputFilter;
    RTBIO_DriverIO(0, ENCODERINPUT, IORESET, 1,
                   &enkodery_new2_P.Kyvadlo1_Channels, NULL, &parm);
  }

  /* S-Function Block: <Root>/Kyvadlo2 */
  {
    ENCODERINPARM parm;
    parm.quad = (QUADMODE) 2;
    parm.index = (INDEXPULSE) 0;
    parm.infilter = enkodery_new2_P.Kyvadlo2_InputFilter;
    RTBIO_DriverIO(0, ENCODERINPUT, IORESET, 1,
                   &enkodery_new2_P.Kyvadlo2_Channels, NULL, &parm);
  }

  /* InitializeConditions for UnitDelay: '<S25>/Delay_x' */
  enkodery_new2_DW.Delay_x_DSTATE = enkodery_new2_P.Delay_x_InitialCondition;

  /* InitializeConditions for Integrator: '<S18>/Integrator' */
  enkodery_new2_X.Integrator_CSTATE = enkodery_new2_P.Integrator_IC;

  /* InitializeConditions for Integrator: '<S18>/Filter' */
  enkodery_new2_X.Filter_CSTATE = enkodery_new2_P.Filter_IC;

  /* InitializeConditions for UnitDelay: '<S6>/UD' */
  enkodery_new2_DW.UD_DSTATE =
    enkodery_new2_P.DiscreteDerivative1_ICPrevScaledInput;

  /* InitializeConditions for UnitDelay: '<S22>/Delay_x' */
  enkodery_new2_DW.Delay_x_DSTATE_k = enkodery_new2_P.Delay_x_InitialCondition_l;

  /* InitializeConditions for UnitDelay: '<S8>/UD' */
  enkodery_new2_DW.UD_DSTATE_i =
    enkodery_new2_P.DiscreteDerivative3_ICPrevScaledInput;

  /* InitializeConditions for UnitDelay: '<S23>/Delay_x' */
  enkodery_new2_DW.Delay_x_DSTATE_o = enkodery_new2_P.Delay_x_InitialCondition_j;

  /* InitializeConditions for UnitDelay: '<S7>/UD' */
  enkodery_new2_DW.UD_DSTATE_g =
    enkodery_new2_P.DiscreteDerivative2_ICPrevScaledInput;

  /* InitializeConditions for UnitDelay: '<S21>/Delay_x' */
  enkodery_new2_DW.Delay_x_DSTATE_e = enkodery_new2_P.Delay_x_InitialCondition_i;

  /* InitializeConditions for UnitDelay: '<S9>/UD' */
  enkodery_new2_DW.UD_DSTATE_m =
    enkodery_new2_P.DiscreteDerivative4_ICPrevScaledInput;

  /* InitializeConditions for UnitDelay: '<S10>/UD' */
  enkodery_new2_DW.UD_DSTATE_gm =
    enkodery_new2_P.DiscreteDerivative5_ICPrevScaledInput;

  /* InitializeConditions for UnitDelay: '<S24>/Delay_x' */
  enkodery_new2_DW.Delay_x_DSTATE_p =
    enkodery_new2_P.Delay_x_InitialCondition_ip;

  /* InitializeConditions for UnitDelay: '<S20>/Delay_x' */
  enkodery_new2_DW.Delay_x_DSTATE_n = enkodery_new2_P.Delay_x_InitialCondition_b;
}

/* Model terminate function */
void enkodery_new2_terminate(void)
{
  /* S-Function Block: <Root>/BRAKE */
  {
    RTBIO_DriverIO(0, DIGITALOUTPUT, IOWRITE, 1, &enkodery_new2_P.BRAKE_Channels,
                   &enkodery_new2_P.BRAKE_FinalValue,
                   &enkodery_new2_P.BRAKE_BitMode);
  }

  /* S-Function Block: <Root>/DIR */
  {
    RTBIO_DriverIO(0, DIGITALOUTPUT, IOWRITE, 1, &enkodery_new2_P.DIR_Channels,
                   &enkodery_new2_P.DIR_FinalValue, &enkodery_new2_P.DIR_BitMode);
  }

  /* S-Function Block: <Root>/PWM */
  {
    {
      RTBIO_DriverIO(0, FREQUENCYOUTPUT, IOWRITE, 1,
                     &enkodery_new2_P.PWM_Channels,
                     &enkodery_new2_P.PWM_Frequency,
                     &enkodery_new2_P.PWM_DutyFinalValue);
    }
  }
}

/*========================================================================*
 * Start of Classic call interface                                        *
 *========================================================================*/

/* Solver interface called by GRT_Main */
#ifndef USE_GENERATED_SOLVER

void rt_ODECreateIntegrationData(RTWSolverInfo *si)
{
  UNUSED_PARAMETER(si);
  return;
}                                      /* do nothing */

void rt_ODEDestroyIntegrationData(RTWSolverInfo *si)
{
  UNUSED_PARAMETER(si);
  return;
}                                      /* do nothing */

void rt_ODEUpdateContinuousStates(RTWSolverInfo *si)
{
  UNUSED_PARAMETER(si);
  return;
}                                      /* do nothing */

#endif

void MdlOutputs(int_T tid)
{
  enkodery_new2_output();
  UNUSED_PARAMETER(tid);
}

void MdlUpdate(int_T tid)
{
  enkodery_new2_update();
  UNUSED_PARAMETER(tid);
}

void MdlInitializeSizes(void)
{
}

void MdlInitializeSampleTimes(void)
{
}

void MdlInitialize(void)
{
}

void MdlStart(void)
{
  enkodery_new2_initialize();
}

void MdlTerminate(void)
{
  enkodery_new2_terminate();
}

/* Registration function */
RT_MODEL_enkodery_new2_T *enkodery_new2(void)
{
  /* Registration code */

  /* initialize non-finites */
  rt_InitInfAndNaN(sizeof(real_T));

  /* non-finite (run-time) assignments */
  enkodery_new2_P.Motor_InputFilter = rtInf;

  /* initialize real-time model */
  (void) memset((void *)enkodery_new2_M, 0,
                sizeof(RT_MODEL_enkodery_new2_T));

  {
    /* Setup solver object */
    rtsiSetSimTimeStepPtr(&enkodery_new2_M->solverInfo,
                          &enkodery_new2_M->Timing.simTimeStep);
    rtsiSetTPtr(&enkodery_new2_M->solverInfo, &rtmGetTPtr(enkodery_new2_M));
    rtsiSetStepSizePtr(&enkodery_new2_M->solverInfo,
                       &enkodery_new2_M->Timing.stepSize0);
    rtsiSetdXPtr(&enkodery_new2_M->solverInfo,
                 &enkodery_new2_M->ModelData.derivs);
    rtsiSetContStatesPtr(&enkodery_new2_M->solverInfo, (real_T **)
                         &enkodery_new2_M->ModelData.contStates);
    rtsiSetNumContStatesPtr(&enkodery_new2_M->solverInfo,
      &enkodery_new2_M->Sizes.numContStates);
    rtsiSetNumPeriodicContStatesPtr(&enkodery_new2_M->solverInfo,
      &enkodery_new2_M->Sizes.numPeriodicContStates);
    rtsiSetPeriodicContStateIndicesPtr(&enkodery_new2_M->solverInfo,
      &enkodery_new2_M->ModelData.periodicContStateIndices);
    rtsiSetPeriodicContStateRangesPtr(&enkodery_new2_M->solverInfo,
      &enkodery_new2_M->ModelData.periodicContStateRanges);
    rtsiSetErrorStatusPtr(&enkodery_new2_M->solverInfo, (&rtmGetErrorStatus
      (enkodery_new2_M)));
    rtsiSetRTModelPtr(&enkodery_new2_M->solverInfo, enkodery_new2_M);
  }

  rtsiSetSimTimeStep(&enkodery_new2_M->solverInfo, MAJOR_TIME_STEP);
  enkodery_new2_M->ModelData.intgData.y = enkodery_new2_M->ModelData.odeY;
  enkodery_new2_M->ModelData.intgData.f[0] = enkodery_new2_M->ModelData.odeF[0];
  enkodery_new2_M->ModelData.intgData.f[1] = enkodery_new2_M->ModelData.odeF[1];
  enkodery_new2_M->ModelData.intgData.f[2] = enkodery_new2_M->ModelData.odeF[2];
  enkodery_new2_M->ModelData.contStates = ((real_T *) &enkodery_new2_X);
  rtsiSetSolverData(&enkodery_new2_M->solverInfo, (void *)
                    &enkodery_new2_M->ModelData.intgData);
  rtsiSetSolverName(&enkodery_new2_M->solverInfo,"ode3");

  /* Initialize timing info */
  {
    int_T *mdlTsMap = enkodery_new2_M->Timing.sampleTimeTaskIDArray;
    mdlTsMap[0] = 0;
    mdlTsMap[1] = 1;
    enkodery_new2_M->Timing.sampleTimeTaskIDPtr = (&mdlTsMap[0]);
    enkodery_new2_M->Timing.sampleTimes =
      (&enkodery_new2_M->Timing.sampleTimesArray[0]);
    enkodery_new2_M->Timing.offsetTimes =
      (&enkodery_new2_M->Timing.offsetTimesArray[0]);

    /* task periods */
    enkodery_new2_M->Timing.sampleTimes[0] = (0.0);
    enkodery_new2_M->Timing.sampleTimes[1] = (0.001);

    /* task offsets */
    enkodery_new2_M->Timing.offsetTimes[0] = (0.0);
    enkodery_new2_M->Timing.offsetTimes[1] = (0.0);
  }

  rtmSetTPtr(enkodery_new2_M, &enkodery_new2_M->Timing.tArray[0]);

  {
    int_T *mdlSampleHits = enkodery_new2_M->Timing.sampleHitArray;
    mdlSampleHits[0] = 1;
    mdlSampleHits[1] = 1;
    enkodery_new2_M->Timing.sampleHits = (&mdlSampleHits[0]);
  }

  rtmSetTFinal(enkodery_new2_M, -1);
  enkodery_new2_M->Timing.stepSize0 = 0.001;
  enkodery_new2_M->Timing.stepSize1 = 0.001;

  /* External mode info */
  enkodery_new2_M->Sizes.checksums[0] = (951661141U);
  enkodery_new2_M->Sizes.checksums[1] = (2803132232U);
  enkodery_new2_M->Sizes.checksums[2] = (3988897231U);
  enkodery_new2_M->Sizes.checksums[3] = (221167092U);

  {
    static const sysRanDType rtAlwaysEnabled = SUBSYS_RAN_BC_ENABLE;
    static RTWExtModeInfo rt_ExtModeInfo;
    static const sysRanDType *systemRan[5];
    enkodery_new2_M->extModeInfo = (&rt_ExtModeInfo);
    rteiSetSubSystemActiveVectorAddresses(&rt_ExtModeInfo, systemRan);
    systemRan[0] = &rtAlwaysEnabled;
    systemRan[1] = &rtAlwaysEnabled;
    systemRan[2] = &rtAlwaysEnabled;
    systemRan[3] = &rtAlwaysEnabled;
    systemRan[4] = &rtAlwaysEnabled;
    rteiSetModelMappingInfoPtr(enkodery_new2_M->extModeInfo,
      &enkodery_new2_M->SpecialInfo.mappingInfo);
    rteiSetChecksumsPtr(enkodery_new2_M->extModeInfo,
                        enkodery_new2_M->Sizes.checksums);
    rteiSetTPtr(enkodery_new2_M->extModeInfo, rtmGetTPtr(enkodery_new2_M));
  }

  enkodery_new2_M->solverInfoPtr = (&enkodery_new2_M->solverInfo);
  enkodery_new2_M->Timing.stepSize = (0.001);
  rtsiSetFixedStepSize(&enkodery_new2_M->solverInfo, 0.001);
  rtsiSetSolverMode(&enkodery_new2_M->solverInfo, SOLVER_MODE_SINGLETASKING);

  /* block I/O */
  enkodery_new2_M->ModelData.blockIO = ((void *) &enkodery_new2_B);
  (void) memset(((void *) &enkodery_new2_B), 0,
                sizeof(B_enkodery_new2_T));

  /* parameters */
  enkodery_new2_M->ModelData.defaultParam = ((real_T *)&enkodery_new2_P);

  /* states (continuous) */
  {
    real_T *x = (real_T *) &enkodery_new2_X;
    enkodery_new2_M->ModelData.contStates = (x);
    (void) memset((void *)&enkodery_new2_X, 0,
                  sizeof(X_enkodery_new2_T));
  }

  /* states (dwork) */
  enkodery_new2_M->ModelData.dwork = ((void *) &enkodery_new2_DW);
  (void) memset((void *)&enkodery_new2_DW, 0,
                sizeof(DW_enkodery_new2_T));

  /* data type transition information */
  {
    static DataTypeTransInfo dtInfo;
    (void) memset((char_T *) &dtInfo, 0,
                  sizeof(dtInfo));
    enkodery_new2_M->SpecialInfo.mappingInfo = (&dtInfo);
    dtInfo.numDataTypes = 14;
    dtInfo.dataTypeSizes = &rtDataTypeSizes[0];
    dtInfo.dataTypeNames = &rtDataTypeNames[0];

    /* Block I/O transition table */
    dtInfo.B = &rtBTransTable;

    /* Parameters transition table */
    dtInfo.P = &rtPTransTable;
  }

  /* Initialize Sizes */
  enkodery_new2_M->Sizes.numContStates = (2);/* Number of continuous states */
  enkodery_new2_M->Sizes.numPeriodicContStates = (0);/* Number of periodic continuous states */
  enkodery_new2_M->Sizes.numY = (0);   /* Number of model outputs */
  enkodery_new2_M->Sizes.numU = (0);   /* Number of model inputs */
  enkodery_new2_M->Sizes.sysDirFeedThru = (0);/* The model is not direct feedthrough */
  enkodery_new2_M->Sizes.numSampTimes = (2);/* Number of sample times */
  enkodery_new2_M->Sizes.numBlocks = (147);/* Number of blocks */
  enkodery_new2_M->Sizes.numBlockIO = (40);/* Number of block outputs */
  enkodery_new2_M->Sizes.numBlockPrms = (317);/* Sum of parameter "widths" */
  return enkodery_new2_M;
}

/*========================================================================*
 * End of Classic call interface                                          *
 *========================================================================*/
