#### File: D:\bobes\0 skola\_car4\my matlab\HIL - PID speed\firmware\Vejlupek_Car4_HIL_PIDSpeed_dspic\sources\Vejlupek_Car4_HIL_PIDSpeed.c
        1   /*
        2    * File: Vejlupek_Car4_HIL_PIDSpeed.c
        3    *
        4    * Real-Time Workshop code generated for Simulink model Vejlupek_Car4_HIL_PIDSpeed.
        5    *
        6    * Model version                        : 1.460
        7    * Real-Time Workshop file version      : 7.3  (R2009a)  15-Jan-2009
        8    * Real-Time Workshop file generated on : Fri Apr 30 16:12:44 2010
        9    * TLC version                          : 7.3 (Jan 18 2009)
       10    * C source code generated on           : Fri Apr 30 16:12:44 2010
       11    *--------------------------------------------------------------
       12    *   Embedded Coder for Microchip dsPIC family.                 |
       13    *   Generate .c and .h files from your Matlab/simulink model   |
       14    *   and compile the diagram to .hex and .coff file that can be |
       15    *   downloaded directly into the microcontroller               |
       16    *                                                              |
       17    * Licence Accorded to Ing. Robert Grepl, Ph.D.                 |
       18    *                     Technical University Brno                |
       19    *                                                              |
       20    *   Written by Lubin KERHUEL -  http://www.kerhuel.eu          |
       21    *  - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - |
       22    *   Version 3.1                               26-Nov-2009      |
       23    *   For Matlab 7.8             R2009a                          |
       24    *--------------------------------------------------------------
       25    */
       26
       27   #include "Vejlupek_Car4_HIL_PIDSpeed.h"
       28   #include "Vejlupek_Car4_HIL_PIDSpeed_private.h"
       29
       30   /* Exported block states */
       31   uint16_T ct_turns;                     /* '<Root>/Encoder_01' */
       32
       33   /* Block signals (auto storage) */
       34   BlockIO_Vejlupek_Car4_HIL_PIDSp Vejlupek_Car4_HIL_PIDSpeed_B;
       35
       36   /* Block states (auto storage) */
       37   D_Work_Vejlupek_Car4_HIL_PIDSpe Vejlupek_Car4_HIL_PIDSpee_DWork;
       38
       39   /* Real-time model */
       40   RT_MODEL_Vejlupek_Car4_HIL_PIDS Vejlupek_Car4_HIL_PIDSpeed_M_;
       41   RT_MODEL_Vejlupek_Car4_HIL_PIDS *Vejlupek_Car4_HIL_PIDSpeed_M =
       42     &Vejlupek_Car4_HIL_PIDSpeed_M_;
       43   static void rate_monotonic_scheduler(void);
       44   void sMultiWordShl(const uint32_T u1[], int16_T n1, uint16_T n2, uint32_T y[],
       45                      int16_T n)
       46   {
       47     int16_T nb;
       48     int16_T nc;
       49     int16_T i;
       50     uint32_T ys;
       51     uint32_T u1i;
       52     uint32_T yi;
       53     uint16_T nr;
       54     uint16_T nl;
       55     int16_T loop_ub;
       56     int16_T loop_ub_0;
       57     int16_T loop_ub_1;
       58     nb = (int16_T)(n2 >> 5);
       59     ys = (u1[n1 - 1] & 2147483648UL) != 0UL ? MAX_uint32_T : 0UL;
       60     nc = nb > n ? n : nb;
       61     u1i = 0UL;
       62     loop_ub = nc - 1;
       63     for (i = 0; i <= loop_ub; i = i + 1) {
       64       y[i] = 0UL;
       65     }
       66
       67     if (nb < n) {
       68       nl = n2 - ((uint16_T)nb << 5U);
       69       nb = n1 + nb;
       70       if (nb > n) {
       71         nb = n;
       72       }
       73
       74       nb = nb - i;
       75       if (nl > 0U) {
       76         nr = 32U - nl;
       77         loop_ub_0 = nb - 1;
       78         for (nc = 0; nc <= loop_ub_0; nc = nc + 1) {
       79           yi = u1i >> nr;
       80           u1i = u1[nc];
       81           y[i] = u1i << nl | yi;
       82           i = i + 1;
       83         }
       84
       85         if (i < n) {
       86           y[i] = u1i >> nr | ys << nl;
       87           i = i + 1;
       88         }
       89       } else {
       90         loop_ub_1 = nb - 1;
       91         for (nc = 0; nc <= loop_ub_1; nc = nc + 1) {
       92           y[i] = u1[nc];
       93           i = i + 1;
       94         }
       95       }
       96     }
       97
       98     while (i < n) {
       99       y[i] = ys;
      100       i = i + 1;
      101     }
      102   }
      103
      104   void sLong2MultiWord(int32_T u, uint32_T y[], int16_T n)
      105   {
      106     uint32_T yi;
      107     int16_T i;
      108     int16_T loop_ub;
      109     y[0] = (uint32_T)u;
      110     yi = u < 0L ? MAX_uint32_T : 0UL;
      111     loop_ub = n - 1;
      112     for (i = 1; i <= loop_ub; i = i + 1) {
      113       y[i] = yi;
      114     }
      115   }
      116
      117   void MultiWordAdd(const uint32_T u1[], const uint32_T u2[], uint32_T y[],
      118                     int16_T n)
      119   {
      120     uint32_T yi;
      121     uint32_T u1i;
      122     uint32_T carry = 0UL;
      123     int16_T i;
      124     int16_T loop_ub;
      125     loop_ub = n - 1;
      126     for (i = 0; i <= loop_ub; i = i + 1) {
      127       u1i = u1[i];
      128       yi = (u1i + u2[i]) + carry;
      129       y[i] = yi;
      130       carry = carry != 0UL ? yi <= u1i ? 1UL : 0UL : yi < u1i ? 1UL : 0UL;
      131     }
      132   }
      133
      134   uint32_T MultiWord2uLong(const uint32_T u[])
      135   {
      136     uint32_T y;
      137     y = u[0];
      138     return y;
      139   }
      140
      141   void sMultiWordShr(const uint32_T u1[], int16_T n1, uint16_T n2, uint32_T y[],
      142                      int16_T n)
      143   {
      144     int16_T nb;
      145     int16_T i;
      146     uint32_T ys;
      147     uint32_T yi;
      148     uint32_T u1i;
      149     int16_T nc;
      150     uint16_T nr;
      151     uint16_T nl;
      152     int16_T i1;
      153     int16_T loop_ub;
      154     int16_T loop_ub_0;
      155     nb = (int16_T)(n2 >> 5);
      156     i = 0;
      157     ys = (u1[n1 - 1] & 2147483648UL) != 0UL ? MAX_uint32_T : 0UL;
      158     if (nb < n1) {
      159       nc = n + nb;
      160       if (nc > n1) {
      161         nc = n1;
      162       }
      163
      164       nr = n2 - ((uint16_T)nb << 5U);
      165       if (nr > (uint16_T)0) {
      166         nl = 32U - nr;
      167         u1i = u1[nb];
      168         loop_ub = nc - 1;
      169         for (i1 = nb + 1; i1 <= loop_ub; i1 = i1 + 1) {
      170           yi = u1i >> nr;
      171           u1i = u1[i1];
      172           y[i] = u1i << nl | yi;
      173           i = i + 1;
      174         }
      175
      176         y[i] = (nc < n1 ? u1[nc] : ys) << nl | u1i >> nr;
      177         i = i + 1;
      178       } else {
      179         loop_ub_0 = nc - 1;
      180         for (i1 = nb; i1 <= loop_ub_0; i1 = i1 + 1) {
      181           y[i] = u1[i1];
      182           i = i + 1;
      183         }
      184       }
      185     }
      186
      187     while (i < n) {
      188       y[i] = ys;
      189       i = i + 1;
      190     }
      191   }
      192
      193   /* Set which subrates need to run this base step (base rate always runs).
      194    * This function must be called prior to calling the model step function
      195    * in order to "remember" which rates need to run this base step.  The
      196    * buffering of events allows for overlapping preemption.
      197    */
      198   void Vejlupek_Car4_HIL_PIDSpeed_SetEventsForThisBaseStep(boolean_T *eventFlags)
      199   {
      200     /* Task runs when its counter is zero, computed via rtmStepTask macro */
      201     eventFlags[1] = ((boolean_T)rtmStepTask(Vejlupek_Car4_HIL_PIDSpeed_M, 1));
      202   }
      203
      204   /* This function updates active task flag for each subrate
      205    * and rate transition flags for tasks that exchagne data.
      206    * The function assumes rate-monotonic multitasking scheduler.
      207    * The function must be called at model base rate so that
      208    * the generated code self-manages all its subrates and rate
      209    * transition flags.
      210    */
      211   static void rate_monotonic_scheduler(void)
      212   {
      213     /* Compute which subrates run during the next base time step.  Subrates
      214      * are an integer multiple of the base rate counter.  Therefore, the subtask
      215      * counter is reset when it reaches its limit (zero means run).
      216      */
      217     if (++Vejlupek_Car4_HIL_PIDSpeed_M->Timing.TaskCounters.TID[1] == 10) {/* Sample time: [0.1s, 0.0s] */
      218       Vejlupek_Car4_HIL_PIDSpeed_M->Timing.TaskCounters.TID[1] = 0;
      219     }
      220   }
      221
      222   /* Model step function for TID0 */
      223   void Vejlupek_Car4_HIL_PIDSpeed_step0(void) /* Sample time: [0.01s, 0.0s] */
      224   {
      225     {                                    /* Sample time: [0.01s, 0.0s] */
      226       rate_monotonic_scheduler();
      227     }
      228
      229     {
      230       real_T rtb_HzLED;
      231
      232       /* S-Function "dsPIC_Digital_OutputRead" Block: <S6>/Termal_Flag */
      233       Vejlupek_Car4_HIL_PIDSpeed_B.Termal_Flag = LATCbits.LATC5;
      234
      235       /* S-Function "ADC" Block: <S6>/proud */
      236       Vejlupek_Car4_HIL_PIDSpeed_B.proud = ADC1BUF0;
      237
      238       /* S-Function "dsPIC_Digital_OutputWrite" Block: <S6>/Brake_(odpojeni_napeti) */
      239       LATAbits.LATA7 = FALSE;
      240
      241       /* DiscretePulseGenerator: '<S1>/2 Hz LED' */
      242       rtb_HzLED = (Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter < 50L) &&
      243         (Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter >= 0L) ? 1.0 : 0.0;
      244       if (Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter >= 99L) {
      245         Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter = 0L;
      246       } else {
      247         Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter =
      248           Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter + 1L;
      249       }
      250
      251       /* DataTypeConversion: '<S1>/Data Type Conversion' */
      252       Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion_e = (rtb_HzLED != 0.0);
      253
      254       /* S-Function "dsPIC_Digital_OutputWrite" Block: <S1>/A8 - GP LED */
      255       LATAbits.LATA8 = Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion_e;
      256     }
      257   }
      258
      259   /* Model step function for TID1 */
      260   void Vejlupek_Car4_HIL_PIDSpeed_step1(void) /* Sample time: [0.1s, 0.0s] */
      261   {
      262     /* local block i/o variables */
      263     real_T rtb_TSamp;
      264     int16_T rtb_Sum1_m;
      265
      266     {
      267       int64m_T numAccum;
      268       int32_T k;
      269       uint8_T rtb_UnitDelay4;
      270       uint8_T rtb_UnitDelay9;
      271       uint8_T rtb_UnitDelay7;
      272       uint8_T rtb_UnitDelay8;
      273       int16_T rtb_MultiportSwitch;
      274       real_T rtb_DiscreteTimeIntegrator;
      275       real_T rtb_Diff;
      276       uint8_T rtb_UnitDelay1_a;
      277       uint8_T rtb_UnitDelay2;
      278       uint8_T rtb_UnitDelay3;
      279       uint8_T rtb_UnitDelay5;
      280       uint8_T rtb_UnitDelay6;
      281       uint16_T rtb_Saturation;
      282       int32_T rtb_Switch1;
      283       uint16_T rtb_DiscreteFilter2;
      284       int16_T rtb_PGain;
      285       int16_T rtb_Sum1;
      286       real_T tmp;
      287       real_T tmp_0;
      288       int64m_T tmp_1;
      289       int64m_T tmp_2;
      290       int64m_T tmp_3;
      291       int64m_T tmp_4;
      292
      293       /* S-Function "dsPIC_C_function_Call" Block: <S6>/Angle => Count by 1 roll = 1000 QEI 500 mode x2 without index chx */
      294       Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode = GetQEI();
      295
      296       /* Switch: '<S4>/Switch1' incorporates:
      297        *  Constant: '<S4>/Constant1'
      298        *  Sum: '<S4>/Sum4'
      299        *  Sum: '<S4>/Sum5'
      300        *  UnitDelay: '<S4>/Unit Delay1'
      301        */
      302       if ((int32_T)Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode -
      303           (int32_T)Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE >= 32768L) {
      304         rtb_Switch1 = (int32_T)
      305           Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode - 65535L;
      306       } else {
      307         rtb_Switch1 = (int32_T)
      308           Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode;
      309       }
      310
      311       /* DiscreteFilter: '<S4>/Discrete Filter2' */
      312       Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_tmp = rtb_Switch1;
      313       sLong2MultiWord(Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_tmp,
      314                       &tmp_1.chunks[0U], 2);
      315       sMultiWordShl(&tmp_1.chunks[0U], 2, (uint16_T)30, &numAccum.chunks[0U], 2);
      316       sLong2MultiWord(Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_DSTATE,
      317                       &tmp_2.chunks[0U], 2);
      318       sMultiWordShl(&tmp_2.chunks[0U], 2, (uint16_T)30, &tmp_1.chunks[0U], 2);
      319       MultiWordAdd(&numAccum.chunks[0U], &tmp_1.chunks[0U], &tmp_2.chunks[0U], 2);
      320       numAccum = tmp_2;
      321       sMultiWordShr(&numAccum.chunks[0U], 2, (uint16_T)30, &tmp_1.chunks[0U], 2);
      322       rtb_DiscreteFilter2 = (uint16_T)MultiWord2uLong(&tmp_1.chunks[0U]);
      323
      324       /* Switch: '<S4>/Switch' incorporates:
      325        *  Constant: '<S4>/Constant'
      326        *  Sum: '<S4>/Sum2'
      327        *  Sum: '<S4>/Sum3'
      328        *  UnitDelay: '<S4>/Unit Delay1'
      329        */
      330       if ((int32_T)Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode -
      331           (int32_T)Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE >= -32768L) {
      332         rtb_Switch1 = (int32_T)Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE;
      333       } else {
      334         rtb_Switch1 = (int32_T)Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE -
      335           65535L;
      336       }
      337
      338       /* DiscreteFilter: '<S4>/Discrete Filter1' */
      339       Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_tmp = rtb_Switch1;
      340       sLong2MultiWord(Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_tmp,
      341                       &tmp_3.chunks[0U], 2);
      342       sMultiWordShl(&tmp_3.chunks[0U], 2, (uint16_T)30, &numAccum.chunks[0U], 2);
      343       sLong2MultiWord(Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_DSTATE,
      344                       &tmp_4.chunks[0U], 2);
      345       sMultiWordShl(&tmp_4.chunks[0U], 2, (uint16_T)30, &tmp_3.chunks[0U], 2);
      346       MultiWordAdd(&numAccum.chunks[0U], &tmp_3.chunks[0U], &tmp_4.chunks[0U], 2);
      347       numAccum = tmp_4;
      348       sMultiWordShr(&numAccum.chunks[0U], 2, (uint16_T)30, &tmp_3.chunks[0U], 2);
      349       rtb_Saturation = (uint16_T)MultiWord2uLong(&tmp_3.chunks[0U]);
      350
      351       /* DiscreteFilter: '<Root>/Discrete Filter' incorporates:
      352        *  Sum: '<S4>/Sum1'
      353        */
      354       Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_tmp = (int16_T)
      355         rtb_DiscreteFilter2 - (int16_T)rtb_Saturation;
      356       rtb_PGain = (int16_T)((((int32_T)
      357         Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_tmp << 14) + ((int32_T)
      358         Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_DSTATE << 14)) >> 15);
      359
      360       /* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
      361        *  Constant: '<Root>/Constant'
      362        *  Sum: '<Root>/Sum1'
      363        */
      364       Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion = (uint16_T)(int16_T)
      365         ((int32_T)(rtb_PGain >> 1) - 32768L);
      366
      367       /* S-Function "UART_RX" Block: <Root>/Rx Input1 */
      368       {
      369         if (U1STAbits.URXDA != 0)
      370           _U1RXIF = 1;
      371         if (RxPtrHaut_Uart1 != RxPtrBas_Uart1) {
      372           Vejlupek_Car4_HIL_PIDSpeed_B.RxInput1 = (uint8_T)
      373             RxTab_Uart1[RxPtrBas_Uart1 ++];/* Use only the 8 low bytes or RxReg */
      374           RxPtrBas_Uart1 &= (Rx_BUFF_SIZE_Uart1-1);
      375         } else {
      376           Vejlupek_Car4_HIL_PIDSpeed_B.RxInput1 = 0;
      377         }
      378       }
      379
      380       /* UnitDelay: '<S2>/Unit Delay4' */
      381       rtb_UnitDelay4 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay4_DSTATE;
      382
      383       /* UnitDelay: '<S2>/Unit Delay9' */
      384       rtb_UnitDelay9 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay9_DSTATE;
      385
      386       /* UnitDelay: '<S2>/Unit Delay7' */
      387       rtb_UnitDelay7 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay7_DSTATE;
      388
      389       /* UnitDelay: '<S2>/Unit Delay8' */
      390       rtb_UnitDelay8 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay8_DSTATE;
      391
      392       /* MultiPortSwitch: '<S14>/Multiport Switch' incorporates:
      393        *  Constant: '<S10>/Constant'
      394        *  Constant: '<S2>/Constant3'
      395        *  Constant: '<S7>/Constant'
      396        *  Constant: '<S8>/Constant'
      397        *  Constant: '<S9>/Constant'
      398        *  DataTypeConversion: '<S2>/Data Type Conversion'
      399        *  Gain: '<S2>/Gain4'
      400        *  Logic: '<S2>/Logical Operator'
      401        *  RelationalOperator: '<S10>/Compare'
      402        *  RelationalOperator: '<S7>/Compare'
      403        *  RelationalOperator: '<S8>/Compare'
      404        *  RelationalOperator: '<S9>/Compare'
      405        *  Sum: '<S2>/Sum3'
      406        *  UnitDelay: '<S14>/Memory_1'
      407        *  UnitDelay: '<S2>/Unit Delay10'
      408        */
      409       if (((Vejlupek_Car4_HIL_PIDSpeed_B.RxInput1 == 211) &&
      410            (Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay4_DSTATE == 255) &&
      411            (Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay9_DSTATE == 211) &&
      412            (Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay10_DSTATE == 255)) == 0) {
      413         rtb_MultiportSwitch = Vejlupek_Car4_HIL_PIDSpee_DWork.Memory_1_DSTATE;
      414       } else {
      415         rtb_MultiportSwitch = (int16_T)(((int32_T)
      416           Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay7_DSTATE - 32768L) + (int32_T)
      417                                         ((uint16_T)
      418           Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay8_DSTATE << 8));
      419       }
      420
      421       /* Sum: '<Root>/Sum' */
      422       rtb_Sum1 = rtb_MultiportSwitch - (rtb_PGain >> 1);
      423
      424       /* Gain: '<S3>/P Gain' */
      425       rtb_Switch1 = 7291L * (int32_T)rtb_Sum1;
      426       rtb_PGain = (int16_T)((int32_T)((rtb_Switch1 < 0L) && ((rtb_Switch1 & 1023L)
      427         != 0L)) + (rtb_Switch1 >> 10));
      428
      429       /* DiscreteIntegrator: '<S3>/Discrete-Time Integrator' */
      430       rtb_DiscreteTimeIntegrator =
      431         Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE;
      432
      433       /* Gain: '<S3>/D Gain' */
      434       rtb_Switch1 = 12583L * (int32_T)rtb_Sum1;
      435       rtb_Sum1_m = ((rtb_Switch1 < 0L) && ((rtb_Switch1 & 131071L) != 0L)) +
      436         (int16_T)(rtb_Switch1 >> 17);
      437
      438       /* S-Function (sfix_tsampmath): '<S15>/TSamp' */
      439
      440       /* Sample Time Math Block: '<S15>/TSamp'
      441        *
      442        * y = u * K     where     K = 1 / ( w * Ts )
      443        *
      444        * Input0  Data Type:  Fixed Point    S16  2^-5
      445        * Output0 Data Type:  Floating Point real_T
      446        *
      447        * Parameter: WtEt == Weighted Elapsed Time
      448        *   Data Type:  Fixed Point    S16  2^1
      449        *
      450        */
      451       rtb_TSamp = ldexp((real_T)rtb_Sum1_m, (int16_T)-5L) * ldexp((real_T)5,
      452         (int16_T)1L);
      453
      454       /* Sum: '<S15>/Diff' incorporates:
      455        *  UnitDelay: '<S15>/UD'
      456        */
      457       rtb_Diff = rtb_TSamp - Vejlupek_Car4_HIL_PIDSpee_DWork.UD_DSTATE;
      458
      459       /* Sum: '<S3>/Sum1' */
      460       if (rtIsNaN(rtb_Diff) || rtIsInf(rtb_Diff)) {
      461         tmp_0 = 0.0;
      462       } else {
      463         tmp_0 = fmod(floor(rtb_Diff), 4.294967296E+009);
      464       }
      465
      466       if (rtIsNaN(rtb_DiscreteTimeIntegrator) || rtIsInf
      467           (rtb_DiscreteTimeIntegrator)) {
      468         tmp = 0.0;
      469       } else {
      470         tmp = fmod(floor(rtb_DiscreteTimeIntegrator), 4.294967296E+009);
      471       }
      472
      473       rtb_Sum1_m = (int16_T)(((int32_T)(rtb_PGain >> 5) + (tmp < 0.0 ? -((int32_T)
      474                                 (uint32_T)(-tmp)) : (int32_T)(uint32_T)tmp)) +
      475         (tmp_0 < 0.0 ? -((int32_T)(uint32_T)(-tmp_0)) : (int32_T)(uint32_T)tmp_0));
      476
      477       /* DataTypeConversion: '<Root>/Data Type Conversion1' incorporates:
      478        *  Constant: '<Root>/Constant1'
      479        *  Sum: '<Root>/Sum2'
      480        */
      481       Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion1 = (uint16_T)(int16_T)
      482         ((int32_T)rtb_MultiportSwitch - 32768L);
      483
      484       /* DataTypeConversion: '<Root>/Data Type Conversion2' incorporates:
      485        *  Constant: '<Root>/Constant2'
      486        *  Sum: '<Root>/Sum3'
      487        */
      488       Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion2 = (uint16_T)(int16_T)
      489         ((int32_T)(rtb_PGain >> 5) + 1000L);
      490
      491       /* DataTypeConversion: '<Root>/Data Type Conversion3' incorporates:
      492        *  Constant: '<Root>/Constant3'
      493        *  Sum: '<Root>/Sum4'
      494        */
      495       if (rtIsNaN(rtb_DiscreteTimeIntegrator) || rtIsInf
      496           (rtb_DiscreteTimeIntegrator)) {
      497         tmp_0 = 0.0;
      498       } else {
      499         tmp_0 = fmod(floor(rtb_DiscreteTimeIntegrator), 4.294967296E+009);
      500       }
      501
      502       Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion3 = (uint16_T)(int16_T)
      503         ((tmp_0 < 0.0 ? -((int32_T)(uint32_T)(-tmp_0)) : (int32_T)(uint32_T)tmp_0)
      504          + 1000L);
      505
      506       /* DataTypeConversion: '<Root>/Data Type Conversion4' incorporates:
      507        *  Constant: '<Root>/Constant4'
      508        *  Sum: '<Root>/Sum5'
      509        */
      510       if (rtIsNaN(rtb_Diff) || rtIsInf(rtb_Diff)) {
      511         tmp_0 = 0.0;
      512       } else {
      513         tmp_0 = fmod(floor(rtb_Diff), 4.294967296E+009);
      514       }
      515
      516       Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion4 = (uint16_T)(int16_T)
      517         ((tmp_0 < 0.0 ? -((int32_T)(uint32_T)(-tmp_0)) : (int32_T)(uint32_T)tmp_0)
      518          + 1000L);
      519
      520       /* DataTypeConversion: '<Root>/Data Type Conversion5' incorporates:
      521        *  Constant: '<Root>/Constant5'
      522        *  Sum: '<Root>/Sum6'
      523        */
      524       Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion5 = (uint16_T)(int16_T)(1000L
      525         + (int32_T)rtb_Sum1_m);
      526
      527       /* S-Function "dsPIC_UART_TX_Labview_Matlab" Block: <Root>/TX Output Multiplexed for Matlab // Labview */
      528       {
      529         static uint16_T nextToSend = 1;
      530         if ((nextToSend & 1) != 0) {
      531           if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion,1
      532                + 0U*4) )
      533             nextToSend = (nextToSend << 1);
      534         }
      535
      536         if ((nextToSend & 2) != 0) {
      537           if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion1,
      538                                  1 + 1U*4) )
      539             nextToSend = (nextToSend << 1);
      540         }
      541
      542         if ((nextToSend & 4) != 0) {
      543           if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion2,
      544                                  1 + 2U*4) )
      545             nextToSend = (nextToSend << 1);
      546         }
      547
      548         if ((nextToSend & 8) != 0) {
      549           if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion3,
      550                                  1 + 3U*4) )
      551             nextToSend = (nextToSend << 1);
      552         }
      553
      554         if ((nextToSend & 16) != 0) {
      555           if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion4,
      556                                  1 + 4U*4) )
      557             nextToSend = (nextToSend << 1);
      558         }
      559
      560         if (nextToSend >= 32)            /* should be &, but get robust here */
      561         {
      562           if (!send232Buff_Uart1(Vejlupek_Car4_HIL_PIDSpeed_B.DataTypeConversion5,
      563                                  1 + 5U*4) )
      564             nextToSend = 1;
      565         }
      566       }
      567
      568       /* UnitDelay: '<S2>/Unit Delay1' */
      569       rtb_UnitDelay1_a = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE_g;
      570
      571       /* UnitDelay: '<S2>/Unit Delay2' */
      572       rtb_UnitDelay2 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay2_DSTATE;
      573
      574       /* UnitDelay: '<S2>/Unit Delay3' */
      575       rtb_UnitDelay3 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay3_DSTATE;
      576
      577       /* UnitDelay: '<S2>/Unit Delay5' */
      578       rtb_UnitDelay5 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay5_DSTATE;
      579
      580       /* UnitDelay: '<S2>/Unit Delay6' */
      581       rtb_UnitDelay6 = Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay6_DSTATE;
      582
      583       /* Gain: '<S3>/I Gain' */
      584       rtb_Switch1 = 9339L * (int32_T)rtb_Sum1;
      585
      586       /* Saturate: '<S16>/Saturation' incorporates:
      587        *  Abs: '<S16>/Abs'
      588        */
      589       k = (int32_T)rt_ABS(rtb_Sum1_m);
      590
      591       /* Gain: '<S16>/Gain' */
      592       Vejlupek_Car4_HIL_PIDSpeed_B.Gain = 40U * (uint16_T)rt_SATURATE(k, 0L, 1000L);
      593
      594       /* S-Function "dsPIC_PWM_motor" Block: <S16>/PWM Motor Output */
      595       P1DC1 = Vejlupek_Car4_HIL_PIDSpeed_B.Gain ;
      596
      597       /* Switch: '<S16>/Switch' incorporates:
      598        *  Constant: '<S16>/Constant'
      599        *  Constant: '<S16>/Constant1'
      600        */
      601       if (rtb_Sum1_m >= 0) {
      602         Vejlupek_Car4_HIL_PIDSpeed_B.Switch = true;
      603       } else {
      604         Vejlupek_Car4_HIL_PIDSpeed_B.Switch = false;
      605       }
      606
      607       /* S-Function "dsPIC_Digital_OutputWrite" Block: <S16>/smer */
      608       LATCbits.LATC3 = Vejlupek_Car4_HIL_PIDSpeed_B.Switch;
      609
      610       /* Update for UnitDelay: '<S4>/Unit Delay1' */
      611       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE =
      612         Vejlupek_Car4_HIL_PIDSpeed_B.AngleCountby1roll1000QEI500mode;
      613
      614       /* Update for DiscreteFilter: '<S4>/Discrete Filter2' */
      615       Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_DSTATE =
      616         Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter2_tmp;
      617
      618       /* Update for DiscreteFilter: '<S4>/Discrete Filter1' */
      619       Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_DSTATE =
      620         Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter1_tmp;
      621
      622       /* Update for DiscreteFilter: '<Root>/Discrete Filter' */
      623       Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_DSTATE =
      624         Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteFilter_tmp;
      625
      626       /* Update for UnitDelay: '<S2>/Unit Delay4' */
      627       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay4_DSTATE = rtb_UnitDelay3;
      628
      629       /* Update for UnitDelay: '<S2>/Unit Delay9' */
      630       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay9_DSTATE = rtb_UnitDelay8;
      631
      632       /* Update for UnitDelay: '<S2>/Unit Delay10' */
      633       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay10_DSTATE = rtb_UnitDelay9;
      634
      635       /* Update for UnitDelay: '<S14>/Memory_1' */
      636       Vejlupek_Car4_HIL_PIDSpee_DWork.Memory_1_DSTATE = rtb_MultiportSwitch;
      637
      638       /* Update for UnitDelay: '<S2>/Unit Delay7' */
      639       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay7_DSTATE = rtb_UnitDelay6;
      640
      641       /* Update for UnitDelay: '<S2>/Unit Delay8' */
      642       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay8_DSTATE = rtb_UnitDelay7;
      643
      644       /* Update for DiscreteIntegrator: '<S3>/Discrete-Time Integrator' */
      645       Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE = (real_T)
      646         (int16_T)((int32_T)((rtb_Switch1 < 0L) && ((rtb_Switch1 & 4095L) != 0L)) +
      647                   (rtb_Switch1 >> 12)) * 0.03125 +
      648         Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE;
      649       if (Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE >= 127.0)
      650       {
      651         Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE = 127.0;
      652       } else {
      653         if (Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE <=
      654             -127.0) {
      655           Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE = -127.0;
      656         }
      657       }
      658
      659       /* Update for UnitDelay: '<S15>/UD' */
      660       Vejlupek_Car4_HIL_PIDSpee_DWork.UD_DSTATE = rtb_TSamp;
      661
      662       /* Update for UnitDelay: '<S2>/Unit Delay1' */
      663       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay1_DSTATE_g =
      664         Vejlupek_Car4_HIL_PIDSpeed_B.RxInput1;
      665
      666       /* Update for UnitDelay: '<S2>/Unit Delay2' */
      667       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay2_DSTATE = rtb_UnitDelay1_a;
      668
      669       /* Update for UnitDelay: '<S2>/Unit Delay3' */
      670       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay3_DSTATE = rtb_UnitDelay2;
      671
      672       /* Update for UnitDelay: '<S2>/Unit Delay5' */
      673       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay5_DSTATE = rtb_UnitDelay4;
      674
      675       /* Update for UnitDelay: '<S2>/Unit Delay6' */
      676       Vejlupek_Car4_HIL_PIDSpee_DWork.UnitDelay6_DSTATE = rtb_UnitDelay5;
      677     }
      678   }
      679
      680   void Vejlupek_Car4_HIL_PIDSpeed_step(int_T tid)
      681   {
      682     switch (tid) {
      683      case 0 :
      684       Vejlupek_Car4_HIL_PIDSpeed_step0();
      685       break;
      686
      687      case 1 :
      688       Vejlupek_Car4_HIL_PIDSpeed_step1();
      689       break;
      690
      691      default :
      692       break;
      693     }
      694   }
      695
      696   /* Model initialize function */
      697   void Vejlupek_Car4_HIL_PIDSpeed_initialize(boolean_T firstTime)
      698   {
      699     (void)firstTime;
      700
      701     /* Registration code */
      702
      703     /* initialize non-finites */
      704     rt_InitInfAndNaN(sizeof(real_T));
      705
      706     /* initialize real-time model */
      707     (void) memset((void *)Vejlupek_Car4_HIL_PIDSpeed_M,0,
      708                   sizeof(RT_MODEL_Vejlupek_Car4_HIL_PIDS));
      709
      710     /* block I/O */
      711     (void) memset(((void *) &Vejlupek_Car4_HIL_PIDSpeed_B),0,
      712                   sizeof(BlockIO_Vejlupek_Car4_HIL_PIDSp));
      713
      714     /* states (dwork) */
      715     (void) memset((void *)&Vejlupek_Car4_HIL_PIDSpee_DWork, 0,
      716                   sizeof(D_Work_Vejlupek_Car4_HIL_PIDSpe));
      717     Vejlupek_Car4_HIL_PIDSpee_DWork.DiscreteTimeIntegrator_DSTATE = 0.0;
      718     Vejlupek_Car4_HIL_PIDSpee_DWork.UD_DSTATE = 0.0;
      719
      720     /* exported global states */
      721     ct_turns = 0;
      722
      723     /* S-Function "dsPIC_MASTER" initialization Block: <Root>/Master */
      724     /* 1 testfcy 	*/
      725     /*  2.0E+007	*/
      726     /* CONFIG TIMER 1 for scheduling steps 	*/
      727     ConfigIntTimer1(T1_INT_PRIOR_0 & T1_INT_OFF);
      728     T1CON = 0x8010 ;                     /* T1_PS_1_8	*/
      729     PR1 = 49999;
      730
      731     /* Configuration TRIS */
      732     TRISA = 65151;
      733     TRISB = 65519;
      734     TRISC = 65495;
      735
      736     /* Configuration ADCHS */
      737     AD1PCFGL = (260U);
      738
      739     /* S-Function "dsPIC_UART_config" initialization Block: <Root>/UART Configuration */
      740     /* set priority */
      741     _U1RXIP = 0x0007 & 2;
      742     _U1TXIP = 0x0007 & 1;
      743
      744     /* enable/disable interrupt */
      745     _U1RXIE = (3 != 0);
      746     _U1TXIE = 0;
      747     U1MODE = 0x8080 + 1024;
      748     U1STA = 0x0 + 1 + 1024 + 192 + (1<<12);
      749     U1BRG = 64;
      750     TxPtrHaut_Uart1 = 0;
      751     TxPtrBas_Uart1 = 0;
      752     RxPtrHaut_Uart1 = 0;
      753     RxPtrBas_Uart1 = 0;
      754     _U1TXIF = 0;
      755
      756     /* S-Function "dsPIC_ADC" initialization Block: <S6>/proud */
      757     /* Initialise ADC converter */
      758     /* 0 mean each 1 conversion */
      759   #define NoSamplesADC                   0
      760   #define ADCSval                        6
      761   #define SAMCval                        (1 << 8)
      762   #define OutFormatBitsval               (0 << 8)
      763   #define VoltRef                        (0 << 13)
      764
      765     /* Use internal counter Trigger, continuous simulatenous sampling */
      766     AD1CON1 = 0x0004 | (7 << 5) | (0 << 10)| OutFormatBitsval;
      767     AD1CON2 = 0x0400 | (NoSamplesADC << 2) | VoltRef;
      768     AD1CSSL = (32 & 65535);
      769     AD1CON3 = (ADCSval | SAMCval) & 0xff7f;
      770     AD1CHS0 = 0x0000;
      771     IPC2 = (IPC2 | (6 << 12));           /*Interrupt Priority : 6*/
      772     _AD1IF = 0;
      773
      774     /* _AD1IE = 1; */
      775     /* 1 sample/output-Step ; Do not need interrupt so do not enable it. */
      776     /* Clear Off Any Interrupt Due To Configuration */
      777     /* Turn on ADC Module */
      778     AD1CON1bits.ADON= 1;
      779
      780     /* S-Function "dsPIC_PWM_motor" initialization Block: <S16>/PWM Motor Output */
      781     /* Initialise Output Capture */
      782     SetDCMCPWM1(1,0,0);
      783
      784   #define config                         (PWM1_INT_DIS & PWM1_FLTA_DIS_INT & PWM1_INT_PR1 & PWM1_FLTA_INT_PR0)
      785
      786     ConfigIntMCPWM1( config );
      787
      788   #define config1                        (PWM1_EN & 0xDFFF & PWM1_OP_SCALE1 & PWM1_IPCLK_SCALE1 & PWM1_MOD_FREE)
      789   #define config2                        (0xffff & PWM1_PEN1L & PWM1_MOD1_IND & PWM1_PDIS1H & PWM1_PDIS2L & PWM1_PDIS2H & PWM1_PDIS3L & PWM1_PDIS3H)
      790   #define config3                        (PWM1_SEVOPS1 & PWM1_OSYNC_TCY & PWM1_UEN)
      791
      792     OpenMCPWM1(20000,0,config1,config2,config3);
      793
      794     /* Start for DiscretePulseGenerator: '<S1>/2 Hz LED' */
      795     Vejlupek_Car4_HIL_PIDSpee_DWork.clockTickCounter = 0L;
      796
      797     /* S-Function "dsPIC_PIN_Mapping : Config" initialization Block: <Root>/PIN Mapping */
      798     /*unlock */
      799     OSCCON = 0x46;
      800     OSCCON = 0x57;
      801     OSCCONbits.IOLOCK = 0;
      802     RPINR14 = 2048;
      803     RPINR18 = 65298;
      804     RPOR2 = 65283;
      805
      806     /*lock*/
      807     OSCCON = 0x46;
      808     OSCCON = 0x57;
      809     OSCCONbits.IOLOCK = 1;
      810
      811     /* S-Function "dsPIC_C_function_Call" Block: <Root>/C Function Call */
      812     QEIInit();
      813   }
      814
      815   /* File trailer for Real-Time Workshop generated code.
      816    *
      817    * [EOF]
      818    */