Beispiel #1
0
void baseRateTask(void *arg)
{
  baseRateInfo_t info = *((baseRateInfo_t *)arg);
  MW_setTaskPeriod(info.period, info.signo);
  while ((rtmGetErrorStatus(motorControlTask_M) == (NULL)) &&
         !rtmGetStopRequested(motorControlTask_M) ) {
    /* Wait for the next timer interrupt */
    if (MW_sigWaitWithOverrunDetection(&info.sigMask) == 1) {
      printf("Overrun - rate for base rate task too fast.\n");
      fflush(stdout);
    }

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModePauseIfNeeded(motorControlTask_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(motorControlTask_M, true);
      }

      if (rtmGetStopRequested(motorControlTask_M) == true) {
        rtmSetErrorStatus(motorControlTask_M, "Simulation finished");
        break;
      }
    }

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModeOneStep(motorControlTask_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(motorControlTask_M, true);
      }
    }

    motorControlTask_output();

    /* Get model outputs here */

    /* External mode */
    rtExtModeUploadCheckTrigger(2);

    {                                  /* Sample time: [0.0s, 0.0s] */
      rtExtModeUpload(0, motorControlTask_M->Timing.t[0]);
    }

    {                                  /* Sample time: [0.02s, 0.0s] */
      rtExtModeUpload(1, ((motorControlTask_M->Timing.clockTick1) * 0.02));
    }

    motorControlTask_update();
    rtExtModeCheckEndTrigger();
  }                                    /* while */

  sem_post(&stopSem);
}
void baseRateTask(void *arg)
{
  baseRateInfo_t info = *((baseRateInfo_t *)arg);
  MW_setTaskPeriod(info.period, info.signo);
  while ((rtmGetErrorStatus(Serial_M) == (NULL)) && !rtmGetStopRequested
         (Serial_M) ) {
    /* Wait for the next timer interrupt */
    MW_sigWait(&info.sigMask);

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModePauseIfNeeded(Serial_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(Serial_M, true);
      }

      if (rtmGetStopRequested(Serial_M) == true) {
        rtmSetErrorStatus(Serial_M, "Simulation finished");
        break;
      }
    }

    /* External mode */
    {
      boolean_T rtmStopReq = false;
      rtExtModeOneStep(Serial_M->extModeInfo, 2, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(Serial_M, true);
      }
    }

    Serial_output();

    /* Get model outputs here */

    /* External mode */
    rtExtModeUploadCheckTrigger(2);

    {                                  /* Sample time: [0.0s, 0.0s] */
      rtExtModeUpload(0, Serial_M->Timing.t[0]);
    }

    {                                  /* Sample time: [0.05s, 0.0s] */
      rtExtModeUpload(1, ((Serial_M->Timing.clockTick1) * 0.05));
    }

    Serial_update();
    rtExtModeCheckEndTrigger();
  }                                    /* while */

  sem_post(&stopSem);
}
static void rtExtModeSingleTaskUpload(RT_MODEL * S)
{
  int stIdx;
  rtExtModeUploadCheckTrigger(rtmGetNumSampleTimes(S));
  for (stIdx=0; stIdx < 2; stIdx++) {
    if (rtmIsSampleHit(S, stIdx, 0     /*unused*/
                       )) {
      rtExtModeUpload(stIdx,rtmGetTaskTime(S,stIdx));
    }
  }
}
Beispiel #4
0
void baseRateTask(void *arg)
{
  baseRateInfo_t info = *((baseRateInfo_t *)arg);
  MW_setTaskPeriod(info.period, info.signo);
  while ((rtmGetErrorStatus(raspberrypi_audioequalizer_M) == (NULL)) &&
         !rtmGetStopRequested(raspberrypi_audioequalizer_M) ) {
    /* Wait for the next timer interrupt */
    MW_sigWait(&info.sigMask);

    /* External mode */
    {
      boolean_T rtmStopReq = FALSE;
      rtExtModePauseIfNeeded(raspberrypi_audioequalizer_M->extModeInfo, 1,
        &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(raspberrypi_audioequalizer_M, TRUE);
      }

      if (rtmGetStopRequested(raspberrypi_audioequalizer_M) == TRUE) {
        rtmSetErrorStatus(raspberrypi_audioequalizer_M, "Simulation finished");
        break;
      }
    }

    /* External mode */
    {
      boolean_T rtmStopReq = FALSE;
      rtExtModeOneStep(raspberrypi_audioequalizer_M->extModeInfo, 1, &rtmStopReq);
      if (rtmStopReq) {
        rtmSetStopRequested(raspberrypi_audioequalizer_M, TRUE);
      }
    }

    raspberrypi_audioequalizer_output();

    /* Get model outputs here */

    /* External mode */
    rtExtModeUploadCheckTrigger(1);

    {                                  /* Sample time: [0.1s, 0.0s] */
      rtExtModeUpload(0, raspberrypi_audioequalizer_M->Timing.taskTime0);
    }

    raspberrypi_audioequalizer_update();
    rtExtModeCheckEndTrigger();
  }                                    /* while */

  sem_post(&stopSem);
}
Beispiel #5
0
/* Model step function */
void motor_hr_step(void)
{
  real_T tmp;
  uint8_T tmp_0;

  /* Switch: '<Root>/Switch' incorporates:
   *  Constant: '<Root>/Constant2'
   *  Inport: '<Root>/direction'
   *  Inport: '<Root>/enable'
   */
  if (motor_hr_U.enable > motor_hr_P.Switch_Threshold) {
    tmp = motor_hr_U.direction;
  } else {
    tmp = motor_hr_P.Constant2_Value;
  }

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

  /* DataTypeConversion: '<S1>/Data Type Conversion' */
  if (tmp < 256.0) {
    if (tmp >= 0.0) {
      tmp_0 = (uint8_T)tmp;
    } else {
      tmp_0 = 0U;
    }
  } else {
    tmp_0 = MAX_uint8_T;
  }

  /* End of DataTypeConversion: '<S1>/Data Type Conversion' */

  /* S-Function (arduinodigitaloutput_sfcn): '<S1>/Digital Output' */
  MW_digitalWrite(motor_hr_P.DigitalOutput_pinNumber, tmp_0);

  /* Switch: '<Root>/Switch1' incorporates:
   *  Constant: '<Root>/Constant3'
   *  Inport: '<Root>/enable'
   *  Inport: '<Root>/power'
   */
  if (motor_hr_U.enable > motor_hr_P.Switch1_Threshold) {
    tmp = motor_hr_U.power;
  } else {
    tmp = motor_hr_P.Constant3_Value;
  }

  /* End of Switch: '<Root>/Switch1' */

  /* DataTypeConversion: '<S2>/Data Type Conversion' */
  if (tmp < 256.0) {
    if (tmp >= 0.0) {
      tmp_0 = (uint8_T)tmp;
    } else {
      tmp_0 = 0U;
    }
  } else {
    tmp_0 = MAX_uint8_T;
  }

  /* End of DataTypeConversion: '<S2>/Data Type Conversion' */

  /* S-Function (arduinoanalogoutput_sfcn): '<S2>/PWM' */
  MW_analogWrite(motor_hr_P.PWM_pinNumber, tmp_0);

  /* External mode */
  rtExtModeUploadCheckTrigger(1);

  {                                    /* Sample time: [20.0s, 0.0s] */
    rtExtModeUpload(0, motor_hr_M->Timing.taskTime0);
  }

  /* signal main to stop simulation */
  {                                    /* Sample time: [20.0s, 0.0s] */
    if ((rtmGetTFinal(motor_hr_M)!=-1) &&
        !((rtmGetTFinal(motor_hr_M)-motor_hr_M->Timing.taskTime0) >
          motor_hr_M->Timing.taskTime0 * (DBL_EPSILON))) {
      rtmSetErrorStatus(motor_hr_M, "Simulation finished");
    }

    if (rtmGetStopRequested(motor_hr_M)) {
      rtmSetErrorStatus(motor_hr_M, "Simulation finished");
    }
  }

  /* 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.
   */
  motor_hr_M->Timing.taskTime0 =
    (++motor_hr_M->Timing.clockTick0) * motor_hr_M->Timing.stepSize0;
}
Beispiel #6
0
void rt_OneStep(void)
{
  boolean_T eventFlags[2];

  /* Check base rate for overrun */
  if (isRateRunning[0]++) {
    IsrOverrun = 1;
    isRateRunning[0]--;                /* allow future iterations to succeed*/
    return;
  }

  sei();

  /*
   * For a bare-board target (i.e., no operating system), the
   * following code checks whether any subrate overruns,
   * and also sets the rates that need to run this time step.
   */
  velo_id_gain_output();

  /* Get model outputs here */

  /* External mode */
  rtExtModeUploadCheckTrigger(2);

  {                                    /* Sample time: [0.0s, 0.0s] */
    rtExtModeUpload(0, velo_id_gain_M->Timing.t[0]);
  }

  {                                    /* Sample time: [0.01s, 0.0s] */
    rtExtModeUpload(1, ((velo_id_gain_M->Timing.clockTick1) * 0.01));
  }

  velo_id_gain_update();
  cli();
  isRateRunning[0]--;
  if (eventFlags[1]) {
    if (need2runFlags[1]++) {
      IsrOverrun = 1;
      need2runFlags[1]--;              /* allow future iterations to succeed*/
      return;
    }
  }

  if (need2runFlags[1]) {
    if (isRateRunning[1]) {
      /* Yield to higher priority*/
      return;
    }

    isRateRunning[1]++;
    sei();
    switch (1) {
     default:
      break;
    }

    cli();
    need2runFlags[1]--;
    isRateRunning[1]--;
  }
}
Beispiel #7
0
/* Model step function */
void Motor_Test_All_step(void)
{
  /* local block i/o variables */
  real_T rtb_Output;
  uint8_T rtb_FixPtSum1;

  /* MultiPortSwitch: '<S1>/Output' incorporates:
   *  Constant: '<S1>/Vector'
   *  UnitDelay: '<S2>/Output'
   */
  rtb_Output =
    Motor_Test_All_P.RepeatingSequenceStair_OutValue[Motor_Test_All_DW.Output_DSTATE];

  /* ModelReference: '<Root>/Model' */
  motor_hl(&Motor_Test_All_P.enable_Value, &rtb_Output,
           &Motor_Test_All_P.enable_Value);

  /* ModelReference: '<Root>/Model1' */
  motor_vr(&Motor_Test_All_P.enable_Value, &rtb_Output,
           &Motor_Test_All_P.enable_Value);

  /* ModelReference: '<Root>/Model2' */
  motor_vl(&Motor_Test_All_P.enable_Value, &rtb_Output,
           &Motor_Test_All_P.enable_Value);

  /* ModelReference: '<Root>/Model3' */
  motor_hr(&Motor_Test_All_P.enable_Value, &rtb_Output,
           &Motor_Test_All_P.enable_Value);

  /* Sum: '<S3>/FixPt Sum1' incorporates:
   *  Constant: '<S3>/FixPt Constant'
   *  UnitDelay: '<S2>/Output'
   */
  rtb_FixPtSum1 = (uint8_T)((uint16_T)Motor_Test_All_DW.Output_DSTATE +
    Motor_Test_All_P.FixPtConstant_Value);

  /* Switch: '<S4>/FixPt Switch' */
  if (rtb_FixPtSum1 > Motor_Test_All_P.LimitedCounter_uplimit) {
    /* Update for UnitDelay: '<S2>/Output' incorporates:
     *  Constant: '<S4>/Constant'
     */
    Motor_Test_All_DW.Output_DSTATE = Motor_Test_All_P.Constant_Value;
  } else {
    /* Update for UnitDelay: '<S2>/Output' */
    Motor_Test_All_DW.Output_DSTATE = rtb_FixPtSum1;
  }

  /* End of Switch: '<S4>/FixPt Switch' */

  /* External mode */
  rtExtModeUploadCheckTrigger(1);

  {                                    /* Sample time: [10.0s, 0.0s] */
    rtExtModeUpload(0, Motor_Test_All_M->Timing.taskTime0);
  }

  /* signal main to stop simulation */
  {                                    /* Sample time: [10.0s, 0.0s] */
    if ((rtmGetTFinal(Motor_Test_All_M)!=-1) &&
        !((rtmGetTFinal(Motor_Test_All_M)-Motor_Test_All_M->Timing.taskTime0) >
          Motor_Test_All_M->Timing.taskTime0 * (DBL_EPSILON))) {
      rtmSetErrorStatus(Motor_Test_All_M, "Simulation finished");
    }

    if (rtmGetStopRequested(Motor_Test_All_M)) {
      rtmSetErrorStatus(Motor_Test_All_M, "Simulation finished");
    }
  }

  /* 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.
   */
  Motor_Test_All_M->Timing.taskTime0 =
    (++Motor_Test_All_M->Timing.clockTick0) * Motor_Test_All_M->Timing.stepSize0;
}
/* Model step function */
void GyroskopAuswertung_step(void)
{
  /* S-Function (MPU6050_CustomBlock): '<Root>/Sensor1' */
  MPU6050_CustomBlock_Outputs_wrapper( &GyroskopAuswertung_B.Sensor1_o1,
    &GyroskopAuswertung_B.Sensor1_o2, &GyroskopAuswertung_B.Sensor1_o3,
    &GyroskopAuswertung_B.Sensor1_o4, &GyroskopAuswertung_B.Sensor1_o5,
    &GyroskopAuswertung_B.Sensor1_o6, &GyroskopAuswertung_DW.Sensor1_DSTATE,
    &GyroskopAuswertung_P.Sensor1_P1, 1, &GyroskopAuswertung_P.Sensor1_P2, 1);

  /* S-Function (MPU6050_CustomBlock): '<Root>/Sensor2' */
  MPU6050_CustomBlock_Outputs_wrapper( &GyroskopAuswertung_B.Sensor2_o1,
    &GyroskopAuswertung_B.Sensor2_o2, &GyroskopAuswertung_B.Sensor2_o3,
    &GyroskopAuswertung_B.Sensor2_o4, &GyroskopAuswertung_B.Sensor2_o5,
    &GyroskopAuswertung_B.Sensor2_o6, &GyroskopAuswertung_DW.Sensor2_DSTATE,
    &GyroskopAuswertung_P.Sensor2_P1, 1, &GyroskopAuswertung_P.Sensor2_P2, 1);

  /* S-Function "MPU6050_CustomBlock_wrapper" Block: <Root>/Sensor1 */
  MPU6050_CustomBlock_Update_wrapper( &GyroskopAuswertung_B.Sensor1_o1,
    &GyroskopAuswertung_B.Sensor1_o2, &GyroskopAuswertung_B.Sensor1_o3,
    &GyroskopAuswertung_B.Sensor1_o4, &GyroskopAuswertung_B.Sensor1_o5,
    &GyroskopAuswertung_B.Sensor1_o6, &GyroskopAuswertung_DW.Sensor1_DSTATE,
    &GyroskopAuswertung_P.Sensor1_P1, 1, &GyroskopAuswertung_P.Sensor1_P2, 1);

  /* S-Function "MPU6050_CustomBlock_wrapper" Block: <Root>/Sensor2 */
  MPU6050_CustomBlock_Update_wrapper( &GyroskopAuswertung_B.Sensor2_o1,
    &GyroskopAuswertung_B.Sensor2_o2, &GyroskopAuswertung_B.Sensor2_o3,
    &GyroskopAuswertung_B.Sensor2_o4, &GyroskopAuswertung_B.Sensor2_o5,
    &GyroskopAuswertung_B.Sensor2_o6, &GyroskopAuswertung_DW.Sensor2_DSTATE,
    &GyroskopAuswertung_P.Sensor2_P1, 1, &GyroskopAuswertung_P.Sensor2_P2, 1);

  /* External mode */
  rtExtModeUploadCheckTrigger(1);

  {                                    /* Sample time: [0.01s, 0.0s] */
    rtExtModeUpload(0, GyroskopAuswertung_M->Timing.taskTime0);
  }

  /* signal main to stop simulation */
  {                                    /* Sample time: [0.01s, 0.0s] */
    if ((rtmGetTFinal(GyroskopAuswertung_M)!=-1) &&
        !((rtmGetTFinal(GyroskopAuswertung_M)-
           GyroskopAuswertung_M->Timing.taskTime0) >
          GyroskopAuswertung_M->Timing.taskTime0 * (DBL_EPSILON))) {
      rtmSetErrorStatus(GyroskopAuswertung_M, "Simulation finished");
    }

    if (rtmGetStopRequested(GyroskopAuswertung_M)) {
      rtmSetErrorStatus(GyroskopAuswertung_M, "Simulation finished");
    }
  }

  /* 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 (!(++GyroskopAuswertung_M->Timing.clockTick0)) {
    ++GyroskopAuswertung_M->Timing.clockTickH0;
  }

  GyroskopAuswertung_M->Timing.taskTime0 =
    GyroskopAuswertung_M->Timing.clockTick0 *
    GyroskopAuswertung_M->Timing.stepSize0 +
    GyroskopAuswertung_M->Timing.clockTickH0 *
    GyroskopAuswertung_M->Timing.stepSize0 * 4294967296.0;
}
Beispiel #9
0
/* Function: rsimOneStepMT =====================================================
 *
 *      Perform one step of the model.
 *      Errors are set in the SimStruct's ErrorStatus, NULL means no errors.
 */
static void rsimOneStepMT(SimStruct *S)
{
    int_T  i;
    const int_T FIRST_TID = gbl_raccel_tid01eq ? 1 : 0;
    int_T* sampleHit = ssGetSampleHitPtr(S);

    ssSetSimTimeStep(S, MAJOR_TIME_STEP);
    
    /* Clear the flag that blocks set in their major time step methods (output,
       update, etc.) to flag that they have changed their state. If this flag is
       true then we need to run model outputs again at the first minor step
       (i.e., at the same time as the last major step).*/
    
    ssClearBlkStateChange(S);

#ifdef DEBUG_TIMING
    rsimDisplayTimingData(S,
                          sizeof(struct SimStruct_tag),
                          sizeof(struct _ssMdlInfo));
#endif
    if (gblExtModeEnabled) {
        rtExtModeOneStep(ssGetRTWExtModeInfo(S),
                         ssGetNumSampleTimes(S),
                         (boolean_T*)&ssGetStopRequested(S));
        if (ssGetStopRequested(S)) return;
    }
    /* Setup the task times and sample hit flags for the discrete rates */
    rsimUpdateDiscreteTaskTimesAndSampleHits(S);
    if (ssGetErrorStatus(S) != NULL) return;

    ssSetLogOutput(S,TRUE);

    /* Do output, log, update for the base rate */
    MdlOutputs(FIRST_TID);

    if (gblExtModeEnabled) {
        rtExtModeUploadCheckTrigger(ssGetNumSampleTimes(S));
        rtExtModeUpload(FIRST_TID, ssGetTaskTime(S, FIRST_TID));
    }

    if (ssGetLogOutput(S)) {
        (void)rt_UpdateTXYLogVars(ssGetRTWLogInfo(S), ssGetTPtr(S));
        if (ssGetErrorStatus(S) != NULL) return;
    }

    MdlUpdate(FIRST_TID);
    if (ssGetErrorStatus(S) != NULL) return;

    /* Do not log outputs during minor time steps */
    ssSetLogOutput(S, FALSE);
    ssSetTimeOfLastOutput(S,ssGetT(S));

    /* Call the solver push time forward based on the continuous dynamics */
    if (ssGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME &&
        !ssGetStopRequested(S) ) {
        rsimUpdateSolverStopTimeForFixedStepMultiTaskingSim(S);
        if (!ssGetStopRequested(S)) rsimAdvanceSolver(S);
    }

    /* Do output and update for the remaining rates */
    ssSetLogOutput(S, TRUE);
    for (i = FIRST_TID+1; i < gbl_raccel_NumST; i++) {
        if ( !sampleHit[i] ) continue;
        MdlOutputs(i);
        rtExtModeUpload(i, ssGetTaskTime(S,i));
        MdlUpdate(i);
    }
    if (ssGetErrorStatus(S) != NULL) return;
    ssSetLogOutput(S, FALSE);

    if (gblExtModeEnabled) {
        rtExtModeCheckEndTrigger();
    }

    /* Update the timing engine and determine the solver stop time */
    rsimUpdateTimingEngineAndSolverStopTime(S);
    if (ssGetErrorStatus(S) != NULL) return;

} /* rsimOneStepMT */
Beispiel #10
0
/* Model step function */
void motor_control_step(void)
{
  real_T rtb_PulseGenerator;
  real32_T rtb_Sum1;
  real32_T rtb_FilterCoefficient;

  /* DiscretePulseGenerator: '<Root>/Pulse Generator' */
  rtb_PulseGenerator = (motor_control_DW.clockTickCounter <
                        motor_control_P.PulseGenerator_Duty) &&
    (motor_control_DW.clockTickCounter >= 0L) ?
    motor_control_P.PulseGenerator_Amp : 0.0;
  if (motor_control_DW.clockTickCounter >= motor_control_P.PulseGenerator_Period
      - 1.0) {
    motor_control_DW.clockTickCounter = 0L;
  } else {
    motor_control_DW.clockTickCounter++;
  }

  /* End of DiscretePulseGenerator: '<Root>/Pulse Generator' */

  /* Sum: '<Root>/Sum2' incorporates:
   *  Constant: '<Root>/ref'
   */
  motor_control_B.Sum2 = (real32_T)(motor_control_P.ref_Value -
    rtb_PulseGenerator);

  /* S-Function (Encoder_read): '<Root>/Encoder' */
  Encoder_read_Outputs_wrapper( &motor_control_B.Encoder,
    &motor_control_DW.Encoder_DSTATE);

  /* Sum: '<Root>/Sum1' */
  rtb_Sum1 = motor_control_B.Sum2 - motor_control_B.Encoder;

  /* Gain: '<S1>/Filter Coefficient' incorporates:
   *  DiscreteIntegrator: '<S1>/Filter'
   *  Gain: '<S1>/Derivative Gain'
   *  Sum: '<S1>/SumD'
   */
  rtb_FilterCoefficient = (motor_control_P.DiscretePIDController_D * rtb_Sum1 -
    motor_control_DW.Filter_DSTATE) * motor_control_P.DiscretePIDController_N;

  /* Sum: '<S1>/Sum' incorporates:
   *  DiscreteIntegrator: '<S1>/Integrator'
   *  Gain: '<S1>/Proportional Gain'
   */
  motor_control_B.Sum = (motor_control_P.DiscretePIDController_P * rtb_Sum1 +
    motor_control_DW.Integrator_DSTATE) + rtb_FilterCoefficient;

  /* Sum: '<Root>/Sum' incorporates:
   *  Constant: '<Root>/Constant'
   */
  motor_control_B.Sum_e = motor_control_B.Sum + motor_control_P.Constant_Value;

  /* S-Function (PWM_init): '<Root>/PWM' */
  PWM_init_Outputs_wrapper(&motor_control_B.Sum_e, &motor_control_DW.PWM_DSTATE);

  /* S-Function "Encoder_read_wrapper" Block: <Root>/Encoder */
  Encoder_read_Update_wrapper( &motor_control_B.Encoder,
    &motor_control_DW.Encoder_DSTATE);

  /* Update for DiscreteIntegrator: '<S1>/Integrator' incorporates:
   *  Gain: '<S1>/Integral Gain'
   */
  motor_control_DW.Integrator_DSTATE += motor_control_P.DiscretePIDController_I *
    rtb_Sum1 * motor_control_P.Integrator_gainval;

  /* Update for DiscreteIntegrator: '<S1>/Filter' */
  motor_control_DW.Filter_DSTATE += motor_control_P.Filter_gainval *
    rtb_FilterCoefficient;

  /* S-Function "PWM_init_wrapper" Block: <Root>/PWM */
  PWM_init_Update_wrapper(&motor_control_B.Sum_e, &motor_control_DW.PWM_DSTATE);

  /* External mode */
  rtExtModeUploadCheckTrigger(1);

  {                                    /* Sample time: [0.05s, 0.0s] */
    rtExtModeUpload(0, motor_control_M->Timing.taskTime0);
  }

  /* signal main to stop simulation */
  {                                    /* Sample time: [0.05s, 0.0s] */
    if ((rtmGetTFinal(motor_control_M)!=-1) &&
        !((rtmGetTFinal(motor_control_M)-motor_control_M->Timing.taskTime0) >
          motor_control_M->Timing.taskTime0 * (DBL_EPSILON))) {
      rtmSetErrorStatus(motor_control_M, "Simulation finished");
    }

    if (rtmGetStopRequested(motor_control_M)) {
      rtmSetErrorStatus(motor_control_M, "Simulation finished");
    }
  }

  /* 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.
   */
  motor_control_M->Timing.taskTime0 =
    (++motor_control_M->Timing.clockTick0) * motor_control_M->Timing.stepSize0;
}
/* Model step function */
void motor_test_hierarchies_step(void)
{
  /* local block i/o variables */
  real_T rtb_FromWs;
  real_T tmp;
  uint8_T tmp_0;

  /* Constant: '<Root>/enable' */
  motor_test_hierarchies_B.enable = motor_test_hierarchies_P.enable_Value;

  /* FromWorkspace: '<S3>/FromWs' */
  {
    real_T *pDataValues = (real_T *)
      motor_test_hierarchies_DW.FromWs_PWORK.DataPtr;
    real_T *pTimeValues = (real_T *)
      motor_test_hierarchies_DW.FromWs_PWORK.TimePtr;
    int_T currTimeIndex = motor_test_hierarchies_DW.FromWs_IWORK.PrevIndex;
    real_T t = motor_test_hierarchies_M->Timing.t[0];

    /* Get index */
    if (t <= pTimeValues[0]) {
      currTimeIndex = 0;
    } else if (t >= pTimeValues[9]) {
      currTimeIndex = 8;
    } else {
      if (t < pTimeValues[currTimeIndex]) {
        while (t < pTimeValues[currTimeIndex]) {
          currTimeIndex--;
        }
      } else {
        while (t >= pTimeValues[currTimeIndex + 1]) {
          currTimeIndex++;
        }
      }
    }

    motor_test_hierarchies_DW.FromWs_IWORK.PrevIndex = currTimeIndex;

    /* Post output */
    {
      real_T t1 = pTimeValues[currTimeIndex];
      real_T t2 = pTimeValues[currTimeIndex + 1];
      if (t1 == t2) {
        if (t < t1) {
          rtb_FromWs = pDataValues[currTimeIndex];
        } else {
          rtb_FromWs = pDataValues[currTimeIndex + 1];
        }
      } else {
        real_T f1 = (t2 - t) / (t2 - t1);
        real_T f2 = 1.0 - f1;
        real_T d1;
        real_T d2;
        int_T TimeIndex= currTimeIndex;
        d1 = pDataValues[TimeIndex];
        d2 = pDataValues[TimeIndex + 1];
        rtb_FromWs = (real_T) rtInterpolate(d1, d2, f1, f2);
        pDataValues += 10;
      }
    }
  }

  /* Outputs for Atomic SubSystem: '<S1>/hl' */

  /* Constant: '<Root>/direction' */
  motor_test_hierarchies_hl(motor_test_hierarchies_B.enable, rtb_FromWs,
    motor_test_hierarchies_P.direction_Value, &motor_test_hierarchies_B.hl,
    (P_hl_motor_test_hierarchies_T *)&motor_test_hierarchies_P.hl);

  /* End of Outputs for SubSystem: '<S1>/hl' */

  /* Outputs for Atomic SubSystem: '<S1>/hr' */
  /* Switch: '<S5>/Switch' incorporates:
   *  Constant: '<Root>/direction'
   *  Constant: '<S5>/Constant2'
   */
  if (motor_test_hierarchies_B.enable >
      motor_test_hierarchies_P.Switch_Threshold) {
    tmp = motor_test_hierarchies_P.direction_Value;
  } else {
    tmp = motor_test_hierarchies_P.Constant2_Value;
  }

  /* End of Switch: '<S5>/Switch' */

  /* DataTypeConversion: '<S10>/Data Type Conversion' */
  if (tmp < 256.0) {
    if (tmp >= 0.0) {
      tmp_0 = (uint8_T)tmp;
    } else {
      tmp_0 = 0U;
    }
  } else {
    tmp_0 = MAX_uint8_T;
  }

  /* End of DataTypeConversion: '<S10>/Data Type Conversion' */

  /* S-Function (arduinodigitaloutput_sfcn): '<S10>/Digital Output' */
  MW_digitalWrite(motor_test_hierarchies_P.DigitalOutput_pinNumber, tmp_0);

  /* Switch: '<S5>/Switch1' incorporates:
   *  Constant: '<S5>/Constant3'
   *  DataTypeConversion: '<S11>/Data Type Conversion'
   */
  if (motor_test_hierarchies_B.enable >
      motor_test_hierarchies_P.Switch1_Threshold) {
    /* DataTypeConversion: '<S11>/Data Type Conversion' */
    if (rtb_FromWs < 256.0) {
      if (rtb_FromWs >= 0.0) {
        motor_test_hierarchies_B.DataTypeConversion_b = (uint8_T)rtb_FromWs;
      } else {
        motor_test_hierarchies_B.DataTypeConversion_b = 0U;
      }
    } else {
      motor_test_hierarchies_B.DataTypeConversion_b = MAX_uint8_T;
    }
  } else if (motor_test_hierarchies_P.Constant3_Value < 256.0) {
    /* DataTypeConversion: '<S11>/Data Type Conversion' incorporates:
     *  Constant: '<S5>/Constant3'
     */
    if (motor_test_hierarchies_P.Constant3_Value >= 0.0) {
      motor_test_hierarchies_B.DataTypeConversion_b = (uint8_T)
        motor_test_hierarchies_P.Constant3_Value;
    } else {
      motor_test_hierarchies_B.DataTypeConversion_b = 0U;
    }
  } else {
    /* DataTypeConversion: '<S11>/Data Type Conversion' */
    motor_test_hierarchies_B.DataTypeConversion_b = MAX_uint8_T;
  }

  /* End of Switch: '<S5>/Switch1' */

  /* S-Function (arduinoanalogoutput_sfcn): '<S11>/PWM' */
  MW_analogWrite(motor_test_hierarchies_P.PWM_pinNumber,
                 motor_test_hierarchies_B.DataTypeConversion_b);

  /* Outputs for Atomic SubSystem: '<S1>/vl' */

  /* Constant: '<Root>/direction' */
  motor_test_hierarchies_hl(motor_test_hierarchies_B.enable, rtb_FromWs,
    motor_test_hierarchies_P.direction_Value, &motor_test_hierarchies_B.vl,
    (P_hl_motor_test_hierarchies_T *)&motor_test_hierarchies_P.vl);

  /* End of Outputs for SubSystem: '<S1>/vl' */

  /* Outputs for Atomic SubSystem: '<S1>/vr' */
  /* Switch: '<S7>/Switch' incorporates:
   *  Constant: '<Root>/direction'
   *  Constant: '<S7>/Constant2'
   */
  if (motor_test_hierarchies_B.enable >
      motor_test_hierarchies_P.Switch_Threshold_k) {
    tmp = motor_test_hierarchies_P.direction_Value;
  } else {
    tmp = motor_test_hierarchies_P.Constant2_Value_n;
  }

  /* End of Switch: '<S7>/Switch' */

  /* DataTypeConversion: '<S14>/Data Type Conversion' */
  if (tmp < 256.0) {
    if (tmp >= 0.0) {
      tmp_0 = (uint8_T)tmp;
    } else {
      tmp_0 = 0U;
    }
  } else {
    tmp_0 = MAX_uint8_T;
  }

  /* End of DataTypeConversion: '<S14>/Data Type Conversion' */

  /* S-Function (arduinodigitaloutput_sfcn): '<S14>/Digital Output' */
  MW_digitalWrite(motor_test_hierarchies_P.DigitalOutput_pinNumber_a, tmp_0);

  /* Switch: '<S7>/Switch1' incorporates:
   *  Constant: '<S7>/Constant3'
   *  DataTypeConversion: '<S15>/Data Type Conversion'
   */
  if (motor_test_hierarchies_B.enable >
      motor_test_hierarchies_P.Switch1_Threshold_o) {
    /* DataTypeConversion: '<S15>/Data Type Conversion' */
    if (rtb_FromWs < 256.0) {
      if (rtb_FromWs >= 0.0) {
        motor_test_hierarchies_B.DataTypeConversion = (uint8_T)rtb_FromWs;
      } else {
        motor_test_hierarchies_B.DataTypeConversion = 0U;
      }
    } else {
      motor_test_hierarchies_B.DataTypeConversion = MAX_uint8_T;
    }
  } else if (motor_test_hierarchies_P.Constant3_Value_f < 256.0) {
    /* DataTypeConversion: '<S15>/Data Type Conversion' incorporates:
     *  Constant: '<S7>/Constant3'
     */
    if (motor_test_hierarchies_P.Constant3_Value_f >= 0.0) {
      motor_test_hierarchies_B.DataTypeConversion = (uint8_T)
        motor_test_hierarchies_P.Constant3_Value_f;
    } else {
      motor_test_hierarchies_B.DataTypeConversion = 0U;
    }
  } else {
    /* DataTypeConversion: '<S15>/Data Type Conversion' */
    motor_test_hierarchies_B.DataTypeConversion = MAX_uint8_T;
  }

  /* End of Switch: '<S7>/Switch1' */

  /* S-Function (arduinoanalogoutput_sfcn): '<S15>/PWM' */
  MW_analogWrite(motor_test_hierarchies_P.PWM_pinNumber_n,
                 motor_test_hierarchies_B.DataTypeConversion);

  /* External mode */
  rtExtModeUploadCheckTrigger(2);

  {                                    /* Sample time: [0.0s, 0.0s] */
    rtExtModeUpload(0, motor_test_hierarchies_M->Timing.t[0]);
  }

  {                                    /* Sample time: [1.0s, 0.0s] */
    rtExtModeUpload(1, ((motor_test_hierarchies_M->Timing.clockTick1) ));
  }

  /* signal main to stop simulation */
  {                                    /* Sample time: [0.0s, 0.0s] */
    if ((rtmGetTFinal(motor_test_hierarchies_M)!=-1) &&
        !((rtmGetTFinal(motor_test_hierarchies_M)-
           motor_test_hierarchies_M->Timing.t[0]) >
          motor_test_hierarchies_M->Timing.t[0] * (DBL_EPSILON))) {
      rtmSetErrorStatus(motor_test_hierarchies_M, "Simulation finished");
    }

    if (rtmGetStopRequested(motor_test_hierarchies_M)) {
      rtmSetErrorStatus(motor_test_hierarchies_M, "Simulation finished");
    }
  }

  /* 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.
   */
  motor_test_hierarchies_M->Timing.t[0] =
    (++motor_test_hierarchies_M->Timing.clockTick0) *
    motor_test_hierarchies_M->Timing.stepSize0;

  {
    /* Update absolute timer for sample time: [1.0s, 0.0s] */
    /* The "clockTick1" counts the number of times the code of this task has
     * been executed. The resolution of this integer timer is 1.0, which is the step size
     * of the task. Size of "clockTick1" ensures timer will not overflow during the
     * application lifespan selected.
     */
    motor_test_hierarchies_M->Timing.clockTick1++;
  }
}
Beispiel #12
0
/* Function: rtOneStep ========================================================
 *
 * Abstract:
 *      Perform one step of the model. This function is modeled such that
 *      it could be called from an interrupt service routine (ISR) with minor
 *      modifications.
 *
 *      This routine is modeled for use in a multitasking environment and
 *	therefore needs to be fully re-entrant when it is called from an
 *	interrupt service routine.
 *
 * Note:
 *      Error checking is provided which will only be used if this routine
 *      is attached to an interrupt.
 *
 */
static void rt_OneStep(RT_MODEL *S)
{
    int_T  i;
    real_T tnext;
    int_T  *sampleHit = rtmGetSampleHitPtr(S);

    /***********************************************
     * Check and see if base step time is too fast *
     ***********************************************/
    if (GBLbuf.isrOverrun++) {
        GBLbuf.stopExecutionFlag = 1;
        return;
    }

    /***********************************************
     * Check and see if error status has been set  *
     ***********************************************/
    if (rtmGetErrorStatus(S) != NULL) {
        GBLbuf.stopExecutionFlag = 1;
        return;
    }
    /* enable interrupts here */

    /*
     * In a multi-tasking environment, this would be removed from the base rate
     * and called as a "background" task.
     */
    rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
                     rtmGetNumSampleTimes(S),
                     (boolean_T *)&rtmGetStopRequested(S));

    /***********************************************
     * Update discrete events                      *
     ***********************************************/
    tnext = rt_SimUpdateDiscreteEvents(rtmGetNumSampleTimes(S),
                                       rtmGetTimingData(S),
                                       rtmGetSampleHitPtr(S),
                                       rtmGetPerTaskSampleHitsPtr(S));
    rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
    for (i=FIRST_TID+1; i < NUMST; i++) {
        if (sampleHit[i] && GBLbuf.eventFlags[i]++) {
            GBLbuf.isrOverrun--; 
            GBLbuf.overrunFlags[i]++;    /* Are we sampling too fast for */
            GBLbuf.stopExecutionFlag=1;  /*   sample time "i"?           */
            return;
        }
    }
    /*******************************************
     * Step the model for the base sample time *
     *******************************************/
    MdlOutputs(FIRST_TID);

    rtExtModeUploadCheckTrigger(rtmGetNumSampleTimes(S));
    rtExtModeUpload(FIRST_TID,rtmGetTaskTime(S, FIRST_TID));

   /* GBLbuf.errmsg = rt_UpdateTXYLogVars(rtmGetRTWLogInfo(S),
                                        rtmGetTPtr(S));
    if (GBLbuf.errmsg != NULL) {
        GBLbuf.stopExecutionFlag = 1;
        return;
    }*/

   /* rt_UpdateSigLogVars(rtmGetRTWLogInfo(S), rtmGetTPtr(S));*/

    MdlUpdate(FIRST_TID);

    if (rtmGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
        rt_UpdateContinuousStates(S);
    }
     else {
        rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(S), 
                                     rtmGetTimingData(S), 0);
    }

#if FIRST_TID == 1
    rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(S), 
                                 rtmGetTimingData(S),1);
#endif


    /************************************************************************
     * Model step complete for base sample time, now it is okay to          *
     * re-interrupt this ISR.                                               *
     ************************************************************************/

    GBLbuf.isrOverrun--;


    /*********************************************
     * Step the model for any other sample times *
     *********************************************/
    for (i=FIRST_TID+1; i<NUMST; i++) {
        /* If task "i" is running, don't run any lower priority task */
        if (GBLbuf.overrunFlags[i]) return;

        if (GBLbuf.eventFlags[i]) {
            GBLbuf.overrunFlags[i]++;

            MdlOutputs(i);
 
            rtExtModeUpload(i, rtmGetTaskTime(S,i));

            MdlUpdate(i);

            rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(S), 
                                         rtmGetTimingData(S),i);

            /* Indicate task complete for sample time "i" */
            GBLbuf.overrunFlags[i]--;
            GBLbuf.eventFlags[i]--;
        }
    }

    rtExtModeCheckEndTrigger();

} /* end rtOneStep */