ex33d_pid_acc.c

其中提到遺傳學的程式碼與應用提供給次淚相向的研究者參考下載

    ex33D_pid_B->IW115_p[0] = ex33D_pid_P->IW115_Value_i[0];
    ex33D_pid_B->IW115_p[1] = ex33D_pid_P->IW115_Value_i[1];

    rtb_Product_l[0] = ex33D_pid_B->IW115_p[0] * ex33D_pid_B->SE1_a;
    rtb_Product_l[1] = ex33D_pid_B->IW115_p[1] * ex33D_pid_B->SDE001_b;

    /* Sum: '<S41>/Sum' */
    rtb_Sum_hq = rtb_Product_l[0];
    rtb_Sum_hq += rtb_Product_l[1];

    {
      real_T cg_in_0_61_0[5];
      int32_T i1;

      for(i1=0; i1<5; i1++) {
        ex33D_pid_B->b1_j[i1] = ex33D_pid_P->b1_Value_g[i1];
      }

      cg_in_0_61_0[0] = rtb_Sum_g;
      cg_in_0_61_0[1] = rtb_Sum_l;
      cg_in_0_61_0[2] = rtb_Sum_hl;
      cg_in_0_61_0[3] = rtb_Sum_o;
      cg_in_0_61_0[4] = rtb_Sum_hq;
      for(i1=0; i1<5; i1++) {
        rtb_netsum_o[i1] = cg_in_0_61_0[i1] + ex33D_pid_B->b1_j[i1];

        rtb_Product_kr[i1] = ex33D_pid_B->IW211_a[i1] *
          rt_SATURATE(rtb_netsum_o[i1], ex33D_pid_P->Saturation_LowerSat_h,
         ex33D_pid_P->Saturation_UpperSat_l);
      }
    }

    /* Sum: '<S46>/Sum' */
    rtb_Sum_f = rtb_Product_kr[0];
    rtb_Sum_f += rtb_Product_kr[1];
    rtb_Sum_f += rtb_Product_kr[2];
    rtb_Sum_f += rtb_Product_kr[3];
    rtb_Sum_f += rtb_Product_kr[4];

    ex33D_pid_B->b2_h = ex33D_pid_P->b2_Value_o;

    ex33D_pid_B->SU1_m = (rtb_Sum_f + ex33D_pid_B->b2_h) *
      ex33D_pid_P->SU1_Gain_c;
  }

  ex33D_pid_B->Sum9 = ex33D_pid_B->Sum2 + ex33D_pid_B->SU1_m;

  /* Derivative Block: <Root>/Derivative1 */
  {
    real_T t = ssGetTaskTime(S,tid);
    real_T timeStampA = ex33D_pid_DWork->Derivative1_RWORK_i.TimeStampA;
    real_T timeStampB = ex33D_pid_DWork->Derivative1_RWORK_i.TimeStampB;

    if (timeStampA >= t && timeStampB >= t) {
      rtb_Derivative1_k = 0.0;
    } else {
      real_T deltaT;
      real_T *lastBank = &ex33D_pid_DWork->Derivative1_RWORK_i.TimeStampA;
      if (timeStampA < timeStampB) {
        if (timeStampB < t) {
          lastBank += 2;
        }
      } else if (timeStampA >= t) {
        lastBank += 2;
      }
      deltaT = t - *lastBank++;
      rtb_Derivative1_k = (ex33D_pid_B->Sum9 - *lastBank++) / deltaT;
    }
  }

  ex33D_pid_B->Sum4 = rtb_Gain12 - rtb_TransferFcn3;

  if (ssIsSampleHit(S, 1, tid)) {       /* Sample time: [0.001s, 0.0s] */

    {
      int32_T i1;

      for(i1=0; i1<5; i1++) {
        ex33D_pid_B->IW211_o[i1] = ex33D_pid_P->IW211_Value_e[i1];
      }

      ex33D_pid_B->IW111_n[0] = ex33D_pid_P->IW111_Value_e[0];
      ex33D_pid_B->IW111_n[1] = ex33D_pid_P->IW111_Value_e[1];
    }
  }

  ex33D_pid_B->SE1_p = ex33D_pid_B->Sum4 * ex33D_pid_P->SE1_Gain_i;

  /* Derivative Block: <S3>/Derivative1 */
  {
    real_T t = ssGetTaskTime(S,tid);
    real_T timeStampA = ex33D_pid_DWork->Derivative1_RWORK_k.TimeStampA;
    real_T timeStampB = ex33D_pid_DWork->Derivative1_RWORK_k.TimeStampB;

    if (timeStampA >= t && timeStampB >= t) {
      rtb_Derivative1_j = 0.0;
    } else {
      real_T deltaT;
      real_T *lastBank = &ex33D_pid_DWork->Derivative1_RWORK_k.TimeStampA;
      if (timeStampA < timeStampB) {
        if (timeStampB < t) {
          lastBank += 2;
        }
      } else if (timeStampA >= t) {
        lastBank += 2;
      }
      deltaT = t - *lastBank++;
      rtb_Derivative1_j = (ex33D_pid_B->Sum4 - *lastBank++) / deltaT;
    }
  }

  ex33D_pid_B->SDE001_g = rtb_Derivative1_j * ex33D_pid_P->SDE001_Gain_a;

  if (ssIsSampleHit(S, 1, tid)) {       /* Sample time: [0.001s, 0.0s] */

    rtb_Product_kv[0] = ex33D_pid_B->IW111_n[0] * ex33D_pid_B->SE1_p;
    rtb_Product_kv[1] = ex33D_pid_B->IW111_n[1] * ex33D_pid_B->SDE001_g;

    /* Sum: '<S58>/Sum' */
    rtb_Sum_ho = rtb_Product_kv[0];
    rtb_Sum_ho += rtb_Product_kv[1];

    ex33D_pid_B->IW112_l[0] = ex33D_pid_P->IW112_Value_b[0];
    ex33D_pid_B->IW112_l[1] = ex33D_pid_P->IW112_Value_b[1];

    rtb_Product_og[0] = ex33D_pid_B->IW112_l[0] * ex33D_pid_B->SE1_p;
    rtb_Product_og[1] = ex33D_pid_B->IW112_l[1] * ex33D_pid_B->SDE001_g;

    /* Sum: '<S59>/Sum' */
    rtb_Sum_k = rtb_Product_og[0];
    rtb_Sum_k += rtb_Product_og[1];

    ex33D_pid_B->IW113_n[0] = ex33D_pid_P->IW113_Value_f[0];
    ex33D_pid_B->IW113_n[1] = ex33D_pid_P->IW113_Value_f[1];

    rtb_Product_c[0] = ex33D_pid_B->IW113_n[0] * ex33D_pid_B->SE1_p;
    rtb_Product_c[1] = ex33D_pid_B->IW113_n[1] * ex33D_pid_B->SDE001_g;

    /* Sum: '<S60>/Sum' */
    rtb_Sum_lj = rtb_Product_c[0];
    rtb_Sum_lj += rtb_Product_c[1];

    ex33D_pid_B->IW114_a[0] = ex33D_pid_P->IW114_Value_p[0];
    ex33D_pid_B->IW114_a[1] = ex33D_pid_P->IW114_Value_p[1];

    rtb_Product_fa[0] = ex33D_pid_B->IW114_a[0] * ex33D_pid_B->SE1_p;
    rtb_Product_fa[1] = ex33D_pid_B->IW114_a[1] * ex33D_pid_B->SDE001_g;

    /* Sum: '<S61>/Sum' */
    rtb_Sum_n = rtb_Product_fa[0];
    rtb_Sum_n += rtb_Product_fa[1];

    ex33D_pid_B->IW115_n[0] = ex33D_pid_P->IW115_Value_b[0];
    ex33D_pid_B->IW115_n[1] = ex33D_pid_P->IW115_Value_b[1];

    rtb_Product_e[0] = ex33D_pid_B->IW115_n[0] * ex33D_pid_B->SE1_p;
    rtb_Product_e[1] = ex33D_pid_B->IW115_n[1] * ex33D_pid_B->SDE001_g;

    /* Sum: '<S62>/Sum' */
    rtb_Sum_hw = rtb_Product_e[0];
    rtb_Sum_hw += rtb_Product_e[1];

    {
      real_T cg_in_0_91_0[5];
      int32_T i1;

      for(i1=0; i1<5; i1++) {
        ex33D_pid_B->b1_b[i1] = ex33D_pid_P->b1_Value_o[i1];
      }

      cg_in_0_91_0[0] = rtb_Sum_ho;
      cg_in_0_91_0[1] = rtb_Sum_k;
      cg_in_0_91_0[2] = rtb_Sum_lj;
      cg_in_0_91_0[3] = rtb_Sum_n;
      cg_in_0_91_0[4] = rtb_Sum_hw;
      for(i1=0; i1<5; i1++) {
        rtb_netsum_a[i1] = cg_in_0_91_0[i1] + ex33D_pid_B->b1_b[i1];

        rtb_Product_eu[i1] = ex33D_pid_B->IW211_o[i1] *
          rt_SATURATE(rtb_netsum_a[i1], ex33D_pid_P->Saturation_LowerSat_j,
         ex33D_pid_P->Saturation_UpperSat_e);
      }
    }

    /* Sum: '<S67>/Sum' */
    rtb_Sum_kj = rtb_Product_eu[0];
    rtb_Sum_kj += rtb_Product_eu[1];
    rtb_Sum_kj += rtb_Product_eu[2];
    rtb_Sum_kj += rtb_Product_eu[3];
    rtb_Sum_kj += rtb_Product_eu[4];

    ex33D_pid_B->b2_m = ex33D_pid_P->b2_Value_j;

    ex33D_pid_B->SU1_h = (rtb_Sum_kj + ex33D_pid_B->b2_m) *
      ex33D_pid_P->SU1_Gain_f;
  }

  ex33D_pid_B->Sum10 = ex33D_pid_B->Sum4 + ex33D_pid_B->SU1_h;

  /* Derivative Block: <Root>/Derivative2 */
  {
    real_T t = ssGetTaskTime(S,tid);
    real_T timeStampA = ex33D_pid_DWork->Derivative2_RWORK.TimeStampA;
    real_T timeStampB = ex33D_pid_DWork->Derivative2_RWORK.TimeStampB;

    if (timeStampA >= t && timeStampB >= t) {
      rtb_Derivative2 = 0.0;
    } else {
      real_T deltaT;
      real_T *lastBank = &ex33D_pid_DWork->Derivative2_RWORK.TimeStampA;
      if (timeStampA < timeStampB) {
        if (timeStampB < t) {
          lastBank += 2;
        }
      } else if (timeStampA >= t) {
        lastBank += 2;
      }
      deltaT = t - *lastBank++;
      rtb_Derivative2 = (ex33D_pid_B->Sum10 - *lastBank++) / deltaT;
    }
  }

  ex33D_pid_B->Sum6 = rtb_Gain12 - rtb_TransferFcn4;

  if (ssIsSampleHit(S, 1, tid)) {       /* Sample time: [0.001s, 0.0s] */

    {
      int32_T i1;

      for(i1=0; i1<5; i1++) {
        ex33D_pid_B->IW211_h[i1] = ex33D_pid_P->IW211_Value_o[i1];
      }

      ex33D_pid_B->IW111_id[0] = ex33D_pid_P->IW111_Value_f[0];
      ex33D_pid_B->IW111_id[1] = ex33D_pid_P->IW111_Value_f[1];
    }
  }

  ex33D_pid_B->SE1_l = ex33D_pid_B->Sum6 * ex33D_pid_P->SE1_Gain_b;

  /* Derivative Block: <S4>/Derivative1 */
  {
    real_T t = ssGetTaskTime(S,tid);
    real_T timeStampA = ex33D_pid_DWork->Derivative1_RWORK_j.TimeStampA;
    real_T timeStampB = ex33D_pid_DWork->Derivative1_RWORK_j.TimeStampB;

    if (timeStampA >= t && timeStampB >= t) {
      rtb_Derivative1_jp = 0.0;
    } else {
      real_T deltaT;
      real_T *lastBank = &ex33D_pid_DWork->Derivative1_RWORK_j.TimeStampA;
      if (timeStampA < timeStampB) {
        if (timeStampB < t) {
          lastBank += 2;
        }
      } else if (timeStampA >= t) {
        lastBank += 2;
      }
      deltaT = t - *lastBank++;
      rtb_Derivative1_jp = (ex33D_pid_B->Sum6 - *lastBank++) / deltaT;
    }
  }

  ex33D_pid_B->SDE001_p = rtb_Derivative1_jp * ex33D_pid_P->SDE001_Gain_d;

  if (ssIsSampleHit(S, 1, tid)) {       /* Sample time: [0.001s, 0.0s] */

    rtb_Product_j[0] = ex33D_pid_B->IW111_id[0] * ex33D_pid_B->SE1_l;
    rtb_Product_j[1] = ex33D_pid_B->IW111_id[1] * ex33D_pid_B->SDE001_p;

    /* Sum: '<S79>/Sum' */
    rtb_Sum_k1 = rtb_Product_j[0];
    rtb_Sum_k1 += rtb_Product_j[1];

    ex33D_pid_B->IW112_j[0] = ex33D_pid_P->IW112_Value_f[0];
    ex33D_pid_B->IW112_j[1] = ex33D_pid_P->IW112_Value_f[1];

    rtb_Product_cn[0] = ex33D_pid_B->IW112_j[0] * ex33D_pid_B->SE1_l;
    rtb_Product_cn[1] = ex33D_pid_B->IW112_j[1] * ex33D_pid_B->SDE001_p;

    /* Sum: '<S80>/Sum' */
    rtb_Sum_fo = rtb_Product_cn[0];
    rtb_Sum_fo += rtb_Product_cn[1];

    ex33D_pid_B->IW113_e[0] = ex33D_pid_P->IW113_Value_p[0];
    ex33D_pid_B->IW113_e[1] = ex33D_pid_P->IW113_Value_p[1];

    rtb_Product_n[0] = ex33D_pid_B->IW113_e[0] * ex33D_pid_B->SE1_l;
    rtb_Product_n[1] = ex33D_pid_B->IW113_e[1] * ex33D_pid_B->SDE001_p;

    /* Sum: '<S81>/Sum' */
    rtb_Sum_gl = rtb_Product_n[0];
    rtb_Sum_gl += rtb_Product_n[1];

    ex33D_pid_B->IW114_h[0] = ex33D_pid_P->IW114_Value_i[0];
    ex33D_pid_B->IW114_h[1] = ex33D_pid_P->IW114_Value_i[1];

    rtb_Product_cw[0] = ex33D_pid_B->IW114_h[0] * ex33D_pid_B->SE1_l;
    rtb_Product_cw[1] = ex33D_pid_B->IW114_h[1] * ex33D_pid_B->SDE001_p;

    /* Sum: '<S82>/Sum' */
    rtb_Sum_g2 = rtb_Product_cw[0];
    rtb_Sum_g2 += rtb_Product_cw[1];

    ex33D_pid_B->IW115_m[0] = ex33D_pid_P->IW115_Value_bh[0];
    ex33D_pid_B->IW115_m[1] = ex33D_pid_P->IW115_Value_bh[1];

    rtb_Product_k1[0] = ex33D_pid_B->IW115_m[0] * ex33D_pid_B->SE1_l;
    rtb_Product_k1[1] = ex33D_pid_B->IW115_m[1] * ex33D_pid_B->SDE001_p;

    /* Sum: '<S83>/Sum' */
    rtb_Sum_k2 = rtb_Product_k1[0];
    rtb_Sum_k2 += rtb_Product_k1[1];

    {
      real_T cg_in_0_121_0[5];
      int32_T i1;

      for(i1=0; i1<5; i1++) {
        ex33D_pid_B->b1_n[i1] = ex33D_pid_P->b1_Value_m[i1];
      }

      cg_in_0_121_0[0] = rtb_Sum_k1;
      cg_in_0_121_0[1] = rtb_Sum_fo;
      cg_in_0_121_0[2] = rtb_Sum_gl;
      cg_in_0_121_0[3] = rtb_Sum_g2;
      cg_in_0_121_0[4] = rtb_Sum_k2;
      for(i1=0; i1<5; i1++) {
        rtb_netsum_ge[i1] = cg_in_0_121_0[i1] + ex33D_pid_B->b1_n[i1];

        rtb_Product_l5[i1] = ex33D_pid_B->IW211_h[i1] *
          rt_SATURATE(rtb_netsum_ge[i1], ex33D_pid_P->Saturation_LowerSat_n,
         ex33D_pid_P->Saturation_UpperSat_k);
      }
    }

    /* Sum: '<S88>/Sum' */
    rtb_Sum_h0 = rtb_Product_l5[0];
    rtb_Sum_h0 += rtb_Product_l5[1];
    rtb_Sum_h0 += rtb_Product_l5[2];
    rtb_Sum_h0 += rtb_Product_l5[3];
    rtb_Sum_h0 += rtb_Product_l5[4];

    ex33D_pid_B->b2_l = ex33D_pid_P->b2_Value_h;

    ex33D_pid_B->SU1_k = (rtb_Sum_h0 + ex33D_pid_B->b2_l) *
      ex33D_pid_P->SU1_Gain_f4;
  }

  ex33D_pid_B->Sum11 = ex33D_pid_B->Sum6 + ex33D_pid_B->SU1_k;

  /* Derivative Block: <Root>/Derivative3 */
  {
    real_T t = ssGetTaskTime(S,tid);
    real_T timeStampA = ex33D_pid_DWork->Derivative3_RWORK.TimeStampA;
    real_T timeStampB = ex33D_pid_DWork->Derivative3_RWORK.TimeStampB;

    if (timeStampA >= t && timeStampB >= t) {
      rtb_Derivative3 = 0.0;
    } else {
      real_T deltaT;
      real_T *lastBank = &ex33D_pid_DWork->Derivative3_RWORK.TimeStampA;
      if (timeStampA < timeStampB) {
        if (timeStampB < t) {
          lastBank += 2;
        }
      } else if (timeStampA >= t) {
        lastBank += 2;
      }
      deltaT = t - *lastBank++;
      rtb_Derivative3 = (ex33D_pid_B->Sum11 - *lastBank++) / deltaT;
    }
  }

  /* Integrator: '<Root>/Integrator' */
  rtb_Integrator = ex33D_pid_X->Integrator_CSTATE;

  /* Integrator: '<Root>/Integrator3' */
  rtb_Integrator3 = ex33D_pid_X->Integrator3_CSTATE;

  ex33D_pid_B->Gain11 = ((ex33D_pid_B->Sum11 + rtb_Integrator3 *
    ex33D_pid_P->Gain9_Gain) + (rtb_Derivative3 * ex33D_pid_P->Gain10_Gain)) *
    ex33D_pid_P->Gain11_Gain;

  ex33D_pid_B->Gain2 = ((ex33D_pid_B->Sum8 + (rtb_Integrator *
    ex33D_pid_P->Gain_Gain)) + (rtb_Derivative * ex33D_pid_P->Gain1_Gain)) *
    ex33D_pid_P->Gain2_Gain;

  /* Integrator: '<Root>/Integrator1' */
  rtb_Integrator1 = ex33D_pid_X->Integrator1_CSTATE;

  ex33D_pid_B->Gain5 = ((ex33D_pid_B->Sum9 + rtb_Integrator1 *
    ex33D_pid_P->Gain3_Gain) + rtb_Derivative1_k * ex33D_pid_P->Gain4_Gain) *
    ex33D_pid_P->Gain5_Gain;

  /* Integrator: '<Root>/Integrator2' */
  rtb_Integrator2 = ex33D_pid_X->Integrator2_CSTATE;

  ex33D_pid_B->Gain8 = ((ex33D_pid_B->Sum10 + rtb_Integrator2 *
    ex33D_pid_P->Gain6_Gain) + rtb_Derivative2 * ex33D_pid_P->Gain7_Gain) *
    ex33D_pid_P->Gain8_Gain;

  /* TransportDelay Block: <Root>/Transport Delay1 */
  {

    real_T **uBuffer =
      (real_T**)&ex33D_pid_DWork->TransportDelay1_PWORK.TUbufferPtrs[0];
    real_T **tBuffer =
      (real_T**)&ex33D_pid_DWork->TransportDelay1_PWORK.TUbufferPtrs[1];
    real_T simTime = ssGetT(S);