/*
 * File: NXTway_ET2012.c
 *
 * Code generated for Simulink model 'NXTway_ET2012'.
 *
 * Model version                  : 1.617
 * Simulink Coder version         : 8.2 (R2012a) 29-Dec-2011
 * TLC version                    : 8.2 (Dec 29 2011)
 * C/C++ source code generated on : Mon Apr 16 09:34:27 2012
 *
 * Target selection: realtime.tlc
 * Embedded hardware selection: ARM Compatible->ARM 7
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "NXTway_ET2012.h"
#include "NXTway_ET2012_private.h"
#include "nxt_util.h"

/* Block signals (auto storage) */
BlockIO_NXTway_ET2012 NXTway_ET2012_B;

/* Block states (auto storage) */
D_Work_NXTway_ET2012 NXTway_ET2012_DWork;

/* Previous zero-crossings (trigger) states */
PrevZCSigStates_NXTway_ET2012 NXTway_ET2012_PrevZCSigState;

/* Real-time model */
RT_MODEL_NXTway_ET2012 NXTway_ET2012_M_;
RT_MODEL_NXTway_ET2012 *const NXTway_ET2012_M = &NXTway_ET2012_M_;
static void rate_scheduler(void);

/*
 *   This function updates active task flag for each subrate.
 * The function is called at model base rate, hence the
 * generated code self-manages all its subrates.
 */
static void rate_scheduler(void)
{
  /* 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).
   */
  (NXTway_ET2012_M->Timing.TaskCounters.TID[1])++;
  if ((NXTway_ET2012_M->Timing.TaskCounters.TID[1]) > 24) {/* Sample time: [0.1s, 0.0s] */
    NXTway_ET2012_M->Timing.TaskCounters.TID[1] = 0;
  }
}

/* Model output function */
void NXTway_ET2012_output(void)
{
  int32_T rtb_RightWheelMotorEncoder_0;
  int32_T rtb_LeftWheelMotorEncoder_0;
  uint16_T rtb_Battery_0;
  uint16_T rtb_TouchSensor_0;
  int8_T tmp;
  int8_T tmp_0;
  int8_T tmp_1;
  real32_T rtb_Switch;
  real32_T rtb_DiscreteFilter;
  real32_T rtb_theta;
  real32_T rtb_Saturation1;
  real32_T rtb_psidot;

  /* S-Function (nxt_gyro_sensor): '<S1>/Gyro Sensor' */
  NXTway_ET2012_B.GyroSensor = getGyroSensorValue(1U, 2U, 600);

  /* S-Function (nxt_light_sensor): '<S1>/Light Sensor' */
  NXTway_ET2012_B.LightSensor = getLightSensorValue(3U, 2U);

  /* Switch: '<S7>/Switch' incorporates:
   *  Constant: '<S7>/Line Edge'
   *  Constant: '<S7>/Turn left'
   *  Constant: '<S7>/Turn right'
   *  RelationalOperator: '<S7>/Relational Operator'
   */
  if (NXTway_ET2012_B.LightSensor > NXTway_ET2012_P.LineEdge_Value) {
    rtb_Switch = NXTway_ET2012_P.Turnright_Value;
  } else {
    rtb_Switch = NXTway_ET2012_P.Turnleft_Value;
  }

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

  /* S-Function (nxt_encoder): '<S1>/Right Wheel Motor Encoder' */
  rtb_RightWheelMotorEncoder_0 = getEncoderValueNoReset(2U);

  /* S-Function (nxt_encoder): '<S1>/Left Wheel Motor Encoder' */
  rtb_LeftWheelMotorEncoder_0 = getEncoderValueNoReset(3U);

  /* S-Function (nxt_battery): '<S2>/Battery' */
  rtb_Battery_0 = getBattery();
  if (NXTway_ET2012_M->Timing.TaskCounters.TID[1] == 0) {
    /* S-Function (nxt_touch_sensor): '<S1>/Touch Sensor' incorporates:
     *  Constant: '<S9>/ON'
     *  Constant: '<S9>/Tail Angle for Drive'
     */
    rtb_TouchSensor_0 = getTouchSensorValue(4U);

    /* Outputs for Triggered SubSystem: '<S2>/Start Actions' incorporates:
     *  TriggerPort: '<S9>/Trigger'
     */
    if ((rtb_TouchSensor_0 > 0) &&
        (NXTway_ET2012_PrevZCSigState.StartActions_Trig_ZCE != POS_ZCSIG)) {
      NXTway_ET2012_B.TailAngleforDrive =
        NXTway_ET2012_P.TailAngleforDrive_Value;
      NXTway_ET2012_B.ON = NXTway_ET2012_P.ON_Value;
    }

    NXTway_ET2012_PrevZCSigState.StartActions_Trig_ZCE = (uint8_T)
      (rtb_TouchSensor_0 > 0 ? (int32_T)POS_ZCSIG : (int32_T)ZERO_ZCSIG);

    /* End of S-Function (nxt_touch_sensor): '<S1>/Touch Sensor' */
    /* End of Outputs for SubSystem: '<S2>/Start Actions' */
  }

  /* Outputs for Enabled SubSystem: '<S2>/Balance and Drive Control' incorporates:
   *  EnablePort: '<S6>/Enable'
   */
  if (NXTway_ET2012_B.ON) {
    /* DiscreteFilter: '<S6>/Discrete Filter' incorporates:
     *  DataTypeConversion: '<S6>/Data Type Conversion6'
     *  S-Function (nxt_battery): '<S2>/Battery'
     */
    NXTway_ET2012_DWork.DiscreteFilter_tmp = ((real32_T)rtb_Battery_0 -
      NXTway_ET2012_P.DiscreteFilter_DenCoef[1] *
      NXTway_ET2012_DWork.DiscreteFilter_DSTATE) /
      NXTway_ET2012_P.DiscreteFilter_DenCoef[0];

    /* Outputs for Atomic SubSystem: '<S6>/NXTway-GS Balance Control' */
    /* Sum: '<S18>/Sum' incorporates:
     *  Constant: '<S13>/Constant6'
     *  Constant: '<S7>/Forward Speed'
     *  Gain: '<S13>/Gain1'
     *  Gain: '<S18>/Gain2'
     *  Gain: '<S18>/Gain3'
     *  Product: '<S13>/Divide'
     *  UnitDelay: '<S18>/Unit Delay'
     */
    NXTway_ET2012_B.Sum = NXTway_ET2012_P.ForwardSpeed_Value / (real32_T)
      NXTway_ET2012_P.Constant6_Value * NXTway_ET2012_P.Gain1_Gain *
      NXTway_ET2012_P.Gain3_Gain_a + NXTway_ET2012_P.Gain2_Gain *
      NXTway_ET2012_DWork.UnitDelay_DSTATE_g;

    /* Gain: '<S14>/Gain' incorporates:
     *  DataTypeConversion: '<S6>/Data Type Conversion4'
     *  DataTypeConversion: '<S6>/Data Type Conversion5'
     *  Gain: '<S14>/deg2rad'
     *  Gain: '<S14>/deg2rad1'
     *  S-Function (nxt_encoder): '<S1>/Left Wheel Motor Encoder'
     *  S-Function (nxt_encoder): '<S1>/Right Wheel Motor Encoder'
     *  Sum: '<S14>/Sum1'
     *  Sum: '<S14>/Sum4'
     *  Sum: '<S14>/Sum6'
     *  UnitDelay: '<S20>/Unit Delay'
     */
    rtb_theta = ((NXTway_ET2012_P.deg2rad_Gain * (real32_T)
                  rtb_LeftWheelMotorEncoder_0 +
                  NXTway_ET2012_DWork.UnitDelay_DSTATE_p) +
                 (NXTway_ET2012_P.deg2rad1_Gain * (real32_T)
                  rtb_RightWheelMotorEncoder_0 +
                  NXTway_ET2012_DWork.UnitDelay_DSTATE_p)) *
      NXTway_ET2012_P.Gain_Gain;

    /* Sum: '<S21>/Sum' incorporates:
     *  Gain: '<S21>/Gain2'
     *  Gain: '<S21>/Gain3'
     *  UnitDelay: '<S21>/Unit Delay'
     */
    NXTway_ET2012_B.Sum_l = NXTway_ET2012_P.Gain3_Gain_g * rtb_theta +
      NXTway_ET2012_P.Gain2_Gain_n * NXTway_ET2012_DWork.UnitDelay_DSTATE_e;

    /* Gain: '<S14>/deg2rad2' incorporates:
     *  Constant: '<S2>/Gyro Offset'
     *  DataTypeConversion: '<S6>/Data Type Conversion2'
     *  DataTypeConversion: '<S6>/Data Type Conversion3'
     *  Sum: '<S14>/Sum2'
     */
    rtb_psidot = ((real32_T)NXTway_ET2012_B.GyroSensor - (real32_T)
                  NXTway_ET2012_P.GyroOffset_Value) *
      NXTway_ET2012_P.deg2rad2_Gain;

    /* Gain: '<S12>/Gain' incorporates:
     *  Constant: '<S13>/Constant2'
     *  Constant: '<S13>/Constant3'
     *  Constant: '<S16>/Constant'
     *  DiscreteFilter: '<S6>/Discrete Filter'
     *  Gain: '<S11>/Feedback Gain'
     *  Gain: '<S11>/Integral Gain'
     *  Gain: '<S16>/Gain3'
     *  Gain: '<S19>/Gain'
     *  Product: '<S12>/Product'
     *  Sum: '<S11>/Sum'
     *  Sum: '<S11>/Sum2'
     *  Sum: '<S16>/Sum2'
     *  Sum: '<S19>/Sum'
     *  UnitDelay: '<S15>/Unit Delay'
     *  UnitDelay: '<S17>/Unit Delay'
     *  UnitDelay: '<S19>/Unit Delay'
     *  UnitDelay: '<S20>/Unit Delay'
     */
    rtb_Saturation1 = ((((NXTway_ET2012_B.Sum - (NXTway_ET2012_B.Sum_l -
      NXTway_ET2012_DWork.UnitDelay_DSTATE_g4) * NXTway_ET2012_P.Gain_Gain_c) *
                         NXTway_ET2012_P.FeedbackGain_Gain[2] +
                         ((NXTway_ET2012_DWork.UnitDelay_DSTATE_a - rtb_theta) *
                          NXTway_ET2012_P.FeedbackGain_Gain[0] +
                          (NXTway_ET2012_P.Constant2_Value -
      NXTway_ET2012_DWork.UnitDelay_DSTATE_p) *
                          NXTway_ET2012_P.FeedbackGain_Gain[1])) +
                        (NXTway_ET2012_P.Constant3_Value - rtb_psidot) *
                        NXTway_ET2012_P.FeedbackGain_Gain[3]) +
                       NXTway_ET2012_P.IntegralGain_Gain *
                       NXTway_ET2012_DWork.UnitDelay_DSTATE) /
      (NXTway_ET2012_P.DiscreteFilter_NumCoef *
       NXTway_ET2012_DWork.DiscreteFilter_tmp * NXTway_ET2012_P.Gain3_Gain -
       NXTway_ET2012_P.Constant_Value) * NXTway_ET2012_P.Gain_Gain_a;

    /* Gain: '<S13>/Gain2' incorporates:
     *  Constant: '<S13>/Constant1'
     *  Product: '<S13>/Divide1'
     */
    rtb_DiscreteFilter = rtb_Switch / (real32_T)NXTway_ET2012_P.Constant1_Value *
      NXTway_ET2012_P.Gain2_Gain_b;

    /* Sum: '<S12>/Sum' */
    rtb_Switch = rtb_Saturation1 + rtb_DiscreteFilter;

    /* Saturate: '<S12>/Saturation' */
    if (rtb_Switch >= NXTway_ET2012_P.Saturation_UpperSat) {
      rtb_Switch = NXTway_ET2012_P.Saturation_UpperSat;
    } else {
      if (rtb_Switch <= NXTway_ET2012_P.Saturation_LowerSat) {
        rtb_Switch = NXTway_ET2012_P.Saturation_LowerSat;
      }
    }

    /* End of Saturate: '<S12>/Saturation' */

    /* Sum: '<S12>/Sum1' */
    rtb_Saturation1 -= rtb_DiscreteFilter;

    /* Saturate: '<S12>/Saturation1' */
    if (rtb_Saturation1 >= NXTway_ET2012_P.Saturation1_UpperSat) {
      rtb_Saturation1 = NXTway_ET2012_P.Saturation1_UpperSat;
    } else {
      if (rtb_Saturation1 <= NXTway_ET2012_P.Saturation1_LowerSat) {
        rtb_Saturation1 = NXTway_ET2012_P.Saturation1_LowerSat;
      }
    }

    /* End of Saturate: '<S12>/Saturation1' */

    /* Sum: '<S17>/Sum' incorporates:
     *  Gain: '<S17>/Gain'
     *  UnitDelay: '<S17>/Unit Delay'
     */
    NXTway_ET2012_B.Sum_j = NXTway_ET2012_P.Gain_Gain_i * NXTway_ET2012_B.Sum +
      NXTway_ET2012_DWork.UnitDelay_DSTATE_a;

    /* Sum: '<S20>/Sum' incorporates:
     *  Gain: '<S20>/Gain'
     *  UnitDelay: '<S20>/Unit Delay'
     */
    NXTway_ET2012_B.Sum_l2 = NXTway_ET2012_P.Gain_Gain_h * rtb_psidot +
      NXTway_ET2012_DWork.UnitDelay_DSTATE_p;

    /* DataTypeConversion: '<S11>/Data Type Conversion1' */
    if (rtb_Saturation1 < 0.0F) {
      rtb_psidot = (real32_T)ceil(rtb_Saturation1);
    } else {
      rtb_psidot = (real32_T)floor(rtb_Saturation1);
    }

    if (rtIsNaNF(rtb_psidot) || rtIsInfF(rtb_psidot)) {
      rtb_psidot = 0.0F;
    } else {
      rtb_psidot = (real32_T)fmod(rtb_psidot, 256.0F);
    }

    NXTway_ET2012_B.DataTypeConversion1 = (int8_T)(rtb_psidot < 0.0F ? (int8_T)
      -(int8_T)(uint8_T)-rtb_psidot : (int8_T)(uint8_T)rtb_psidot);

    /* End of DataTypeConversion: '<S11>/Data Type Conversion1' */

    /* DataTypeConversion: '<S11>/Data Type Conversion8' */
    if (rtb_Switch < 0.0F) {
      rtb_psidot = (real32_T)ceil(rtb_Switch);
    } else {
      rtb_psidot = (real32_T)floor(rtb_Switch);
    }

    if (rtIsNaNF(rtb_psidot) || rtIsInfF(rtb_psidot)) {
      rtb_psidot = 0.0F;
    } else {
      rtb_psidot = (real32_T)fmod(rtb_psidot, 256.0F);
    }

    NXTway_ET2012_B.DataTypeConversion8 = (int8_T)(rtb_psidot < 0.0F ? (int8_T)
      -(int8_T)(uint8_T)-rtb_psidot : (int8_T)(uint8_T)rtb_psidot);

    /* End of DataTypeConversion: '<S11>/Data Type Conversion8' */

    /* Sum: '<S15>/Sum' incorporates:
     *  Gain: '<S15>/Gain'
     *  Sum: '<S11>/Sum1'
     *  UnitDelay: '<S15>/Unit Delay'
     *  UnitDelay: '<S17>/Unit Delay'
     */
    NXTway_ET2012_B.Sum_h = (NXTway_ET2012_DWork.UnitDelay_DSTATE_a - rtb_theta)
      * NXTway_ET2012_P.Gain_Gain_f + NXTway_ET2012_DWork.UnitDelay_DSTATE;

    /* End of Outputs for SubSystem: '<S6>/NXTway-GS Balance Control' */
  }

  /* End of Outputs for SubSystem: '<S2>/Balance and Drive Control' */

  /* S-Function (nxt_motor): '<S3>/Motor' */
  tmp = NXTway_ET2012_B.DataTypeConversion1;
  setMotor(&tmp, 3U, 2U);

  /* S-Function (nxt_motor): '<S4>/Motor' */
  tmp_0 = NXTway_ET2012_B.DataTypeConversion8;
  setMotor(&tmp_0, 2U, 2U);

  /* S-Function (nxt_encoder): '<S1>/Tail Motor Encoder' */
  rtb_RightWheelMotorEncoder_0 = getEncoderValueNoReset(1U);

  /* Gain: '<S10>/P Gain' incorporates:
   *  DataTypeConversion: '<S10>/Data Type Conversion1'
   *  DataTypeConversion: '<S10>/Data Type Conversion6'
   *  S-Function (nxt_encoder): '<S1>/Tail Motor Encoder'
   *  Sum: '<S10>/Sum'
   */
  rtb_psidot = ((real32_T)NXTway_ET2012_B.TailAngleforDrive - (real32_T)
                rtb_RightWheelMotorEncoder_0) * NXTway_ET2012_P.PGain_Gain;

  /* Saturate: '<S10>/Saturation' */
  if (rtb_psidot >= NXTway_ET2012_P.Saturation_UpperSat_m) {
    rtb_psidot = NXTway_ET2012_P.Saturation_UpperSat_m;
  } else {
    if (rtb_psidot <= NXTway_ET2012_P.Saturation_LowerSat_c) {
      rtb_psidot = NXTway_ET2012_P.Saturation_LowerSat_c;
    }
  }

  /* DataTypeConversion: '<S10>/Data Type Conversion2' incorporates:
   *  Saturate: '<S10>/Saturation'
   */
  if (rtb_psidot < 0.0F) {
    rtb_psidot = (real32_T)ceil(rtb_psidot);
  } else {
    rtb_psidot = (real32_T)floor(rtb_psidot);
  }

  if (rtIsNaNF(rtb_psidot) || rtIsInfF(rtb_psidot)) {
    rtb_psidot = 0.0F;
  } else {
    rtb_psidot = (real32_T)fmod(rtb_psidot, 256.0F);
  }

  /* S-Function (nxt_motor): '<S5>/Motor' incorporates:
   *  DataTypeConversion: '<S10>/Data Type Conversion2'
   */
  tmp_1 = (int8_T)(rtb_psidot < 0.0F ? (int8_T)-(int8_T)(uint8_T)-rtb_psidot :
                   (int8_T)(uint8_T)rtb_psidot);
  setMotor(&tmp_1, 1U, 2U);
  if (NXTway_ET2012_M->Timing.TaskCounters.TID[1] == 0) {
    /* S-Function (nxt_lcd): '<S22>/LCD' incorporates:
     *  DataTypeConversion: '<S22>/Data Type Conversion'
     */
    lcdDisplay((int32_T)NXTway_ET2012_B.GyroSensor, NXTway_ET2012_ConstP.LCD_p1,
               1U, 1U);

    /* S-Function (nxt_lcd): '<S23>/LCD' incorporates:
     *  DataTypeConversion: '<S23>/Data Type Conversion'
     */
    lcdDisplay((int32_T)NXTway_ET2012_B.LightSensor,
               NXTway_ET2012_ConstP.LCD_p1_m, 2U, 1U);

    /* S-Function (nxt_ultrasonic_sensor): '<S1>/Ultrasonic Sensor' */
    getUltrasonicSensorValue(2U);
  }
}

/* Model update function */
void NXTway_ET2012_update(void)
{
  /* Update for Enabled SubSystem: '<S2>/Balance and Drive Control' incorporates:
   *  Update for EnablePort: '<S6>/Enable'
   */
  if (NXTway_ET2012_B.ON) {
    /* Update for DiscreteFilter: '<S6>/Discrete Filter' */
    NXTway_ET2012_DWork.DiscreteFilter_DSTATE =
      NXTway_ET2012_DWork.DiscreteFilter_tmp;

    /* Update for Atomic SubSystem: '<S6>/NXTway-GS Balance Control' */
    /* Update for UnitDelay: '<S15>/Unit Delay' */
    NXTway_ET2012_DWork.UnitDelay_DSTATE = NXTway_ET2012_B.Sum_h;

    /* Update for UnitDelay: '<S17>/Unit Delay' */
    NXTway_ET2012_DWork.UnitDelay_DSTATE_a = NXTway_ET2012_B.Sum_j;

    /* Update for UnitDelay: '<S18>/Unit Delay' */
    NXTway_ET2012_DWork.UnitDelay_DSTATE_g = NXTway_ET2012_B.Sum;

    /* Update for UnitDelay: '<S20>/Unit Delay' */
    NXTway_ET2012_DWork.UnitDelay_DSTATE_p = NXTway_ET2012_B.Sum_l2;

    /* Update for UnitDelay: '<S21>/Unit Delay' */
    NXTway_ET2012_DWork.UnitDelay_DSTATE_e = NXTway_ET2012_B.Sum_l;

    /* Update for UnitDelay: '<S19>/Unit Delay' */
    NXTway_ET2012_DWork.UnitDelay_DSTATE_g4 = NXTway_ET2012_B.Sum_l;

    /* End of Update for SubSystem: '<S6>/NXTway-GS Balance Control' */
  }

  /* End of Update for SubSystem: '<S2>/Balance and Drive Control' */
  rate_scheduler();
}

/* Model initialize function */
void NXTway_ET2012_initialize(void)
{
  /* Registration code */

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

  /* initialize real-time model */
  (void) memset((void *)NXTway_ET2012_M, 0,
                sizeof(RT_MODEL_NXTway_ET2012));

  /* block I/O */
  (void) memset(((void *) &NXTway_ET2012_B), 0,
                sizeof(BlockIO_NXTway_ET2012));

  /* states (dwork) */
  (void) memset((void *)&NXTway_ET2012_DWork, 0,
                sizeof(D_Work_NXTway_ET2012));

  /* Start for S-Function (nxt_light_sensor): '<S1>/Light Sensor' */
  initLightSensor(3U);

  /* Start for Triggered SubSystem: '<S2>/Start Actions' */
  /* VirtualOutportStart for Outport: '<S9>/Tail Angle' */
  NXTway_ET2012_B.TailAngleforDrive = NXTway_ET2012_P.TailAngle_Y0;

  /* VirtualOutportStart for Outport: '<S9>/Balancer switch  ' */
  NXTway_ET2012_B.ON = NXTway_ET2012_P.Balancerswitch_Y0;

  /* End of Start for SubSystem: '<S2>/Start Actions' */

  /* Start for Enabled SubSystem: '<S2>/Balance and Drive Control' */
  /* InitializeConditions for DiscreteFilter: '<S6>/Discrete Filter' */
  NXTway_ET2012_DWork.DiscreteFilter_DSTATE =
    NXTway_ET2012_P.DiscreteFilter_InitialStates;

  /* InitializeConditions for Atomic SubSystem: '<S6>/NXTway-GS Balance Control' */
  /* InitializeConditions for UnitDelay: '<S15>/Unit Delay' */
  NXTway_ET2012_DWork.UnitDelay_DSTATE = NXTway_ET2012_P.UnitDelay_X0;

  /* InitializeConditions for UnitDelay: '<S17>/Unit Delay' */
  NXTway_ET2012_DWork.UnitDelay_DSTATE_a = NXTway_ET2012_P.UnitDelay_X0_g;

  /* InitializeConditions for UnitDelay: '<S18>/Unit Delay' */
  NXTway_ET2012_DWork.UnitDelay_DSTATE_g = NXTway_ET2012_P.UnitDelay_X0_m;

  /* InitializeConditions for UnitDelay: '<S20>/Unit Delay' */
  NXTway_ET2012_DWork.UnitDelay_DSTATE_p = NXTway_ET2012_P.UnitDelay_X0_k;

  /* InitializeConditions for UnitDelay: '<S21>/Unit Delay' */
  NXTway_ET2012_DWork.UnitDelay_DSTATE_e = NXTway_ET2012_P.UnitDelay_X0_h;

  /* InitializeConditions for UnitDelay: '<S19>/Unit Delay' */
  NXTway_ET2012_DWork.UnitDelay_DSTATE_g4 = NXTway_ET2012_P.UnitDelay_X0_l;

  /* End of InitializeConditions for SubSystem: '<S6>/NXTway-GS Balance Control' */
  /* End of Start for SubSystem: '<S2>/Balance and Drive Control' */

  /* Start for S-Function (nxt_ultrasonic_sensor): '<S1>/Ultrasonic Sensor' */
  initUltrasonicSensor(2U);
  NXTway_ET2012_PrevZCSigState.StartActions_Trig_ZCE = POS_ZCSIG;
}

/* Model terminate function */
void NXTway_ET2012_terminate(void)
{
  /* Terminate for S-Function (nxt_light_sensor): '<S1>/Light Sensor' */
  terminateLightSensor(3U);

  /* Terminate for S-Function (nxt_motor): '<S3>/Motor' */
  terminateMotor(3U, 2U);

  /* Terminate for S-Function (nxt_motor): '<S4>/Motor' */
  terminateMotor(2U, 2U);

  /* Terminate for S-Function (nxt_motor): '<S5>/Motor' */
  terminateMotor(1U, 2U);

  /* Terminate for S-Function (nxt_ultrasonic_sensor): '<S1>/Ultrasonic Sensor' */
  terminateUltrasonicSensor(2U);
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
