/*
 * File: Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct.c
 *
 * Real-Time Workshop code generated for Simulink model Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct.
 *
 * Model version                        : 1.809
 * Real-Time Workshop file version      : 7.3  (R2009a)  15-Jan-2009
 * Real-Time Workshop file generated on : Tue Apr 13 23:03:43 2010
 * TLC version                          : 7.3 (Jan 18 2009)
 * C source code generated on           : Tue Apr 13 23:03:44 2010
 *--------------------------------------------------------------
 *   Embedded Coder for Microchip dsPIC family.                 |
 *   Generate .c and .h files from your Matlab/simulink model   |
 *   and compile the diagram to .hex and .coff file that can be |
 *   downloaded directly into the microcontroller               |
 *                                                              |
 * Licence Accorded to Ing. Robert Grepl, Ph.D.                 |
 *                     Technical University Brno                |
 *                                                              |
 *   Written by Lubin KERHUEL -  http://www.kerhuel.eu          |
 *  - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - |
 *   Version 3.1                               26-Nov-2009      |
 *   For Matlab 7.8             R2009a                          |
 *--------------------------------------------------------------
 */

#include "Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct.h"
#include "Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_private.h"

/* Exported block states */
uint16_T Current_M3;                   /* '<Root>/Uint16: Current M3' */
uint16_T Current_M4;                   /* '<Root>/Uint16: Current M4' */
uint16_T ct_turnsE1;                   /* '<Root>/Uint16: Encoder 1' */
uint16_T ct_turnsE2;                   /* '<Root>/Uint16: Encoder 2' */

/* Block signals (auto storage) */
BlockIO_Vejlupek_CAR4_dsPIC33_A Vejlupek_CAR4_dsPIC33_ACU44_S_B;

/* Block states (auto storage) */
D_Work_Vejlupek_CAR4_dsPIC33_AC Vejlupek_CAR4_dsPIC33_ACU_DWork;

/* Real-time model */
RT_MODEL_Vejlupek_CAR4_dsPIC33_ Vejlupek_CAR4_dsPIC33_ACU44__M_;
RT_MODEL_Vejlupek_CAR4_dsPIC33_ *Vejlupek_CAR4_dsPIC33_ACU44__M =
  &Vejlupek_CAR4_dsPIC33_ACU44__M_;
static void rate_monotonic_scheduler(void);

/* Set which subrates need to run this base step (base rate always runs).
 * This function must be called prior to calling the model step function
 * in order to "remember" which rates need to run this base step.  The
 * buffering of events allows for overlapping preemption.
 */
void Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_SetEventsForThisBaseStep
  (boolean_T *eventFlags)
{
  /* Task runs when its counter is zero, computed via rtmStepTask macro */
  eventFlags[1] = ((boolean_T)rtmStepTask(Vejlupek_CAR4_dsPIC33_ACU44__M, 1));
  eventFlags[2] = ((boolean_T)rtmStepTask(Vejlupek_CAR4_dsPIC33_ACU44__M, 2));
}

/* This function updates active task flag for each subrate
 * and rate transition flags for tasks that exchagne data.
 * The function assumes rate-monotonic multitasking scheduler.
 * The function must be called at model base rate so that
 * the generated code self-manages all its subrates and rate
 * transition flags.
 */
static void rate_monotonic_scheduler(void)
{
  /* To ensure a safe and deterministic data transfer between two rates,
   * data is transferred at the priority of a fast task and the frequency
   * of the slow task.  The following flags indicate when the data transfer
   * happens.  That is, a rate interaction flag is set true when both rates
   * will run, and false otherwise.
   */

  /* tid 1 shares data with slower tid rate: 2 */
  if (Vejlupek_CAR4_dsPIC33_ACU44__M->Timing.TaskCounters.TID[1] == 0) {
    Vejlupek_CAR4_dsPIC33_ACU44__M->Timing.RateInteraction.TID1_2 =
      (Vejlupek_CAR4_dsPIC33_ACU44__M->Timing.TaskCounters.TID[2] == 0);
  }

  /* Compute which subrates run during the next base time step.  Subrates
   * are an integer multiple of the base rate counter.  Therefore, the subtask
   * counter is reset when it reaches its limit (zero means run).
   */
  if (++Vejlupek_CAR4_dsPIC33_ACU44__M->Timing.TaskCounters.TID[1] == 5) {/* Sample time: [0.005s, 0.0s] */
    Vejlupek_CAR4_dsPIC33_ACU44__M->Timing.TaskCounters.TID[1] = 0;
  }

  if (++Vejlupek_CAR4_dsPIC33_ACU44__M->Timing.TaskCounters.TID[2] == 10) {/* Sample time: [0.01s, 0.0s] */
    Vejlupek_CAR4_dsPIC33_ACU44__M->Timing.TaskCounters.TID[2] = 0;
  }
}

/* Model step function for TID0 */
void Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_step0(void) /* Sample time: [0.001s, 0.0s] */
{
  {                                    /* Sample time: [0.001s, 0.0s] */
    rate_monotonic_scheduler();
  }
}

/* Model step function for TID1 */
void Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_step1(void) /* Sample time: [0.005s, 0.0s] */
{
  if (Vejlupek_CAR4_dsPIC33_ACU44__M->Timing.RateInteraction.TID1_2) {
    /* RateTransition: '<Root>/Rate Transition12' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition12 =
      Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition12_Buffer0;

    /* RateTransition: '<Root>/Rate Transition13' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition13 =
      Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition13_Buffer0;

    /* RateTransition: '<Root>/Rate Transition6' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition6 =
      Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition6_Buffer0;

    /* RateTransition: '<Root>/Rate Transition7' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition7 =
      Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition7_Buffer0;

    /* RateTransition: '<Root>/Rate Transition1' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition1 =
      Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition1_Buffer0;

    /* RateTransition: '<Root>/Rate Transition2' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition2 =
      Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition2_Buffer0;

    /* RateTransition: '<Root>/Rate Transition3' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition3 =
      Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition3_Buffer0;
  }

  /* S-Function "dsPIC_UART_TX_Labview_Matlab" Block: <Root>/TX Output Ts = 5e-3 */
  {
    static uint16_T nextToSend = 1;
    if ((nextToSend & 1) != 0) {
      if (!send232Buff_Uart1(Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition12,1
                             + 0U*4) )
        nextToSend = (nextToSend << 1);
    }

    if ((nextToSend & 2) != 0) {
      if (!send232Buff_Uart1(Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition13,1
                             + 1U*4) )
        nextToSend = (nextToSend << 1);
    }

    if ((nextToSend & 4) != 0) {
      if (!send232Buff_Uart1(Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition6,1 +
           2U*4) )
        nextToSend = (nextToSend << 1);
    }

    if ((nextToSend & 8) != 0) {
      if (!send232Buff_Uart1(Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition7,1 +
           3U*4) )
        nextToSend = (nextToSend << 1);
    }

    if ((nextToSend & 16) != 0) {
      if (!send232Buff_Uart1(Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition1,1 +
           4U*4) )
        nextToSend = (nextToSend << 1);
    }

    if ((nextToSend & 32) != 0) {
      if (!send232Buff_Uart1(Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition2,1 +
           5U*4) )
        nextToSend = (nextToSend << 1);
    }

    if (nextToSend >= 64)              /* should be &, but get robust here */
    {
      if (!send232Buff_Uart1(Vejlupek_CAR4_dsPIC33_ACU44_S_B.RateTransition3,0 +
           6U*4) )
        nextToSend = 1;
    }
  }

  /* S-Function "ADC" Block: <S8>/current */
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.current_o1 = ADCBuffChannelDMA[0];
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.current_o2 = ADCBuffChannelDMA[1];

  /* DataStoreWrite: '<Root>/Current M3 Ts = 5e-3' */
  Current_M3 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.current_o2;

  /* DataStoreWrite: '<Root>/Current M4 Ts = 5e-3' */
  Current_M4 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.current_o1;

  /* S-Function "dsPIC_C_function_Call" Block: <S10>/C Function Call [ecanFunctions.c] */
  ecan1_receive_matlab
    (&Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[0]);

  /* Outputs for enable SubSystem: '<Root>/Enabled Subsystem' incorporates:
   *  Constant: '<S5>/Constant'
   *  DataTypeConversion: '<S35>/Extract Desired Bits'
   *  EnablePort: '<S4>/Enable'
   *  Logic: '<Root>/Logical Operator'
   *  RelationalOperator: '<S31>/Compare'
   *  RelationalOperator: '<S5>/Compare'
   */
  if (((Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[0] &
        536870911UL) == 1024UL) &&
      ((Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[0] != 0UL)
       != 0)) {
    if (Vejlupek_CAR4_dsPIC33_ACU_DWork.EnabledSubsystem_MODE == SUBSYS_DISABLED)
    {
      Vejlupek_CAR4_dsPIC33_ACU_DWork.EnabledSubsystem_MODE = SUBSYS_ENABLED;
    }
  } else {
    if (Vejlupek_CAR4_dsPIC33_ACU_DWork.EnabledSubsystem_MODE == SUBSYS_ENABLED)
    {
      Vejlupek_CAR4_dsPIC33_ACU_DWork.EnabledSubsystem_MODE = SUBSYS_DISABLED;
    }
  }

  if (Vejlupek_CAR4_dsPIC33_ACU_DWork.EnabledSubsystem_MODE == SUBSYS_ENABLED) {
    /* DataStoreWrite: '<S4>/Data Store Write' incorporates:
     *  ArithShift: '<S18>/shift'
     *  DataTypeConversion: '<S15>/Data Type Conversion4'
     *  DataTypeConversion: '<S15>/Data Type Conversion5'
     *  DataTypeConversion: '<S15>/Data Type Conversion6'
     *  DataTypeConversion: '<S36>/Extract Desired Bits'
     *  DataTypeConversion: '<S37>/Extract Desired Bits'
     *  S-Function (sfix_bitop): '<S15>/null lower 4 bites2'
     */
    Vejlupek_CAR4_dsPIC33_ACU_DWork.Steering_Rear = (int16_T)((uint16_T)(uint8_T)
      (Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[1] >> 8) << 8
      | (uint16_T)(uint8_T)
      Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[1]);

    /* DataStoreWrite: '<S4>/Data Store Write1' incorporates:
     *  ArithShift: '<S17>/shift'
     *  DataTypeConversion: '<S14>/Data Type Conversion4'
     *  DataTypeConversion: '<S14>/Data Type Conversion5'
     *  DataTypeConversion: '<S14>/Data Type Conversion6'
     *  DataTypeConversion: '<S38>/Extract Desired Bits'
     *  DataTypeConversion: '<S39>/Extract Desired Bits'
     *  S-Function (sfix_bitop): '<S14>/null lower 4 bites2'
     */
    Vejlupek_CAR4_dsPIC33_ACU_DWork.Motor_Momentum = (int16_T)((uint16_T)
      (Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[1] >> 24) <<
      8 | (uint16_T)(uint8_T)
      (Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[1] >> 16));

    /* DataStoreWrite: '<S4>/Data Store Write2' incorporates:
     *  ArithShift: '<S19>/shift'
     *  DataTypeConversion: '<S16>/Data Type Conversion4'
     *  DataTypeConversion: '<S16>/Data Type Conversion5'
     *  DataTypeConversion: '<S40>/Extract Desired Bits'
     *  DataTypeConversion: '<S41>/Extract Desired Bits'
     *  S-Function (sfix_bitop): '<S16>/null lower 4 bites2'
     */
    Vejlupek_CAR4_dsPIC33_ACU_DWork.Buttons = (uint16_T)(uint8_T)
      (Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[2] >> 8) << 8
      | (uint16_T)(uint8_T)
      Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[2];

    /* DataStoreWrite: '<S4>/Data Store Write3' incorporates:
     *  DataTypeConversion: '<S42>/Extract Desired Bits'
     *  DataTypeConversion: '<S4>/Data Type Conversion4'
     */
    Vejlupek_CAR4_dsPIC33_ACU_DWork.DISABLE = ((uint8_T)
      (Vejlupek_CAR4_dsPIC33_ACU44_S_B.CFunctionCallecanFunctionsc[2] >> 16) !=
      0);
  }

  /* end of Outputs for SubSystem: '<Root>/Enabled Subsystem' */

  /* Embedded MATLAB: '<S11>/myMux' incorporates:
   *  ArithShift: '<S27>/shift'
   *  ArithShift: '<S28>/shift'
   *  ArithShift: '<S29>/shift'
   *  ArithShift: '<S30>/shift'
   *  ArithShift: '<S51>/shift'
   *  Constant: '<S11>/identifier'
   *  Constant: '<S9>/data length - set maualy'
   *  DataStoreRead: '<S9>/Current M1 Ts = 5e-3'
   *  DataStoreRead: '<S9>/Current M2 Ts = 5e-3'
   *  DataStoreRead: '<S9>/QEI 1 Ts = 5e-3'
   *  DataStoreRead: '<S9>/QEI 2 Ts = 5e-3'
   *  DataTypeConversion: '<S22>/Data Type Conversion2'
   *  DataTypeConversion: '<S22>/Data Type Conversion3'
   *  DataTypeConversion: '<S23>/Data Type Conversion2'
   *  DataTypeConversion: '<S23>/Data Type Conversion3'
   *  DataTypeConversion: '<S24>/Data Type Conversion2'
   *  DataTypeConversion: '<S24>/Data Type Conversion3'
   *  DataTypeConversion: '<S25>/Data Type Conversion2'
   *  DataTypeConversion: '<S25>/Data Type Conversion3'
   *  DataTypeConversion: '<S49>/Data Type Conversion'
   *  S-Function (sfix_bitop): '<S22>/Get Lower Byte'
   *  S-Function (sfix_bitop): '<S23>/Get Lower Byte'
   *  S-Function (sfix_bitop): '<S24>/Get Lower Byte'
   *  S-Function (sfix_bitop): '<S25>/Get Lower Byte'
   *  S-Function (sfix_bitop): '<S49>/Bitwise Operator'
   *  SignalConversion: '<S47>/TmpHiddenBufferAt SFunction Inport5'
   */
  /* Embedded MATLAB Function 'Send ECAN1 Message/myMux': '<S47>:1' */
  /* '<S47>:1:2' */
  /* '<S47>:1:3' */
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[0] = 2048U;
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[1] = 768U;
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[2] = 768U;
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[3] = 0U;
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[4] = 0U;
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[5] = 0U;
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[6] = 0U;
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[7] = 0U;

  /* '<S47>:1:7' */
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[4] = (uint16_T)(uint8_T)(ct_turnsE1 & 255U) <<
    8 | (uint16_T)(uint8_T)(ct_turnsE1 >> 8);

  /* '<S47>:1:12' */
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[5] = (uint16_T)(uint8_T)(ct_turnsE2 & 255U) <<
    8 | (uint16_T)(uint8_T)(ct_turnsE2 >> 8);

  /* '<S47>:1:17' */
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[6] = (uint16_T)(uint8_T)(Current_M3 & 255U) <<
    8 | (uint16_T)(uint8_T)(Current_M3 >> 8);

  /* '<S47>:1:22' */
  Vejlupek_CAR4_dsPIC33_ACU44_S_B.y[7] = (uint16_T)(uint8_T)(Current_M4 & 255U) <<
    8 | (uint16_T)(uint8_T)(Current_M4 >> 8);

  /* Outputs for enable SubSystem: '<S11>/C-function Call' incorporates:
   *  Constant: '<S26>/Constant'
   *  Constant: '<S9>/data length - set maualy'
   *  EnablePort: '<S44>/Enable'
   *  RelationalOperator: '<S26>/Compare'
   */
  if (Vejlupek_CAR4_dsPIC33_ACU_DWork.CfunctionCall_MODE == SUBSYS_DISABLED) {
    Vejlupek_CAR4_dsPIC33_ACU_DWork.CfunctionCall_MODE = SUBSYS_ENABLED;
  }

  if (Vejlupek_CAR4_dsPIC33_ACU_DWork.CfunctionCall_MODE == SUBSYS_ENABLED) {
    /* S-Function "dsPIC_C_function_Call" Block: <S44>/ECAN1 Transmit [ecanFunctions.c] */
    ecan1_transmit_matlab(Vejlupek_CAR4_dsPIC33_ACU44_S_B.y);
  }

  /* end of Outputs for SubSystem: '<S11>/C-function Call' */
}

/* Model step function for TID2 */
void Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_step2(void) /* Sample time: [0.01s, 0.0s] */
{
  {
    uint16_T rtb_ButtonsTs1e2;
    int16_T rtb_Abs;
    uint16_T rtb_CurrentM3Ts1e2toUART1;
    uint16_T rtb_CurrentM4Ts1e2toUART2;
    boolean_T rtb_DISABLETs1e2toUART;
    int16_T rtb_Motor_MomentumTs1e2toUART;
    int16_T rtb_Steering_RearTs1e2toUART;
    boolean_T rtb_DISABLETs1e2;
    real_T rtb_PulseGenerator4;

    /* DataStoreRead: '<S2>/Motor_Momentum Ts = 1e-2' */
    rtb_Abs = Vejlupek_CAR4_dsPIC33_ACU_DWork.Steering_Rear;

    /* Switch: '<S2>/Switch1' incorporates:
     *  Constant: '<S2>/Zero steering angle'
     *  DataStoreRead: '<S2>/Buttons Ts = 1e-2'
     *  S-Function (sfix_bitop): '<S2>/Bitwise Operator2'
     */
    if ((Vejlupek_CAR4_dsPIC33_ACU_DWork.Buttons & 16U) > 0U) {
      rtb_Abs = 0;
    }

    /* Sum: '<Root>/Sum1' incorporates:
     *  Constant: '<Root>/Servo 4'
     */
    rtb_ButtonsTs1e2 = 3810U + (uint16_T)rtb_Abs;

    /* Saturate: '<Root>/Saturation' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.Saturation = rt_SATURATE(rtb_ButtonsTs1e2,
      2185U, 4835U);

    /* Sum: '<Root>/Sum' incorporates:
     *  Constant: '<Root>/Servo 2'
     */
    rtb_ButtonsTs1e2 = (uint16_T)rtb_Abs + 3620U;

    /* Saturate: '<Root>/Saturation1' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.Saturation1 = rt_SATURATE(rtb_ButtonsTs1e2,
      2790U, 5170U);

    /* S-Function "dsPIC_PWM_OC_HW" Block: <Root>/Output Compare HW */
    {
      if (Vejlupek_CAR4_dsPIC33_ACU44_S_B.Saturation > 49999)/* Saturate */
        OC1RS = 49999;
      else
        OC1RS = Vejlupek_CAR4_dsPIC33_ACU44_S_B.Saturation;
      if (Vejlupek_CAR4_dsPIC33_ACU44_S_B.Saturation1 > 49999)/* Saturate */
        OC2RS = 49999;
      else
        OC2RS = Vejlupek_CAR4_dsPIC33_ACU44_S_B.Saturation1;
    }

    /* DataStoreRead: '<Root>/Current M3 Ts = 1e-2 to UART1' */
    rtb_CurrentM3Ts1e2toUART1 = Current_M3;

    /* DataStoreRead: '<Root>/Current M4 Ts = 1e-2 to UART2' */
    rtb_CurrentM4Ts1e2toUART2 = Current_M4;

    /* DataStoreRead: '<Root>/DISABLE Ts = 1e-2 to UART' */
    rtb_DISABLETs1e2toUART = Vejlupek_CAR4_dsPIC33_ACU_DWork.DISABLE;

    /* DataStoreRead: '<Root>/Motor_Momentum Ts = 1e-2 to UART' */
    rtb_Motor_MomentumTs1e2toUART =
      Vejlupek_CAR4_dsPIC33_ACU_DWork.Motor_Momentum;

    /* S-Function "dsPIC_C_function_Call" Block: <S8>/Angle => Count by 1 roll = 1000 QEI 500 mode x2 without index chx */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.AngleCountby1roll1000QEI500mode = GetQEI1();

    /* S-Function "dsPIC_C_function_Call" Block: <S8>/Angle => Count by 1 roll = 1000 QEI 500 mode x2 without index chx1 */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.AngleCountby1roll1000QEI500mo_d = GetQEI2();

    /* DataStoreRead: '<Root>/Steering_Rear Ts = 1e-2 to UART' */
    rtb_Steering_RearTs1e2toUART = Vejlupek_CAR4_dsPIC33_ACU_DWork.Steering_Rear;

    /* DataStoreRead: '<S1>/Steering_Rear Ts = 1e-2' */
    rtb_Abs = Vejlupek_CAR4_dsPIC33_ACU_DWork.Steering_Rear;

    /* Switch: '<S1>/Switch' incorporates:
     *  DataStoreRead: '<S1>/Buttons Ts = 1e-2'
     *  DataStoreRead: '<S1>/Motor_Momentum Ts = 1e-2'
     *  S-Function (sfix_bitop): '<S1>/Bitwise Operator1'
     */
    if (!((Vejlupek_CAR4_dsPIC33_ACU_DWork.Buttons & 16U) > 0U)) {
      rtb_Abs = Vejlupek_CAR4_dsPIC33_ACU_DWork.Motor_Momentum;
    }

    /* Gain: '<Root>/Gain' */
    rtb_Abs <<= 1U;

    /* Saturate: '<Root>/Saturation2' */
    rtb_Abs = rt_SATURATE(rtb_Abs, -4250, 4250);

    /* RelationalOperator: '<S20>/Compare' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.Compare = (rtb_Abs <= 0);

    /* S-Function "dsPIC_Digital_OutputWrite" Block: <S7>/DIR 1 */
    LATAbits.LATA10 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.Compare;

    /* RelationalOperator: '<S21>/Compare' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.Compare_d = (rtb_Abs <= 0);

    /* S-Function "dsPIC_Digital_OutputWrite" Block: <S7>/DIR 2 */
    LATBbits.LATB10 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.Compare_d;

    /* Gain: '<S7>/Different Frequency Correction M1' incorporates:
     *  Abs: '<S7>/Abs'
     */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.DifferentFrequencyCorrectionM1 = (uint16_T)
      (5 * rt_ABS(rtb_Abs) >> 2);

    /* Abs: '<S7>/Abs1' */
    rtb_Abs = rt_ABS(rtb_Abs);

    /* Gain: '<S7>/Different Frequency Correction M2' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.DifferentFrequencyCorrectionM2 = (uint16_T)
      (5 * rtb_Abs >> 2);

    /* S-Function "dsPIC_PWM_motor" Block: <S7>/PWM Motor Output */
    P1DC1 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.DifferentFrequencyCorrectionM1 ;
    P1DC2 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.DifferentFrequencyCorrectionM2 ;

    /* DataStoreRead: '<Root>/DISABLE Ts = 1e-2' */
    rtb_DISABLETs1e2 = Vejlupek_CAR4_dsPIC33_ACU_DWork.DISABLE;

    /* DataStoreRead: '<Root>/Buttons Ts = 1e-2' */
    rtb_ButtonsTs1e2 = Vejlupek_CAR4_dsPIC33_ACU_DWork.Buttons;

    /* Logic: '<S12>/Logical Operator1' incorporates:
     *  S-Function (sfix_bitop): '<S12>/Bitwise Operator'
     */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.LogicalOperator1 =
      (Vejlupek_CAR4_dsPIC33_ACU_DWork.DISABLE ||
       ((Vejlupek_CAR4_dsPIC33_ACU_DWork.Buttons & 8U) != 0U));

    /* S-Function "dsPIC_Digital_OutputWrite" Block: <S7>/DISable 1 */
    LATAbits.LATA7 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.LogicalOperator1;

    /* Logic: '<S12>/Logical Operator2' incorporates:
     *  S-Function (sfix_bitop): '<S12>/Bitwise Operator1'
     */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.LogicalOperator2 = (rtb_DISABLETs1e2 ||
      ((rtb_ButtonsTs1e2 & 4U) != 0U));

    /* S-Function "dsPIC_Digital_OutputWrite" Block: <S7>/DISable 2 */
    LATCbits.LATC9 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.LogicalOperator2;

    /* DiscretePulseGenerator: '<S6>/Pulse Generator4' */
    rtb_PulseGenerator4 = (Vejlupek_CAR4_dsPIC33_ACU_DWork.clockTickCounter <
      50L) && (Vejlupek_CAR4_dsPIC33_ACU_DWork.clockTickCounter >= 0L) ? 1.0 :
      0.0;
    if (Vejlupek_CAR4_dsPIC33_ACU_DWork.clockTickCounter >= 99L) {
      Vejlupek_CAR4_dsPIC33_ACU_DWork.clockTickCounter = 0L;
    } else {
      Vejlupek_CAR4_dsPIC33_ACU_DWork.clockTickCounter =
        Vejlupek_CAR4_dsPIC33_ACU_DWork.clockTickCounter + 1L;
    }

    /* DataTypeConversion: '<S6>/Data Type Conversion11' */
    Vejlupek_CAR4_dsPIC33_ACU44_S_B.DataTypeConversion11 = (rtb_PulseGenerator4
      != 0.0);

    /* S-Function "dsPIC_Digital_OutputWrite" Block: <S6>/Op controlLED */
    LATAbits.LATA8 = Vejlupek_CAR4_dsPIC33_ACU44_S_B.DataTypeConversion11;
    Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition12_Buffer0 =
      rtb_CurrentM3Ts1e2toUART1;
    Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition13_Buffer0 =
      rtb_CurrentM4Ts1e2toUART2;
    Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition6_Buffer0 =
      Vejlupek_CAR4_dsPIC33_ACU44_S_B.AngleCountby1roll1000QEI500mode;
    Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition7_Buffer0 =
      Vejlupek_CAR4_dsPIC33_ACU44_S_B.AngleCountby1roll1000QEI500mo_d;
    Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition1_Buffer0 =
      rtb_Steering_RearTs1e2toUART;
    Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition2_Buffer0 =
      rtb_Motor_MomentumTs1e2toUART;
    Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition3_Buffer0 =
      rtb_DISABLETs1e2toUART;
  }
}

void Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_step(int_T tid)
{
  switch (tid) {
   case 0 :
    Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_step0();
    break;

   case 1 :
    Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_step1();
    break;

   case 2 :
    Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_step2();
    break;

   default :
    break;
  }
}

/* Model initialize function */
void Vejlupek_CAR4_dsPIC33_ACU44_SlaveBoard_direct_initialize(boolean_T
  firstTime)
{
  (void)firstTime;

  /* Registration code */

  /* initialize real-time model */
  (void) memset((void *)Vejlupek_CAR4_dsPIC33_ACU44__M,0,
                sizeof(RT_MODEL_Vejlupek_CAR4_dsPIC33_));

  /* block I/O */
  (void) memset(((void *) &Vejlupek_CAR4_dsPIC33_ACU44_S_B),0,
                sizeof(BlockIO_Vejlupek_CAR4_dsPIC33_A));

  /* states (dwork) */
  (void) memset((void *)&Vejlupek_CAR4_dsPIC33_ACU_DWork, 0,
                sizeof(D_Work_Vejlupek_CAR4_dsPIC33_AC));

  /* exported global states */
  Current_M3 = 0;
  Current_M4 = 0;
  ct_turnsE1 = 0;
  ct_turnsE2 = 0;

  /* S-Function "dsPIC_MASTER" initialization Block: <Root>/Master */
  /* 0 testfcy 	*/
  /*  2.0E+007	*/
  /* CONFIG TIMER 1 for scheduling steps 	*/
  ConfigIntTimer1(T1_INT_PRIOR_0 & T1_INT_OFF);
  T1CON = 0x8000 ;                     /* T1_PS_1_1	*/
  PR1 = 19999;

  /* Configuration TIMER 3 */
  ConfigIntTimer3(T3_INT_PRIOR_0 & T3_INT_OFF);
  OpenTimer3(T3_ON & T3_GATE_OFF &
             T3_PS_1_8 & T3_SOURCE_INT , 49999);

  /* Configuration TRIS */
  TRISA = 64127;
  TRISB = 58351;
  TRISC = 64991;

  /* Configuration ADCHS */
  AD1PCFGL = (496U);

  /* S-Function "dsPIC_UART_config" initialization Block: <Root>/UART 1 Configuration */
  /* set priority */
  _U1RXIP = 0x0007 & 2;
  _U1TXIP = 0x0007 & 1;

  /* enable/disable interrupt */
  _U1RXIE = (3 != 0);
  _U1TXIE = 0;
  U1MODE = 0x8080 + 1024;
  U1STA = 0x0 + 1 + 1024 + 192 + (1<<12);
  U1BRG = 10;
  TxPtrHaut_Uart1 = 0;
  TxPtrBas_Uart1 = 0;
  RxPtrHaut_Uart1 = 0;
  RxPtrBas_Uart1 = 0;
  _U1TXIF = 0;

  /* S-Function "dsPIC_ADC" initialization Block: <S8>/current */
  /* Initialise ADC converter */
  /* 0 mean each 1 conversion */
#define NoSamplesADC                   1
#define ADCSval                        26
#define SAMCval                        (1 << 8)
#define OutFormatBitsval               (0 << 8)
#define VoltRef                        (0 << 13)

  /* Use internal counter Trigger, continuous simulatenous sampling */
  AD1CON1 = 0x0004 | (7 << 5) | (1 << 10)| OutFormatBitsval;
  AD1CON2 = 0x0400 | (NoSamplesADC << 2) | VoltRef;
  AD1CSSL = (3 & 65535);
  AD1CON3 = (ADCSval | SAMCval) & 0xff7f;
  AD1CHS0 = 0x0000;
  IPC2 = (IPC2 | (6 << 12));           /*Interrupt Priority : 6*/
  _AD1IF = 0;
  DMACS0 = 0;
  DMA4CON = 0;
  DMA4REQ = 13;
  DMA4STA = __builtin_dmaoffset(ADCBuffChannelDMA);
  DMA4PAD = (volatile uint16_T) &ADC1BUF0;
  DMA4CNT = 2-1;
  _DMA4IF = 0;
  _DMA4IE = 0;
  AD1CON1bits.ADON= 1;
  DMA4CONbits.CHEN = 1;

  /* _AD1IE = 1; */
  /* 1 sample/output-Step ; Do not need interrupt so do not enable it. */
  /* Clear Off Any Interrupt Due To Configuration */
  /* Turn on ADC Module */
  AD1CON1bits.ADON= 1;

  /* S-Function "dsPIC_PWM_out" initialization Block: <Root>/Output Compare HW */
  /* Initialise Output Capture */
  /*config output capture*/
  OC1RS = 0;
  OC1R = 0;
  OC1CON = (1 << 15) + (1 << 3) + 6;

  /*config output capture*/
  OC2RS = 0;
  OC2R = 0;
  OC2CON = (1 << 15) + (1 << 3) + 6;

  /* S-Function "dsPIC_PWM_motor" initialization Block: <S7>/PWM Motor Output */
  /* Initialise Output Capture */
  SetDCMCPWM1(1,0,0);
  SetDCMCPWM1(2,0,0);

#define config                         (PWM1_INT_DIS & PWM1_FLTA_DIS_INT & PWM1_INT_PR1 & PWM1_FLTA_INT_PR0)

  ConfigIntMCPWM1( config );

#define config1                        (PWM1_EN & 0xDFFF & PWM1_OP_SCALE1 & PWM1_IPCLK_SCALE1 & PWM1_MOD_FREE)
#define config2                        (0xffff & PWM1_PEN1L & PWM1_MOD1_IND & PWM1_PDIS1H & PWM1_PEN2L & PWM1_MOD2_IND & PWM1_PDIS2H & PWM1_PDIS3L & PWM1_PDIS3H)
#define config3                        (PWM1_SEVOPS1 & PWM1_OSYNC_TCY & PWM1_UEN)

  OpenMCPWM1(2500,0,config1,config2,config3);

  /* Start for DiscretePulseGenerator: '<S6>/Pulse Generator4' */
  Vejlupek_CAR4_dsPIC33_ACU_DWork.clockTickCounter = 0L;

  /* S-Function "dsPIC_PIN_Mapping : Config" initialization Block: <Root>/PIN Mapping */
  /*unlock */
  OSCCON = 0x46;
  OSCCON = 0x57;
  OSCCONbits.IOLOCK = 0;
  RPINR14 = 4368;
  RPINR16 = 515;
  RPINR18 = 65298;
  RPINR26 = 20;
  RPOR2 = 65283;
  RPOR5 = 5119;
  RPOR6 = 65298;
  RPOR10 = 4351;

  /*lock*/
  OSCCON = 0x46;
  OSCCON = 0x57;
  OSCCONbits.IOLOCK = 1;

  /* S-Function "dsPIC_C_function_Call" Block: <S3>/C Function Call [ecanFunctions.c] */
  ecan1_init(&Vejlupek_CAR4_dsPIC33_AC_ConstP.Constant_Value_b[0]);

  /* S-Function "dsPIC_C_function_Call" Block: <Root>/QEI Inicialization */
  QEIInit();

  /* InitializeConditions for RateTransition: '<Root>/Rate Transition12' */
  Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition12_Buffer0 = 0U;

  /* InitializeConditions for RateTransition: '<Root>/Rate Transition13' */
  Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition13_Buffer0 = 0U;

  /* InitializeConditions for RateTransition: '<Root>/Rate Transition6' */
  Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition6_Buffer0 = 0U;

  /* InitializeConditions for RateTransition: '<Root>/Rate Transition7' */
  Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition7_Buffer0 = 0U;

  /* InitializeConditions for RateTransition: '<Root>/Rate Transition1' */
  Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition1_Buffer0 = 0;

  /* InitializeConditions for RateTransition: '<Root>/Rate Transition2' */
  Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition2_Buffer0 = 0;

  /* InitializeConditions for RateTransition: '<Root>/Rate Transition3' */
  Vejlupek_CAR4_dsPIC33_ACU_DWork.RateTransition3_Buffer0 = false;
}

/* File trailer for Real-Time Workshop generated code.
 *
 * [EOF]
 */
