#include "WS.h"
#include "WS_private.h"

#define WS_IN_Down                     (1)
#define WS_IN_left                     (12)
#define WS_IN_UP_R                     (6)
#define WS_IN_right                    (14)
#define WS_IN_UP_L                     (4)
#define WS_IN_left2                    (13)
#define WS_IN_right_half               (15)
#define WS_IN_UP_LH                    (5)
#define WS_IN_UP_END                   (3)
#define WS_IN_UP_R2                    (7)
#define WS_IN_UP_R3                    (9)
#define WS_IN_UP_B                     (2)
#define WS_IN_UP_R31                   (10)
#define WS_IN_UP_RH                    (11)
#define WS_IN_UP_R2H                   (8)

BlockIO_WS WS_B;
D_Work_WS WS_DWork;
ExternalInputs_WS WS_U;
ExternalOutputs_WS WS_Y;
RT_MODEL_WS WS_M_;
RT_MODEL_WS *WS_M = &WS_M_;
static void WS_enter_internal_c1_WS(void );
static void WS_Down(void );
static void WS_UP_R(void );
static void WS_UP_L(void );
static void WS_UP_LH(void );
static void WS_UP_R2(void );
static void WS_UP_R3(void );
static void WS_UP_B(void );
static void WS_UP_R31(void );
static void WS_UP_RH(void );
static void WS_UP_R2H(void );
static void WS_enter_internal_c1_WS(void )
{
  if (WS_U.end_condition == 7.0) {
    WS_B.Zdata = WS_U.koshi_h;
    WS_B.ZEdata = 0.0;
    WS_B.ZLdata = WS_U.koshi_h;
    WS_B.ZELdata = 0.0;
    WS_B.Times_H = 1.0;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.flag_data = (real_T)3.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_B;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_r1 = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
  } else {
    WS_B.Xdata = 0.0;
    WS_B.XEdata = 0.0;
    WS_B.XLdata = 0.0;
    WS_B.XELdata = 0.0;
    WS_B.Ydata = 0.0;
    WS_B.YEdata = 0.0;
    WS_B.YLdata = 0.0;
    WS_B.YELdata = 0.0;
    WS_B.ZEdata = WS_U.koshi_h;
    WS_B.ZELdata = WS_U.koshi_h;
    WS_B.R1data = 0.0;
    WS_B.R1Edata = WS_U.koshi_bend;
    WS_B.Times_H = 1.0;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.flag_data = (real_T)3.0F;
    if (WS_U.end_condition == 5.0) {
      WS_B.Xdata = 0.0;
      WS_B.XEdata = -WS_U.kasoku;
      WS_B.XLdata = 0.0;
      WS_B.XELdata = -WS_U.kasoku;
      WS_B.Zdata = WS_U.koshi_h;
      WS_B.ZEdata = WS_U.koshi_h - WS_U.initial_kick;
      WS_B.ZLdata = WS_U.koshi_h;
      WS_B.ZELdata = WS_U.koshi_h;
      WS_B.Ydata = 0.0;
      WS_B.YEdata = WS_U.shoki_y;
      WS_B.YLdata = 0.0;
      WS_B.YELdata = WS_U.shoki_y;
      WS_B.R1data = WS_U.koshi_bend;
      WS_B.Times_H = WS_B.Divide1;
      WS_DWork.Chart2.Ltimer = WS_U.tim_c;
      WS_B.flag_data = (real_T)0.0F;
      WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_left;
      WS_B.Timer_x = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      WS_B.Timer_y = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    } else {
      WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_Down;
      WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_r1 = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    }
  }
}

static void WS_Down(void )
{
  if (WS_B.Timer_x > WS_B.Times_H) {
    if (WS_U.end_condition >= 4.0) {
      if (WS_U.end_condition <= 5.0) {
        WS_B.Zdata = WS_U.koshi_h;
        WS_B.ZEdata = 0.0;
        WS_B.ZLdata = WS_U.koshi_h;
        WS_B.ZELdata = 0.0;
        WS_B.Ydata = 0.0;
        WS_B.YLdata = 0.0;
        WS_B.XEdata = 0.0;
        WS_B.R1data = WS_U.koshi_bend;
        WS_B.R1Edata = WS_U.koshi_bend;
        WS_B.R2data = 0.0;
        WS_DWork.Chart2.Ltimer = WS_U.tim_c;
        WS_B.flag_data = (real_T)3.0F;
        WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_B;
        WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_r1 = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        return;
      } else if (WS_U.end_condition == 6.0) {
        WS_B.Zdata = WS_U.koshi_h;
        WS_B.ZLdata = WS_U.koshi_h;
        WS_B.ZEdata = WS_U.koshi_h;
        WS_B.ZELdata = WS_U.koshi_h;
        WS_B.R1data = WS_U.koshi_bend;
        WS_B.R1Edata = WS_U.koshi_bend;
        WS_B.flag_data = (real_T)4.0F;
        WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_END;
        WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        return;
      }
    }

    WS_B.Xdata = 0.0;
    WS_B.XEdata = -WS_U.kasoku;
    WS_B.XLdata = 0.0;
    WS_B.XELdata = -WS_U.kasoku;
    WS_B.Zdata = WS_U.koshi_h;
    WS_B.ZEdata = WS_U.koshi_h - WS_U.initial_kick;
    WS_B.ZLdata = WS_U.koshi_h;
    WS_B.ZELdata = WS_U.koshi_h;
    WS_B.Ydata = 0.0;
    WS_B.YEdata = WS_U.shoki_y;
    WS_B.YLdata = 0.0;
    WS_B.YELdata = WS_U.shoki_y;
    WS_B.R1data = WS_U.koshi_bend;
    WS_B.Times_H = WS_B.Divide;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.flag_data = (real_T)0.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_left;
    WS_B.Timer_x = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_y = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  } else {
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_Down;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_r1 = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
  }
}

static void WS_UP_R(void )
{
  if (WS_B.Timer_x > WS_B.Times_H) {
    WS_B.Xdata = WS_U.hohaba_mm;
    WS_B.XEdata = 0.0;
    WS_B.XLdata = -WS_U.keridashi;
    WS_B.XELdata = -WS_U.keridashi - WS_U.hohaba_mm;
    WS_B.Ydata = WS_U.side_dat + WS_U.r_side;
    WS_B.YEdata = -WS_U.side_dat;
    WS_B.YLdata = WS_U.side_dat - WS_U.r_side;
    WS_B.YELdata = -WS_U.side_dat - WS_U.r_side;
    WS_B.Zdata = WS_U.koshi_h;
    WS_B.ZEdata = WS_U.koshi_h;
    WS_B.ZLdata = WS_U.koshi_h;
    WS_B.ZELdata = WS_U.koshi_h - WS_U.kick_hight;
    WS_B.R2data = WS_U.arm_bend;
    WS_B.R1data = WS_U.koshi_bend;
    WS_B.R1Edata = WS_U.koshi_bend - WS_U.koshi_kickup;
    WS_B.R3data = WS_U.kokan;
    WS_B.R3Edata = WS_U.kokan;
    WS_B.R3Ldata = 0.0;
    WS_B.R3LEdata = 0.0;
    WS_B.Times_H = WS_B.Divide1;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.Lcounter = WS_B.Lcounter + 1.0;
    WS_B.flag_data = (real_T)0.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_right;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_r1 = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  } else {
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  }
}

static void WS_UP_L(void )
{
  if (WS_B.Timer_x > WS_B.Times_H) {
    WS_B.Zdata = WS_U.koshi_h;
    WS_B.ZEdata = WS_U.koshi_h - WS_U.kick_hight;
    WS_B.ZLdata = WS_U.koshi_h + WS_U.up_hight;
    WS_B.ZELdata = WS_U.koshi_h;
    WS_B.Ydata = -WS_U.side_dat + WS_U.l_side;
    WS_B.YEdata = WS_U.side_dat + WS_U.l_side;
    WS_B.YLdata = -WS_U.side_dat - WS_U.l_side;
    WS_B.YELdata = WS_U.side_dat;
    WS_B.Xdata = -WS_U.keridashi;
    WS_B.XEdata = -WS_U.keridashi - WS_U.hohaba_mm;
    WS_B.XLdata = WS_U.hohaba_mm;
    WS_B.XELdata = 0.0;
    WS_B.R2data = -WS_U.arm_bend;
    WS_B.R1data = WS_U.koshi_bend;
    WS_B.R1Edata = WS_U.koshi_bend - WS_U.koshi_kickup;
    WS_B.R3data = 0.0;
    WS_B.R3Edata = 0.0;
    WS_B.R3Ldata = WS_U.kokan;
    WS_B.R3LEdata = WS_U.kokan;
    WS_B.Times_H = WS_B.Divide1;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.Lcounter = WS_B.Lcounter + 1.0;
    WS_B.flag_data = (real_T)0.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_left2;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_r1 = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  } else {
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_L;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  }
}

static void WS_UP_LH(void )
{
  if (WS_B.Timer_x > WS_B.Times_H / 2.0) {
    WS_B.Zdata = WS_U.koshi_h;
    WS_B.ZELdata = WS_U.koshi_h + WS_U.leg_hight;
    WS_B.ZLdata = WS_U.koshi_h + WS_U.up_hight;
    WS_B.Ydata = -WS_U.side_dat;
    WS_B.YEdata = -WS_U.side_dat + WS_U.l_side;
    WS_B.YLdata = -WS_U.side_dat - WS_U.r_side;
    WS_B.YELdata = -WS_U.side_dat - WS_U.l_side;
    WS_B.Xdata = 0.0;
    WS_B.XEdata = -WS_U.keridashi;
    WS_B.XLdata = -WS_U.keridashi - WS_U.hohaba_mm;
    WS_B.XELdata = WS_U.hohaba_mm;
    WS_B.R2data = WS_U.arm_bend;
    WS_B.R2Edata = -WS_U.arm_bend;
    WS_B.R1data = WS_U.koshi_bend;
    WS_B.R1Edata = WS_U.koshi_bend;
    WS_B.R3data = WS_U.kokan;
    WS_B.R3Edata = 0.0;
    WS_B.R3Ldata = 0.0;
    WS_B.R3LEdata = WS_U.kokan;
    WS_B.flag_data = (real_T)1.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_L;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  } else {
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_LH;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  }
}

static void WS_UP_R2(void )
{
  if (WS_B.Timer_x > WS_B.Times_H) {
    WS_B.Ydata = WS_U.side_dat + WS_U.r_side;
    WS_B.YEdata = -WS_U.side_dat;
    WS_B.YLdata = WS_U.side_dat - WS_U.r_side;
    WS_B.YELdata = -WS_U.side_dat - WS_U.r_side;
    WS_B.Xdata = WS_U.hohaba_mm;
    WS_B.XEdata = 0.0;
    WS_B.XLdata = -WS_U.keridashi;
    WS_B.XELdata = -WS_U.keridashi - WS_U.hohaba_mm;
    WS_B.Zdata = WS_U.koshi_h + WS_U.up_hight;
    WS_B.ZEdata = WS_U.koshi_h;
    WS_B.ZLdata = WS_U.koshi_h;
    WS_B.ZELdata = WS_U.koshi_h - WS_U.kick_hight;
    WS_B.R3data = WS_U.kokan;
    WS_B.R3Edata = WS_U.kokan;
    WS_B.R3Ldata = 0.0;
    WS_B.R3LEdata = 0.0;
    WS_B.R2data = WS_U.arm_bend;
    WS_B.R1data = WS_U.koshi_bend;
    WS_B.R1Edata = WS_U.koshi_bend - WS_U.koshi_kickup;
    WS_B.Times_H = WS_B.Divide1;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.Lcounter = WS_B.Lcounter + 1.0;
    WS_B.flag_data = (real_T)0.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_right;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_r1 = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  } else {
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R2;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  }
}

static void WS_UP_R3(void )
{
  if (WS_B.Timer_x > WS_B.Times_H) {
    WS_B.ZEdata = WS_U.koshi_h;
    WS_B.ZELdata = WS_U.koshi_h;
    WS_B.Ydata = WS_U.side_dat;
    WS_B.YEdata = 0.0;
    WS_B.YLdata = WS_U.side_dat;
    WS_B.YELdata = 0.0;
    WS_B.Xdata = WS_U.kasoku;
    WS_B.XEdata = 0.0;
    WS_B.XLdata = WS_U.kasoku;
    WS_B.XELdata = 0.0;
    WS_B.R2data = 0.0;
    WS_B.R3data = 0.0;
    WS_B.R1data = WS_U.koshi_bend;
    WS_B.R3Ldata = 0.0;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.flag_data = (real_T)0.0F;
    if (WS_B.Timer_x > WS_B.Times_H / 2.0) {
      WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_right_half;
      WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_y = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      return;
    }
  }

  WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R3;
  WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
  WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
  WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
}

static void WS_UP_B(void )
{
  if (WS_B.Timer_x > WS_B.Times_H) {
    WS_B.Zdata = 0.0;
    WS_B.ZLdata = 0.0;
    WS_B.R1data = 0.0;
    WS_B.R2data = 0.0;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.flag_data = (real_T)4.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_END;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
  } else {
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_B;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_r1 = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
  }
}

static void WS_UP_R31(void )
{
  if (WS_B.Timer_x > WS_B.Times_H) {
    WS_B.ZEdata = WS_U.koshi_h;
    WS_B.ZELdata = WS_U.koshi_h;
    WS_B.Ydata = -WS_U.side_dat;
    WS_B.YEdata = 0.0;
    WS_B.YLdata = -WS_U.side_dat;
    WS_B.YELdata = 0.0;
    WS_B.XLdata = WS_U.kasoku;
    WS_B.XELdata = 0.0;
    WS_B.Xdata = WS_U.kasoku;
    WS_B.XEdata = 0.0;
    WS_B.R2data = 0.0;
    WS_B.R3data = 0.0;
    WS_B.R1data = WS_U.koshi_bend;
    WS_B.R3Ldata = 0.0;
    WS_B.R3LEdata = 0.0;
    WS_DWork.Chart2.Ltimer = WS_U.tim_c;
    WS_B.flag_data = (real_T)0.0F;
    if (WS_B.Timer_x > WS_B.Times_H / 2.0) {
      WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_right_half;
      WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_y = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      return;
    }
  }

  WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R31;
  WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
  WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
  WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
}

static void WS_UP_RH(void )
{
  if (WS_B.Timer_x > WS_B.Times_H / 2.0) {
    WS_B.Xdata = 0.0;
    WS_B.XEdata = WS_U.hohaba_mm;
    WS_B.XLdata = 0.0;
    WS_B.XELdata = -WS_U.keridashi;
    WS_B.Zdata = WS_U.koshi_h;
    WS_B.ZEdata = WS_U.koshi_h + WS_U.leg_hight;
    WS_B.ZLdata = WS_U.koshi_h;
    WS_B.ZELdata = WS_U.koshi_h;
    WS_B.Ydata = WS_U.shoki_y;
    WS_B.YEdata = WS_U.side_dat + WS_U.r_side;
    WS_B.YLdata = WS_U.shoki_y;
    WS_B.YELdata = WS_U.side_dat - WS_U.r_side;
    WS_B.R2data = 0.0;
    WS_B.R2Edata = WS_U.arm_bend;
    WS_B.R3data = 0.0;
    WS_B.R3Edata = WS_U.kokan;
    WS_B.flag_data = (real_T)2.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  } else {
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_RH;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  }
}

static void WS_UP_R2H(void )
{
  if (WS_B.Timer_x > WS_B.Times_H / 2.0) {
    WS_B.ZEdata = WS_U.koshi_h + WS_U.leg_hight;
    WS_B.Zdata = WS_U.koshi_h + WS_U.up_hight;
    WS_B.ZLdata = WS_U.koshi_h;
    WS_B.Ydata = WS_U.side_dat + WS_U.l_side;
    WS_B.YEdata = WS_U.side_dat + WS_U.r_side;
    WS_B.YLdata = WS_U.side_dat;
    WS_B.YELdata = WS_U.side_dat - WS_U.r_side;
    WS_B.XEdata = WS_U.hohaba_mm;
    WS_B.Xdata = -WS_U.keridashi - WS_U.hohaba_mm;
    WS_B.XLdata = 0.0;
    WS_B.XELdata = -WS_U.keridashi;
    WS_B.R2data = -WS_U.arm_bend;
    WS_B.R2Edata = WS_U.arm_bend;
    WS_B.R1data = WS_U.koshi_bend;
    WS_B.R1Edata = WS_U.koshi_bend;
    WS_B.R3data = 0.0;
    WS_B.R3Edata = WS_U.kokan;
    WS_B.R3Ldata = WS_U.kokan;
    WS_B.R3LEdata = 0.0;
    WS_B.flag_data = (real_T)2.0F;
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R2;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  } else {
    WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R2H;
    WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
    WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
    WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
  }
}

void WS_Chart2_Init(void)
{
  WS_DWork.Chart2.is_active_c1_WS = 0U;
  WS_DWork.Chart2.is_c1_WS = 0U;
  WS_DWork.Chart2.Ltimer = 0.0;
  WS_B.Xdata = 0.0;
  WS_B.XEdata = 0.0;
  WS_B.Ydata = 0.0;
  WS_B.YEdata = 0.0;
  WS_B.Zdata = 0.0;
  WS_B.ZEdata = 0.0;
  WS_B.Times_H = 0.0;
  WS_B.XLdata = 0.0;
  WS_B.XELdata = 0.0;
  WS_B.YLdata = 0.0;
  WS_B.YELdata = 0.0;
  WS_B.ZLdata = 0.0;
  WS_B.ZELdata = 0.0;
  WS_B.Lcounter = 0.0;
  WS_B.Timer_x = 0.0;
  WS_B.Timer_y = 0.0;
  WS_B.Timer_zr = 0.0;
  WS_B.Timer_zl = 0.0;
  WS_B.R1data = 0.0;
  WS_B.R1Edata = 0.0;
  WS_B.R2data = 0.0;
  WS_B.R2Edata = 0.0;
  WS_B.R3data = 0.0;
  WS_B.R3Edata = 0.0;
  WS_B.R3Ldata = 0.0;
  WS_B.R3LEdata = 0.0;
  WS_B.flag_data = 0.0;
  WS_B.Timer_r1 = 0.0;
}

void WS_Chart2(void)
{
  if (WS_DWork.Chart2.is_active_c1_WS == 0) {
    WS_DWork.Chart2.is_active_c1_WS = 1U;
    WS_enter_internal_c1_WS();
  } else {
    switch (WS_DWork.Chart2.is_c1_WS) {
     case WS_IN_Down:
      WS_Down();
      break;

     case WS_IN_UP_B:
      WS_UP_B();
      break;

     case WS_IN_UP_END:
      WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_END;
      WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      break;

     case WS_IN_UP_L:
      WS_UP_L();
      break;

     case WS_IN_UP_LH:
      WS_UP_LH();
      break;

     case WS_IN_UP_R:
      WS_UP_R();
      break;

     case WS_IN_UP_R2:
      WS_UP_R2();
      break;

     case WS_IN_UP_R2H:
      WS_UP_R2H();
      break;

     case WS_IN_UP_R3:
      WS_UP_R3();
      break;

     case WS_IN_UP_R31:
      WS_UP_R31();
      break;

     case WS_IN_UP_RH:
      WS_UP_RH();
      break;

     case WS_IN_left:
      if (WS_B.Timer_x > WS_B.Times_H) {
        WS_B.Xdata = -WS_U.kasoku;
        WS_B.XEdata = WS_U.hohaba_mm;
        WS_B.XLdata = -WS_U.kasoku;
        WS_B.XELdata = -WS_U.keridashi;
        WS_B.Zdata = WS_U.koshi_h;
        WS_B.ZEdata = WS_U.koshi_h + WS_U.leg_hight;
        WS_B.ZLdata = WS_U.koshi_h;
        WS_B.ZELdata = WS_U.koshi_h;
        WS_B.Ydata = WS_U.shoki_y;
        WS_B.YEdata = WS_U.side_dat + WS_U.r_side;
        WS_B.YLdata = WS_U.shoki_y;
        WS_B.YELdata = WS_U.side_dat - WS_U.r_side;
        WS_B.R2data = 0.0;
        WS_B.R2Edata = WS_U.arm_bend;
        WS_B.R3data = 0.0;
        WS_B.R3Edata = WS_U.kokan;
        WS_B.Times_H = WS_B.Divide;
        WS_DWork.Chart2.Ltimer = WS_U.tim_c;
        WS_B.flag_data = (real_T)2.0F;
        WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_RH;
        WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      } else {
        WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_left;
        WS_B.Timer_x = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        WS_B.Timer_y = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      }

      break;

     case WS_IN_left2:
      if (WS_B.Timer_x > WS_B.Times_H) {
        WS_B.ZEdata = WS_U.koshi_h + WS_U.leg_hight;
        WS_B.Zdata = WS_U.koshi_h;
        WS_B.ZLdata = WS_U.koshi_h;
        WS_B.Ydata = WS_U.side_dat + WS_U.l_side;
        WS_B.YEdata = WS_U.side_dat + WS_U.r_side;
        WS_B.YLdata = WS_U.side_dat;
        WS_B.YELdata = WS_U.side_dat - WS_U.r_side;
        WS_B.XEdata = WS_U.hohaba_mm;
        WS_B.Xdata = -WS_U.keridashi - WS_U.hohaba_mm;
        WS_B.XLdata = 0.0;
        WS_B.XELdata = -WS_U.keridashi;
        WS_B.R2data = -WS_U.arm_bend;
        WS_B.R2Edata = WS_U.arm_bend;
        WS_B.R1data = WS_U.koshi_bend;
        WS_B.R1Edata = WS_U.koshi_bend;
        WS_B.R3data = 0.0;
        WS_B.R3Edata = WS_U.kokan;
        WS_B.R3Ldata = WS_U.kokan;
        WS_B.R3LEdata = 0.0;
        WS_B.Times_H = WS_B.Divide;
        WS_DWork.Chart2.Ltimer = WS_U.tim_c;
        WS_B.flag_data = (real_T)2.0F;
        if ((WS_B.Lcounter > WS_U.hosuu_dat) || ((5.0 > WS_U.end_condition) &&
             (WS_U.end_condition >= 1.0)) || (WS_U.end_condition == 8.0)) {
          WS_B.YEdata = WS_U.side_dat;
          WS_B.YELdata = WS_U.side_dat;
          WS_B.Xdata = -WS_U.keridashi - WS_U.hohaba_mm;
          WS_B.XEdata = WS_U.kasoku;
          WS_B.XLdata = 0.0;
          WS_B.XELdata = WS_U.kasoku;
          WS_B.R2data = -WS_U.arm_bend;
          WS_B.R2Edata = 0.0;
          WS_B.R3data = 0.0;
          WS_B.R3Edata = 0.0;
          WS_B.R3Ldata = WS_U.kokan;
          WS_B.R3LEdata = 0.0;
          WS_DWork.Chart2.Ltimer = WS_U.tim_c;
          WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R3;
          WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
          WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        } else {
          WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R2H;
          WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
          WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        }
      } else {
        WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_left2;
        WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_r1 = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      }

      break;

     case WS_IN_right:
      if (WS_B.Timer_x > WS_B.Times_H) {
        WS_B.Zdata = WS_U.koshi_h;
        WS_B.ZELdata = WS_U.koshi_h + WS_U.leg_hight;
        WS_B.ZLdata = WS_U.koshi_h;
        WS_B.Ydata = -WS_U.side_dat;
        WS_B.YEdata = -WS_U.side_dat + WS_U.l_side;
        WS_B.YLdata = -WS_U.side_dat - WS_U.r_side;
        WS_B.YELdata = -WS_U.side_dat - WS_U.l_side;
        WS_B.Xdata = 0.0;
        WS_B.XEdata = -WS_U.keridashi;
        WS_B.XLdata = -WS_U.keridashi - WS_U.hohaba_mm;
        WS_B.XELdata = WS_U.hohaba_mm;
        WS_B.R2data = WS_U.arm_bend;
        WS_B.R2Edata = -WS_U.arm_bend;
        WS_B.R1data = WS_U.koshi_bend;
        WS_B.R1Edata = WS_U.koshi_bend;
        WS_B.R3data = WS_U.kokan;
        WS_B.R3Edata = 0.0;
        WS_B.R3Ldata = 0.0;
        WS_B.R3LEdata = WS_U.kokan;
        WS_B.Times_H = WS_B.Divide;
        WS_DWork.Chart2.Ltimer = WS_U.tim_c;
        WS_B.flag_data = (real_T)1.0F;
        if ((WS_B.Lcounter > WS_U.hosuu_dat) || ((5.0 > WS_U.end_condition) &&
             (WS_U.end_condition > 2.0)) || (WS_U.end_condition == 8.0)) {
          WS_B.YEdata = -WS_U.side_dat;
          WS_B.YELdata = -WS_U.side_dat;
          WS_B.XLdata = -WS_U.keridashi - WS_U.hohaba_mm;
          WS_B.XELdata = WS_U.kasoku;
          WS_B.Xdata = 0.0;
          WS_B.XEdata = WS_U.kasoku;
          WS_B.R2data = WS_U.arm_bend;
          WS_B.R2Edata = 0.0;
          WS_B.R3data = WS_U.kokan;
          WS_B.R3Edata = 0.0;
          WS_B.R3Ldata = 0.0;
          WS_B.R3LEdata = 0.0;
          WS_DWork.Chart2.Ltimer = WS_U.tim_c;
          WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_R31;
          WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
          WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        } else {
          WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_LH;
          WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_zr = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
          WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        }
      } else {
        WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_right;
        WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_zl = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_r1 = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
      }

      break;

     case WS_IN_right_half:
      if (WS_B.Timer_x > WS_B.Times_H / 2.0) {
        WS_B.ZEdata = 0.0;
        WS_B.Zdata = WS_U.koshi_h;
        WS_B.ZELdata = 0.0;
        WS_B.ZLdata = WS_U.koshi_h;
        WS_B.Ydata = 0.0;
        WS_B.YLdata = 0.0;
        WS_B.Xdata = 0.0;
        WS_B.XLdata = 0.0;
        WS_B.R1data = WS_U.koshi_bend;
        WS_B.R1Edata = WS_U.koshi_bend;
        WS_B.R2data = 0.0;
        WS_B.Times_H = 1.0;
        WS_DWork.Chart2.Ltimer = WS_U.tim_c;
        WS_B.flag_data = (real_T)3.0F;
        if (WS_U.end_condition > 4.0) {
          WS_B.Times_H = 1.0;
          WS_B.ZEdata = WS_U.koshi_h;
          WS_B.ZELdata = WS_U.koshi_h;
          WS_B.flag_data = (real_T)4.0F;
          WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_END;
          WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        } else {
          WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_UP_B;
          WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_y = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
          WS_B.Timer_r1 = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        }
      } else {
        WS_DWork.Chart2.is_c1_WS = (uint8_T)WS_IN_right_half;
        WS_B.Timer_x = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_y = (WS_U.tim_c - WS_DWork.Chart2.Ltimer) * 2.0;
        WS_B.Timer_zr = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
        WS_B.Timer_zl = WS_U.tim_c - WS_DWork.Chart2.Ltimer;
      }

      break;

     default:
      WS_enter_internal_c1_WS();
      break;
    }
  }
}

void WS_SubsystemY(real_T rtu_0, real_T rtu_1, real_T rtu_2, real_T rtu_3,
                   rtB_SubsystemY_WS *localB)
{
  {
    real_T tmp;
    real_T tmp_0;
    real_T tmp_1;
    real_T tmp_2;
    tmp_0 = rtu_0;
    tmp = rtu_1;
    tmp_1 = rtu_2;
    tmp_2 = rtu_3;
    localB->Fcn = (sin(tmp_1 * 3.1415926535897931 / tmp_2 -
                       1.5707963267948966E+000) + 1.0) * (tmp - tmp_0) / 2.0 +
      tmp_0;
  }
}

void WS_step(void)
{
  WS_B.Divide = 1.0 / WS_U.speed;
  WS_B.Divide1 = WS_B.Divide * WS_U.speed_m;
  WS_Chart2();

  {
    real_T tmp;
    real_T tmp_0;
    real_T tmp_1;
    real_T tmp_2;
    real_T tmp_3;
    real_T tmp_4;
    real_T tmp_5;
    real_T tmp_6;
    tmp_0 = WS_B.R3Ldata;
    tmp = WS_B.R3LEdata;
    tmp_1 = WS_B.Timer_x;
    tmp_2 = WS_B.Times_H;
    WS_B.Fcn_a = (sin(tmp_1 * 3.1415926535897931 / tmp_2 -
                      1.5707963267948966E+000) + 1.0) * (tmp - tmp_0) / 2.0 +
      tmp_0;
    tmp_4 = WS_B.R3data;
    tmp_3 = WS_B.R3Edata;
    tmp_5 = WS_B.Timer_x;
    tmp_6 = WS_B.Times_H;
    WS_B.Fcn_c = (sin(tmp_5 * 3.1415926535897931 / tmp_6 -
                      1.5707963267948966E+000) + 1.0) * (tmp_3 - tmp_4) / 2.0 +
      tmp_4;
    if (WS_U.kokan >= 0.0) {
      if (WS_U.rl_cont >= 0.0) {
        WS_B.Switch9 = WS_B.Fcn_a;
      } else {
        WS_B.Switch9 = WS_B.Fcn_c;
      }

      WS_B.Switch8 = WS_B.Switch9;
    } else {
      if (WS_U.rl_cont >= 0.0) {
        WS_B.Switch8_d = WS_B.Fcn_c;
      } else {
        WS_B.Switch8_d = WS_B.Fcn_a;
      }

      WS_B.Gain2_c = WS_B.Switch8_d * -1.0;
      WS_B.Switch8 = WS_B.Gain2_c;
    }
  }

  WS_SubsystemY(WS_B.Ydata, WS_B.YEdata, WS_B.Timer_y, WS_B.Times_H,
                &WS_B.SubsystemY);

  {
    real_T tmp;
    real_T tmp_0;
    real_T tmp_1;
    real_T tmp_2;
    real_T tmp_3;
    real_T tmp_4;
    real_T tmp_5;
    real_T tmp_6;
    real_T tmp_7;
    real_T tmp_8;
    real_T tmp_9;
    real_T tmp_a;
    real_T tmp_b;
    real_T tmp_c;
    WS_B.Sum = WS_B.SubsystemY.Fcn + WS_U.ybalance;
    tmp_0 = WS_B.YLdata;
    tmp = WS_B.YELdata;
    tmp_1 = WS_B.Timer_y;
    tmp_2 = WS_B.Times_H;
    WS_B.Fcn_i = (sin(tmp_1 * 3.1415926535897931 / tmp_2 -
                      1.5707963267948966E+000) + 1.0) * (tmp - tmp_0) / 2.0 +
      tmp_0;
    WS_B.Sum1 = WS_B.Fcn_i + WS_U.ybalance;
    if (WS_U.rl_cont >= 0.0) {
      WS_B.Switch1 = WS_B.Sum;
    } else {
      WS_B.Gain1_e = WS_B.Sum1 * -1.0;
      WS_B.Switch1 = WS_B.Gain1_e;
    }

    tmp_4 = WS_U.leg_leng;
    tmp_3 = WS_B.Switch1;
    WS_B.Fcn10 = tmp_3 / tmp_4;
    if (WS_B.Fcn10 >= -1.0) {
      WS_B.Switch = WS_B.Fcn10;
    } else {
      WS_B.Switch = -1.0;
    }

    if (WS_B.Switch >= 1.0) {
      WS_B.Switch1_g = 1.0;
    } else {
      WS_B.Switch1_g = WS_B.Switch;
    }

    WS_B.Fcn3 = asin(WS_B.Switch1_g);
    WS_B.Fcn7 = 180.0 * WS_B.Fcn3 / 3.1415926535897931;
    tmp_a = WS_B.Xdata;
    tmp_9 = WS_B.XEdata;
    tmp_b = WS_B.Timer_x;
    tmp_c = WS_B.Times_H;
    WS_B.Fcn1_h = (tmp_9 - tmp_a) * tmp_b / tmp_c + tmp_a;
    WS_B.Sum2 = WS_B.Fcn1_h + WS_U.xbalance;
    tmp_6 = WS_B.XLdata;
    tmp_5 = WS_B.XELdata;
    tmp_7 = WS_B.Timer_x;
    tmp_8 = WS_B.Times_H;
    WS_B.Fcn1 = (tmp_5 - tmp_6) * tmp_7 / tmp_8 + tmp_6;
    WS_B.Sum3 = WS_B.Fcn1 + WS_U.xbalance;
    if (WS_U.rl_cont >= 0.0) {
      WS_B.Switch_d = WS_B.Sum2;
    } else {
      WS_B.Switch_d = WS_B.Sum3;
    }

    if (WS_U.fr_cont >= 0.0) {
      WS_B.Switch6 = WS_B.Switch_d;
    } else {
      WS_B.Gain2 = WS_B.Switch_d * -1.0;
      WS_B.Switch6 = WS_B.Gain2;
    }
  }

  WS_SubsystemY(WS_B.Zdata, WS_B.ZEdata, WS_B.Timer_zr, WS_B.Times_H,
                &WS_B.SubsystemZ);

  {
    real_T cg_in[17];
    int16_T i;
    real_T tmp;
    real_T tmp_0;
    real_T tmp_1;
    real_T tmp_2;
    real_T tmp_3;
    real_T tmp_4;
    real_T tmp_5;
    real_T tmp_6;
    real_T tmp_7;
    real_T tmp_8;
    real_T tmp_9;
    real_T tmp_a;
    real_T tmp_b;
    real_T tmp_c;
    real_T tmp_d;
    real_T tmp_e;
    real_T tmp_f;
    real_T tmp_g;
    real_T tmp_h;
    real_T tmp_i;
    real_T tmp_j;
    real_T tmp_k;
    real_T tmp_l;
    real_T tmp_m;
    real_T tmp_n;
    real_T tmp_o;
    real_T tmp_p;
    real_T tmp_q;
    tmp_0 = WS_B.ZLdata;
    tmp = WS_B.ZELdata;
    tmp_1 = WS_B.Timer_zl;
    tmp_2 = WS_B.Times_H;
    WS_B.Fcn_e = (sin(tmp_1 * 3.1415926535897931 / tmp_2 -
                      1.5707963267948966E+000) + 1.0) * (tmp - tmp_0) / 2.0 +
      tmp_0;
    if (WS_U.rl_cont >= 0.0) {
      WS_B.Switch2 = WS_B.SubsystemZ.Fcn;
    } else {
      WS_B.Switch2 = WS_B.Fcn_e;
    }

    tmp_3 = WS_B.Switch6;
    tmp_4 = WS_U.leg_leng;
    tmp_5 = WS_B.Switch2;
    WS_B.Fcn12 = (tmp_4 - tmp_5) * (tmp_4 - tmp_5) + tmp_3 * tmp_3;
    if (WS_B.Fcn12 >= 10.0) {
      WS_B.Switch_g = WS_B.Fcn12;
    } else {
      WS_B.Switch_g = 10.0;
    }

    if (WS_B.Switch_g >= 50000.0) {
      WS_B.Switch1_c = 50000.0;
    } else {
      WS_B.Switch1_c = WS_B.Switch_g;
    }

    WS_B.Fcn13 = sqrt(WS_B.Switch1_c);
    tmp_7 = WS_U.leg_leng;
    tmp_6 = WS_B.Fcn13;
    WS_B.Fcn2 = tmp_6 / tmp_7;
    if (WS_B.Fcn2 >= -1.0) {
      WS_B.Switch_c = WS_B.Fcn2;
    } else {
      WS_B.Switch_c = -1.0;
    }

    if (WS_B.Switch_c >= 1.0) {
      WS_B.Switch1_m = 1.0;
    } else {
      WS_B.Switch1_m = WS_B.Switch_c;
    }

    WS_B.Fcn1_i = 1.5707963267948966E+000 - asin(WS_B.Switch1_m);
    tmp_8 = WS_B.Switch6;
    tmp_9 = WS_B.Fcn13;
    WS_B.Fcn8 = tmp_8 / tmp_9;
    if (WS_B.Fcn8 >= -1.0) {
      WS_B.Switch_ck = WS_B.Fcn8;
    } else {
      WS_B.Switch_ck = -1.0;
    }

    if (WS_B.Switch_ck >= 1.0) {
      WS_B.Switch1_d = 1.0;
    } else {
      WS_B.Switch1_d = WS_B.Switch_ck;
    }

    WS_B.Fcn9 = asin(WS_B.Switch1_d);
    WS_B.Sum_k = WS_B.Fcn1_i + WS_B.Fcn9;
    WS_B.Fcn4 = 180.0 * WS_B.Sum_k / 3.1415926535897931;
    tmp_b = WS_B.R1data;
    tmp_a = WS_B.R1Edata;
    tmp_c = WS_B.Timer_r1;
    tmp_d = WS_B.Times_H;
    WS_B.Fcn_h = (sin(tmp_c * 3.1415926535897931 / tmp_d -
                      1.5707963267948966E+000) + 1.0) * (tmp_a - tmp_b) / 2.0 +
      tmp_b;
    WS_B.Sum_d = WS_B.Fcn4 + WS_B.Fcn_h;
    WS_B.Gain = WS_B.Fcn1_i * 2.0;
    WS_B.Fcn5 = 180.0 * WS_B.Gain / 3.1415926535897931;
    WS_B.Sum1_m = WS_B.Fcn1_i - WS_B.Fcn9;
    WS_B.Fcn6 = 180.0 * WS_B.Sum1_m / 3.1415926535897931;
    WS_B.Gain1 = WS_B.Switch8 * -1.0;
    if (WS_U.rl_cont >= 0.0) {
      WS_B.Switch5 = WS_B.Sum1;
    } else {
      WS_B.Gain_a = WS_B.Sum * -1.0;
      WS_B.Switch5 = WS_B.Gain_a;
    }

    tmp_f = WS_U.leg_leng;
    tmp_e = WS_B.Switch5;
    WS_B.Fcn10_j = tmp_e / tmp_f;
    if (WS_B.Fcn10_j >= -1.0) {
      WS_B.Switch_b = WS_B.Fcn10_j;
    } else {
      WS_B.Switch_b = -1.0;
    }

    if (WS_B.Switch_b >= 1.0) {
      WS_B.Switch1_h = 1.0;
    } else {
      WS_B.Switch1_h = WS_B.Switch_b;
    }

    WS_B.Fcn3_c = asin(WS_B.Switch1_h);
    WS_B.Fcn7_h = 180.0 * WS_B.Fcn3_c / 3.1415926535897931;
    if (WS_U.rl_cont >= 0.0) {
      WS_B.Switch3 = WS_B.Sum3;
    } else {
      WS_B.Switch3 = WS_B.Sum2;
    }

    if (WS_U.jump >= 0.0) {
      if (WS_U.fr_cont >= 0.0) {
        WS_B.Switch7 = WS_B.Switch3;
      } else {
        WS_B.Gain3 = WS_B.Switch3 * -1.0;
        WS_B.Switch7 = WS_B.Gain3;
      }

      WS_B.Switch10 = WS_B.Switch7;
    } else {
      WS_B.Switch10 = WS_B.Switch6;
    }

    if (WS_U.jump >= 0.0) {
      if (WS_U.rl_cont >= 0.0) {
        WS_B.Switch4 = WS_B.Fcn_e;
      } else {
        WS_B.Switch4 = WS_B.SubsystemZ.Fcn;
      }

      WS_B.Switch11 = WS_B.Switch4;
    } else {
      WS_B.Switch11 = WS_B.Switch2;
    }

    tmp_g = WS_B.Switch10;
    tmp_h = WS_U.leg_leng;
    tmp_i = WS_B.Switch11;
    WS_B.Fcn12_o = (tmp_h - tmp_i) * (tmp_h - tmp_i) + tmp_g * tmp_g;
    if (WS_B.Fcn12_o >= 10.0) {
      WS_B.Switch_k = WS_B.Fcn12_o;
    } else {
      WS_B.Switch_k = 10.0;
    }

    if (WS_B.Switch_k >= 50000.0) {
      WS_B.Switch1_j = 50000.0;
    } else {
      WS_B.Switch1_j = WS_B.Switch_k;
    }

    WS_B.Fcn13_n = sqrt(WS_B.Switch1_j);
    tmp_k = WS_U.leg_leng;
    tmp_j = WS_B.Fcn13_n;
    WS_B.Fcn2_d = tmp_j / tmp_k;
    if (WS_B.Fcn2_d >= -1.0) {
      WS_B.Switch_o = WS_B.Fcn2_d;
    } else {
      WS_B.Switch_o = -1.0;
    }

    if (WS_B.Switch_o >= 1.0) {
      WS_B.Switch1_ml = 1.0;
    } else {
      WS_B.Switch1_ml = WS_B.Switch_o;
    }

    WS_B.Fcn1_m = 1.5707963267948966E+000 - asin(WS_B.Switch1_ml);
    tmp_l = WS_B.Switch10;
    tmp_m = WS_B.Fcn13_n;
    WS_B.Fcn8_j = tmp_l / tmp_m;
    if (WS_B.Fcn8_j >= -1.0) {
      WS_B.Switch_gw = WS_B.Fcn8_j;
    } else {
      WS_B.Switch_gw = -1.0;
    }

    if (WS_B.Switch_gw >= 1.0) {
      WS_B.Switch1_a = 1.0;
    } else {
      WS_B.Switch1_a = WS_B.Switch_gw;
    }

    WS_B.Fcn9_i = asin(WS_B.Switch1_a);
    WS_B.Sum_n = WS_B.Fcn1_m + WS_B.Fcn9_i;
    WS_B.Fcn4_c = 180.0 * WS_B.Sum_n / 3.1415926535897931;
    WS_B.Sum1_e = WS_B.Fcn4_c + WS_B.Fcn_h;
    WS_B.Gain_o = WS_B.Fcn1_m * 2.0;
    WS_B.Fcn5_b = 180.0 * WS_B.Gain_o / 3.1415926535897931;
    WS_B.Sum1_g = WS_B.Fcn1_m - WS_B.Fcn9_i;
    WS_B.Fcn6_e = 180.0 * WS_B.Sum1_g / 3.1415926535897931;
    tmp_o = WS_B.R2data;
    tmp_n = WS_B.R2Edata;
    tmp_p = WS_B.Timer_x;
    tmp_q = WS_B.Times_H;
    WS_B.Fcn_eu = (sin(tmp_p * 3.1415926535897931 / tmp_q -
                       1.5707963267948966E+000) + 1.0) * (tmp_n - tmp_o) / 2.0 +
      tmp_o;
    cg_in[0] = WS_B.Switch8;
    cg_in[1] = WS_B.Fcn7;
    cg_in[2] = WS_B.Sum_d;
    cg_in[3] = WS_B.Fcn5;
    cg_in[4] = WS_B.Fcn6;
    cg_in[5] = WS_B.Fcn7;
    cg_in[6] = WS_B.Gain1;
    cg_in[7] = WS_B.Fcn7_h;
    cg_in[8] = WS_B.Sum1_e;
    cg_in[9] = WS_B.Fcn5_b;
    cg_in[10] = WS_B.Fcn6_e;
    cg_in[11] = WS_B.Fcn7_h;
    cg_in[12] = WS_B.Fcn_h;
    cg_in[13] = WS_B.Fcn_eu;
    cg_in[14] = WS_B.Fcn_c;
    cg_in[15] = WS_B.Fcn_a;
    cg_in[16] = 1.0;
    for (i = 0; i < 17; i++) {
      WS_Y.angle_dat[i] = cg_in[i];
    }

    WS_Y.w_flag = WS_B.flag_data;
    WS_Y.hosuu = WS_B.Lcounter;
  }
}

void WS_initialize(boolean_T firstTime)
{
  (void)firstTime;
  rtmSetErrorStatus(WS_M, (const char_T *)0);

  {
    int_T i;
    void *pVoidBlockIORegion;
    pVoidBlockIORegion = (void *)(&WS_B.Divide);
    for (i = 0; i < 71; i++) {
      ((real_T*)pVoidBlockIORegion)[i] = 0.0;
    }

    pVoidBlockIORegion = (void *)(&WS_B.Gain1_e);
    for (i = 0; i < 38; i++) {
      ((real_T*)pVoidBlockIORegion)[i] = 0.0;
    }

    ((real_T*)&WS_B.SubsystemZ.Fcn)[0] = 0.0;
    ((real_T*)&WS_B.SubsystemY.Fcn)[0] = 0.0;
  }

  (void) memset((char_T *) &WS_DWork,0,
                sizeof(D_Work_WS));
  WS_U.tim_c = 0.0;
  WS_U.side_dat = 0.0;
  WS_U.hohaba_mm = 0.0;
  WS_U.keridashi = 0.0;
  WS_U.speed = 0.0;
  WS_U.hosuu_dat = 0.0;
  WS_U.koshi_h = 0.0;
  WS_U.leg_hight = 0.0;
  WS_U.kick_hight = 0.0;
  WS_U.up_hight = 0.0;
  WS_U.r_side = 0.0;
  WS_U.l_side = 0.0;
  WS_U.koshi_bend = 0.0;
  WS_U.arm_bend = 0.0;
  WS_U.kokan = 0.0;
  WS_U.end_condition = 0.0;
  WS_U.rl_cont = 0.0;
  WS_U.fr_cont = 0.0;
  WS_U.ybalance = 0.0;
  WS_U.leg_leng = 0.0;
  WS_U.xbalance = 0.0;
  WS_U.kasoku = 0.0;
  WS_U.koshi_kickup = 0.0;
  WS_U.jump = 0.0;
  WS_U.speed_m = 0.0;
  WS_U.initial_kick = 0.0;
  WS_U.shoki_y = 0.0;

  {
    int_T i;
    for (i = 0; i < 17; i++) {
      WS_Y.angle_dat[i] = 0.0;
    }
  }

  WS_Y.w_flag = 0.0;
  WS_Y.hosuu = 0.0;
  WS_Chart2_Init();
}

void WS_terminate(void)
{
}
