c7_deltarobot.c

It is a huge matlab document, kinematic represantation and control of Modified Delta Mechanism which

/* Include files */

#include "DeltaRobot_sfun.h"
#include "c7_DeltaRobot.h"
#define CHARTINSTANCE_CHARTNUMBER      (chartInstance.chartNumber)
#define CHARTINSTANCE_INSTANCENUMBER   (chartInstance.instanceNumber)
#include "DeltaRobot_sfun_debug_macros.h"

/* Type Definitions */

/* Named Constants */
#define c7_IN_NO_ACTIVE_CHILD          (0)

/* Variable Declarations */

/* Variable Definitions */
static SFc7_DeltaRobotInstanceStruct chartInstance;

/* Function Declarations */
static void initialize_c7_DeltaRobot(void);
static void initialize_params_c7_DeltaRobot(void);
static void enable_c7_DeltaRobot(void);
static void disable_c7_DeltaRobot(void);
static void finalize_c7_DeltaRobot(void);
static void sf_c7_DeltaRobot(void);
static void c7_c7_DeltaRobot(void);
static void init_script_number_translation(uint32_T c7_machineNumber, uint32_T
  c7_chartNumber);
static void c7_eml_xgemm(real_T c7_A[9], real_T c7_B[9], real_T c7_C[9], real_T
  c7_b_C[9]);
static const mxArray *c7_sf_marshall(void *c7_chartInstance, void *c7_u);
static const mxArray *c7_b_sf_marshall(void *c7_chartInstance, void *c7_u);
static void c7_info_helper(c7_ResolvedFunctionInfo c7_info[44]);
static void init_dsm_address_info(void);

/* Function Definitions */
static void initialize_c7_DeltaRobot(void)
{
  _sfTime_ = (real_T)ssGetT(chartInstance.S);
  chartInstance.c7_is_active_c7_DeltaRobot = 0U;
}

static void initialize_params_c7_DeltaRobot(void)
{
}

static void enable_c7_DeltaRobot(void)
{
  _sfTime_ = (real_T)ssGetT(chartInstance.S);
}

static void disable_c7_DeltaRobot(void)
{
  _sfTime_ = (real_T)ssGetT(chartInstance.S);
}

static void finalize_c7_DeltaRobot(void)
{
}

static void sf_c7_DeltaRobot(void)
{
  int32_T c7_i0;
  int32_T c7_i1;
  int32_T c7_i2;
  int32_T c7_i3;
  int32_T c7_i4;
  int32_T c7_i5;
  int32_T c7_i6;
  int32_T c7_i7;
  int32_T c7_i8;
  int32_T c7_i9;
  int32_T c7_i10;
  int32_T c7_i11;
  uint8_T c7_previousEvent;
  real_T *c7_xee;
  real_T *c7_yee;
  real_T *c7_zee;
  real_T *c7_xa1;
  real_T *c7_ya1;
  real_T *c7_za1;
  real_T *c7_xa2;
  real_T *c7_ya2;
  real_T *c7_za2;
  real_T *c7_theta_a1;
  real_T *c7_theta_a2;
  real_T *c7_theta_a3;
  real_T *c7_theta_b1;
  real_T *c7_xa3;
  real_T *c7_ya3;
  real_T *c7_theta_b2;
  real_T *c7_theta_b3;
  real_T *c7_phi_b1;
  real_T *c7_phi_b2;
  real_T *c7_phi_b3;
  real_T *c7_za3;
  real_T *c7_xb1;
  real_T *c7_yb1;
  real_T *c7_zb1;
  real_T *c7_xb2;
  real_T *c7_yb2;
  real_T *c7_zb2;
  real_T *c7_xb3;
  real_T *c7_yb3;
  real_T *c7_zb3;
  real_T (*c7_rot_a1)[9];
  real_T (*c7_rot_a2)[9];
  real_T (*c7_rot_a3)[9];
  real_T (*c7_rot_b1)[9];
  real_T (*c7_rot_b2)[9];
  real_T (*c7_rot_b3)[9];
  c7_rot_a1 = (real_T (*)[9])ssGetOutputPortSignal(chartInstance.S, 1);
  c7_xb2 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 22);
  c7_theta_a3 = (real_T *)ssGetInputPortSignal(chartInstance.S, 2);
  c7_xa2 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 13);
  c7_phi_b3 = (real_T *)ssGetInputPortSignal(chartInstance.S, 8);
  c7_zb3 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 27);
  c7_rot_b1 = (real_T (*)[9])ssGetOutputPortSignal(chartInstance.S, 4);
  c7_xa1 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 10);
  c7_theta_a2 = (real_T *)ssGetInputPortSignal(chartInstance.S, 1);
  c7_za1 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 12);
  c7_theta_a1 = (real_T *)ssGetInputPortSignal(chartInstance.S, 0);
  c7_yb2 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 23);
  c7_xb1 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 19);
  c7_za3 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 18);
  c7_theta_b3 = (real_T *)ssGetInputPortSignal(chartInstance.S, 5);
  c7_xee = (real_T *)ssGetOutputPortSignal(chartInstance.S, 7);
  c7_ya1 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 11);
  c7_phi_b2 = (real_T *)ssGetInputPortSignal(chartInstance.S, 7);
  c7_yb1 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 20);
  c7_zee = (real_T *)ssGetOutputPortSignal(chartInstance.S, 9);
  c7_theta_b2 = (real_T *)ssGetInputPortSignal(chartInstance.S, 4);
  c7_theta_b1 = (real_T *)ssGetInputPortSignal(chartInstance.S, 3);
  c7_yee = (real_T *)ssGetOutputPortSignal(chartInstance.S, 8);
  c7_xb3 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 25);
  c7_rot_b2 = (real_T (*)[9])ssGetOutputPortSignal(chartInstance.S, 5);
  c7_ya2 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 14);
  c7_phi_b1 = (real_T *)ssGetInputPortSignal(chartInstance.S, 6);
  c7_zb1 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 21);
  c7_za2 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 15);
  c7_ya3 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 17);
  c7_rot_a3 = (real_T (*)[9])ssGetOutputPortSignal(chartInstance.S, 3);
  c7_xa3 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 16);
  c7_yb3 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 26);
  c7_zb2 = (real_T *)ssGetOutputPortSignal(chartInstance.S, 24);
  c7_rot_b3 = (real_T (*)[9])ssGetOutputPortSignal(chartInstance.S, 6);
  c7_rot_a2 = (real_T (*)[9])ssGetOutputPortSignal(chartInstance.S, 2);
  _sfTime_ = (real_T)ssGetT(chartInstance.S);
  _SFD_CC_CALL(CHART_ENTER_SFUNCTION_TAG,0);
  for (c7_i0 = 0; c7_i0 < 3; c7_i0 = c7_i0 + 1) {
    for (c7_i1 = 0; c7_i1 < 3; c7_i1 = c7_i1 + 1) {
      _SFD_DATA_RANGE_CHECK((*c7_rot_a1)[c7_i1 + 3 * c7_i0], 0U);
    }
  }

  for (c7_i2 = 0; c7_i2 < 3; c7_i2 = c7_i2 + 1) {
    for (c7_i3 = 0; c7_i3 < 3; c7_i3 = c7_i3 + 1) {
      _SFD_DATA_RANGE_CHECK((*c7_rot_a2)[c7_i3 + 3 * c7_i2], 1U);
    }
  }

  for (c7_i4 = 0; c7_i4 < 3; c7_i4 = c7_i4 + 1) {
    for (c7_i5 = 0; c7_i5 < 3; c7_i5 = c7_i5 + 1) {
      _SFD_DATA_RANGE_CHECK((*c7_rot_a3)[c7_i5 + 3 * c7_i4], 2U);
    }
  }

  for (c7_i6 = 0; c7_i6 < 3; c7_i6 = c7_i6 + 1) {
    for (c7_i7 = 0; c7_i7 < 3; c7_i7 = c7_i7 + 1) {
      _SFD_DATA_RANGE_CHECK((*c7_rot_b1)[c7_i7 + 3 * c7_i6], 3U);
    }
  }

  for (c7_i8 = 0; c7_i8 < 3; c7_i8 = c7_i8 + 1) {
    for (c7_i9 = 0; c7_i9 < 3; c7_i9 = c7_i9 + 1) {
      _SFD_DATA_RANGE_CHECK((*c7_rot_b2)[c7_i9 + 3 * c7_i8], 4U);
    }
  }

  for (c7_i10 = 0; c7_i10 < 3; c7_i10 = c7_i10 + 1) {
    for (c7_i11 = 0; c7_i11 < 3; c7_i11 = c7_i11 + 1) {
      _SFD_DATA_RANGE_CHECK((*c7_rot_b3)[c7_i11 + 3 * c7_i10], 5U);
    }
  }

  _SFD_DATA_RANGE_CHECK(*c7_xee, 6U);
  _SFD_DATA_RANGE_CHECK(*c7_yee, 7U);
  _SFD_DATA_RANGE_CHECK(*c7_zee, 8U);
  _SFD_DATA_RANGE_CHECK(*c7_xa1, 9U);
  _SFD_DATA_RANGE_CHECK(*c7_ya1, 10U);
  _SFD_DATA_RANGE_CHECK(*c7_za1, 11U);
  _SFD_DATA_RANGE_CHECK(*c7_xa2, 12U);
  _SFD_DATA_RANGE_CHECK(*c7_ya2, 13U);
  _SFD_DATA_RANGE_CHECK(*c7_za2, 14U);
  _SFD_DATA_RANGE_CHECK(*c7_theta_a1, 15U);
  _SFD_DATA_RANGE_CHECK(*c7_theta_a2, 16U);
  _SFD_DATA_RANGE_CHECK(*c7_theta_a3, 17U);
  _SFD_DATA_RANGE_CHECK(*c7_theta_b1, 18U);
  _SFD_DATA_RANGE_CHECK(*c7_xa3, 19U);
  _SFD_DATA_RANGE_CHECK(*c7_ya3, 20U);
  _SFD_DATA_RANGE_CHECK(*c7_theta_b2, 21U);
  _SFD_DATA_RANGE_CHECK(*c7_theta_b3, 22U);
  _SFD_DATA_RANGE_CHECK(*c7_phi_b1, 23U);
  _SFD_DATA_RANGE_CHECK(*c7_phi_b2, 24U);
  _SFD_DATA_RANGE_CHECK(*c7_phi_b3, 25U);
  _SFD_DATA_RANGE_CHECK(*c7_za3, 26U);
  _SFD_DATA_RANGE_CHECK(*c7_xb1, 27U);
  _SFD_DATA_RANGE_CHECK(*c7_yb1, 28U);
  _SFD_DATA_RANGE_CHECK(*c7_zb1, 29U);
  _SFD_DATA_RANGE_CHECK(*c7_xb2, 30U);
  _SFD_DATA_RANGE_CHECK(*c7_yb2, 31U);
  _SFD_DATA_RANGE_CHECK(*c7_zb2, 32U);
  _SFD_DATA_RANGE_CHECK(*c7_xb3, 33U);
  _SFD_DATA_RANGE_CHECK(*c7_yb3, 34U);
  _SFD_DATA_RANGE_CHECK(*c7_zb3, 35U);
  c7_previousEvent = _sfEvent_;
  _sfEvent_ = CALL_EVENT;
  c7_c7_DeltaRobot();
  _sfEvent_ = c7_previousEvent;
  sf_debug_check_for_state_inconsistency(_DeltaRobotMachineNumber_,
    chartInstance.chartNumber, chartInstance.instanceNumber);
}

static void c7_c7_DeltaRobot(void)
{
  real_T c7_theta_a1;
  real_T c7_theta_a2;
  real_T c7_theta_a3;
  real_T c7_theta_b1;
  real_T c7_theta_b2;
  real_T c7_theta_b3;
  real_T c7_phi_b1;
  real_T c7_phi_b2;
  real_T c7_phi_b3;
  real_T c7_nargout = 27.0;
  real_T c7_nargin = 9.0;
  real_T c7_rot_c3[9];
  real_T c7_rot_c2[9];
  real_T c7_rot_c1[9];
  real_T c7_rb;
  real_T c7_ra;
  real_T c7_LB;
  real_T c7_LA;
  real_T c7_zb3;
  real_T c7_yb3;
  real_T c7_xb3;
  real_T c7_zb2;
  real_T c7_yb2;
  real_T c7_xb2;
  real_T c7_zb1;
  real_T c7_yb1;
  real_T c7_xb1;
  real_T c7_za3;
  real_T c7_ya3;
  real_T c7_xa3;
  real_T c7_za2;
  real_T c7_ya2;
  real_T c7_xa2;
  real_T c7_za1;
  real_T c7_ya1;
  real_T c7_xa1;
  real_T c7_zee;
  real_T c7_yee;
  real_T c7_xee;
  real_T c7_rot_b3[9];
  real_T c7_rot_b2[9];
  real_T c7_rot_b1[9];
  real_T c7_rot_a3[9];
  real_T c7_rot_a2[9];
  real_T c7_rot_a1[9];
  real_T c7_x;
  real_T c7_b;
  real_T c7_a;
  real_T c7_y;
  real_T c7_b_x;
  real_T c7_b_b;
  real_T c7_b_a;
  real_T c7_c_x;
  real_T c7_c_b;
  real_T c7_c_a;
  real_T c7_d_x;
  real_T c7_d_b;
  real_T c7_d_a;
  real_T c7_e_a;
  real_T c7_e_x;
  real_T c7_e_b;
  real_T c7_b_y;
  real_T c7_f_x;
  real_T c7_f_b;
  real_T c7_f_a;
  real_T c7_c_y;
  real_T c7_g_x;
  real_T c7_g_b;
  real_T c7_g_a;
  real_T c7_h_a;
  real_T c7_h_x;
  real_T c7_h_b;
  real_T c7_d_y;
  real_T c7_i_x;
  real_T c7_i_b;
  real_T c7_i_a;
  real_T c7_e_y;
  real_T c7_j_a;
  real_T c7_j_x;
  real_T c7_j_b;
  real_T c7_f_y;
  real_T c7_k_a;
  real_T c7_k_b;
  real_T c7_g_y;
  real_T c7_l_a;
  real_T c7_k_x;
  real_T c7_l_b;
  real_T c7_h_y;
  real_T c7_m_a;
  real_T c7_m_b;
  real_T c7_i_y;
  real_T c7_l_x;
  real_T c7_n_b;
  real_T c7_n_a;
  real_T c7_o_a;
  real_T c7_m_x;
  real_T c7_o_b;
  real_T c7_j_y;
  real_T c7_p_a;
  real_T c7_q_a;
  real_T c7_n_x;
  real_T c7_p_b;
  real_T c7_r_a;
  real_T c7_o_x;
  real_T c7_q_b;
  real_T c7_k_y;
  real_T c7_p_x;
  real_T c7_q_x;
  real_T c7_r_b;
  real_T c7_l_y;
  real_T c7_s_a;
  real_T c7_r_x;
  real_T c7_t_a;
  real_T c7_s_x;
  real_T c7_s_b;
  real_T c7_m_y;
  real_T c7_u_a;
  real_T c7_t_x;
  real_T c7_t_b;
  real_T c7_n_y;
  real_T c7_u_b;
  real_T c7_o_y;
  real_T c7_u_x;
  real_T c7_v_b;
  real_T c7_v_a;
  real_T c7_p_y;
  real_T c7_v_x;
  real_T c7_w_b;
  real_T c7_w_a;
  real_T c7_x_a;
  real_T c7_w_x;
  real_T c7_x_b;
  real_T c7_q_y;
  real_T c7_x_x;
  real_T c7_y_b;
  real_T c7_y_a;
  real_T c7_r_y;
  real_T c7_ab_a;
  real_T c7_y_x;
  real_T c7_ab_b;
  real_T c7_s_y;
  real_T c7_bb_a;
  real_T c7_bb_b;
  real_T c7_t_y;
  real_T c7_cb_a;
  real_T c7_ab_x;
  real_T c7_cb_b;
  real_T c7_u_y;
  real_T c7_db_a;
  real_T c7_db_b;
  real_T c7_v_y;
  real_T c7_bb_x;
  real_T c7_eb_b;
  real_T c7_eb_a;
  real_T c7_fb_a;
  real_T c7_gb_a;
  real_T c7_cb_x;
  real_T c7_fb_b;
  real_T c7_hb_a;
  real_T c7_db_x;
  real_T c7_gb_b;
  real_T c7_w_y;
  real_T c7_eb_x;
  real_T c7_fb_x;
  real_T c7_hb_b;
  real_T c7_x_y;
  real_T c7_ib_a;
  real_T c7_gb_x;
  real_T c7_ib_b;
  real_T c7_y_y;
  real_T c7_hb_x;
  real_T c7_jb_b;
  real_T c7_jb_a;
  real_T c7_ab_y;
  real_T c7_kb_a;
  real_T c7_ib_x;
  real_T c7_lb_a;
  real_T c7_jb_x;
  real_T c7_kb_b;
  real_T c7_bb_y;
  real_T c7_mb_a;
  real_T c7_kb_x;
  real_T c7_lb_b;
  real_T c7_cb_y;
  real_T c7_mb_b;
  real_T c7_db_y;
  real_T c7_lb_x;
  real_T c7_nb_b;
  real_T c7_nb_a;
  real_T c7_ob_a;
  real_T c7_mb_x;
  real_T c7_ob_b;
  real_T c7_eb_y;
  real_T c7_nb_x;
  real_T c7_pb_b;
  real_T c7_pb_a;
  real_T c7_fb_y;
  real_T c7_qb_a;
  real_T c7_ob_x;
  real_T c7_qb_b;
  real_T c7_rb_a;
  real_T c7_pb_x;
  real_T c7_rb_b;
  real_T c7_sb_a;
  real_T c7_qb_x;
  real_T c7_sb_b;
  real_T c7_gb_y;
  real_T c7_rb_x;
  real_T c7_tb_b;
  real_T c7_tb_a;
  real_T c7_hb_y;
  real_T c7_ub_a;
  real_T c7_sb_x;
  real_T c7_ub_b;
  real_T c7_vb_a;
  real_T c7_tb_x;
  real_T c7_vb_b;
  real_T c7_ib_y;
  real_T c7_ub_x;
  real_T c7_wb_b;
  real_T c7_wb_a;
  real_T c7_jb_y;
  int32_T c7_i12;
  int32_T c7_i13;
  int32_T c7_i14;
  int32_T c7_i15;
  int32_T c7_i16;
  int32_T c7_i17;
  int32_T c7_i18;
  int32_T c7_i19;