c2_robot_2link.c

Serial 2 bar robot PD control in simulink

/* Include files */
#include "robot_2link_sfun.h"
#include "c2_robot_2link.h"
#define CHARTINSTANCE_CHARTNUMBER       (chartInstance.chartNumber)
#define CHARTINSTANCE_INSTANCENUMBER    (chartInstance.instanceNumber)
#include "robot_2link_sfun_debug_macros.h"

/* Type Definitions */

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

/* Variable Declarations */

/* Variable Definitions */
static SFc2_robot_2linkInstanceStruct chartInstance;

/* Function Declarations */
static void initialize_c2_robot_2link(void);
static void enable_c2_robot_2link(void);
static void disable_c2_robot_2link(void);
static void finalize_c2_robot_2link(void);
static void sf_c2_robot_2link(void);
static real_T c2_mpower(real_T c2_x1, real_T c2_x2);
static real_T *c2_u(void);
static real_T *c2_Tau1(void);
static real_T *c2_Tau2(void);

/* Function Definitions */
static void initialize_c2_robot_2link(void)
{
  chartInstance.c2_is_active_c2_robot_2link = 0;
  if(!(cdrGetOutputPortReusable(chartInstance.S, 1) != 0)) {
  }
  if(!(cdrGetOutputPortReusable(chartInstance.S, 2) != 0)) {
  }
}

static void enable_c2_robot_2link(void)
{
}

static void disable_c2_robot_2link(void)
{
}

static void finalize_c2_robot_2link(void)
{
}

static void sf_c2_robot_2link(void)
{
  uint8_T c2_previousEvent;
  int32_T c2_em_i0;
  _sfTime_ = (real_T)ssGetT(chartInstance.S);
  for(c2_em_i0 = 0; c2_em_i0 <= 9; c2_em_i0++) {
    _SFD_DATA_RANGE_CHECK(c2_u()[c2_em_i0], 2U);
  }
  _SFD_DATA_RANGE_CHECK(*c2_Tau1(), 0U);
  _SFD_DATA_RANGE_CHECK(*c2_Tau2(), 1U);
  c2_previousEvent = _sfEvent_;
  _sfEvent_ = CALL_EVENT;
  {
    real_T c2_b_u[10];
    real_T c2_b_Tau2;
    real_T c2_b_Tau1;
    _SFD_CC_CALL(CHART_ENTER_DURING_FUNCTION_TAG,0);
    for(c2_em_i0 = 0; c2_em_i0 <= 9; c2_em_i0++) {
      c2_b_u[c2_em_i0] = c2_u()[c2_em_i0];
    }
    {
      real_T c2_m1;
      real_T c2_m2;
      real_T c2_L1;
      real_T c2_L2;
      real_T c2_g;
      real_T c2_ref1;
      real_T c2_ref2;
      real_T c2_ref3;
      real_T c2_ref4;
      real_T c2_ref5;
      real_T c2_ref6;
      real_T c2_t1;
      real_T c2_t2;
      real_T c2_t1d;
      real_T c2_t2d;
      real_T c2_thita[4];
      real_T c2_kp;
      real_T c2_kd;
      real_T c2_alpha;
      real_T c2_beta;
      real_T c2_nita;
      real_T c2_iita;
      real_T c2_thita1D;
      real_T c2_thita1Ddot;
      real_T c2_thita1Dddot;
      real_T c2_thita2D;
      real_T c2_thita2Ddot;
      real_T c2_thita2Dddot;
      real_T c2_M11;
      real_T c2_M12;
      real_T c2_M22;
      real_T c2_v1;
      real_T c2_v2;
      real_T c2_g1;
      real_T c2_g2;
      real_T c2_n1;
      real_T c2_n2;
      real_T c2_e1;
      real_T c2_e2;
      real_T c2_e4;
      real_T c2_e5;
      real_T c2_s1;
      real_T c2_s2;
      real_T c2_em_d0;
      real_T c2_x1;
      real_T c2_x2;
      real_T c2_em_d1;
      real_T c2_x;
      real_T c2_em_d2;
      real_T c2_b_x;
      real_T c2_em_d3;
      real_T c2_c_x;
      real_T c2_em_d4;
      real_T c2_d_x;
      real_T c2_em_d5;
      real_T c2_e_x;
      real_T c2_em_d6;
      real_T c2_f_x;
      real_T c2_em_d7;
      real_T c2_g_x;
      sf_debug_push_symbol_scope(46);
      sf_debug_symbol_scope_add_symbol("u", 0, 1U, 10U, 0U, 0U, 1.0, 0, 0.0, 0U,
       1, &c2_b_u);
      sf_debug_symbol_scope_add_symbol("Tau1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_b_Tau1);
      sf_debug_symbol_scope_add_symbol("Tau2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_b_Tau2);
      sf_debug_symbol_scope_add_symbol("m1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_m1);
      sf_debug_symbol_scope_add_symbol("m2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_m2);
      sf_debug_symbol_scope_add_symbol("L1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_L1);
      sf_debug_symbol_scope_add_symbol("L2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_L2);
      sf_debug_symbol_scope_add_symbol("g", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_g);
      sf_debug_symbol_scope_add_symbol("ref1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_ref1);
      sf_debug_symbol_scope_add_symbol("ref2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_ref2);
      sf_debug_symbol_scope_add_symbol("ref3", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_ref3);
      sf_debug_symbol_scope_add_symbol("ref4", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_ref4);
      sf_debug_symbol_scope_add_symbol("ref5", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_ref5);
      sf_debug_symbol_scope_add_symbol("ref6", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_ref6);
      sf_debug_symbol_scope_add_symbol("t1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_t1);
      sf_debug_symbol_scope_add_symbol("t2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_t2);
      sf_debug_symbol_scope_add_symbol("t1d", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_t1d);
      sf_debug_symbol_scope_add_symbol("t2d", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_t2d);
      sf_debug_symbol_scope_add_symbol("thita", 0, 1U, 4U, 0U, 0U, 1.0, 0, 0.0,
       0U, 1, &c2_thita);
      sf_debug_symbol_scope_add_symbol("kp", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_kp);
      sf_debug_symbol_scope_add_symbol("kd", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_kd);
      sf_debug_symbol_scope_add_symbol("alpha", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_alpha);
      sf_debug_symbol_scope_add_symbol("beta", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_beta);
      sf_debug_symbol_scope_add_symbol("nita", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_nita);
      sf_debug_symbol_scope_add_symbol("iita", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_iita);
      sf_debug_symbol_scope_add_symbol("thita1D", 0, 0U, 0U, 0U, 0U, 1.0, 0,
       0.0, 0U, 0, &c2_thita1D);
      sf_debug_symbol_scope_add_symbol("thita1Ddot", 0, 0U, 0U, 0U, 0U, 1.0, 0,
       0.0, 0U, 0, &c2_thita1Ddot);
      sf_debug_symbol_scope_add_symbol("thita1Dddot", 0, 0U, 0U, 0U, 0U, 1.0, 0,
       0.0, 0U, 0, &c2_thita1Dddot);
      sf_debug_symbol_scope_add_symbol("thita2D", 0, 0U, 0U, 0U, 0U, 1.0, 0,
       0.0, 0U, 0, &c2_thita2D);
      sf_debug_symbol_scope_add_symbol("thita2Ddot", 0, 0U, 0U, 0U, 0U, 1.0, 0,
       0.0, 0U, 0, &c2_thita2Ddot);
      sf_debug_symbol_scope_add_symbol("thita2Dddot", 0, 0U, 0U, 0U, 0U, 1.0, 0,
       0.0, 0U, 0, &c2_thita2Dddot);
      sf_debug_symbol_scope_add_symbol("M11", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_M11);
      sf_debug_symbol_scope_add_symbol("M12", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_M12);
      sf_debug_symbol_scope_add_symbol("M22", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0,
       0U, 0, &c2_M22);
      sf_debug_symbol_scope_add_symbol("v1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_v1);
      sf_debug_symbol_scope_add_symbol("v2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_v2);
      sf_debug_symbol_scope_add_symbol("g1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_g1);
      sf_debug_symbol_scope_add_symbol("g2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_g2);
      sf_debug_symbol_scope_add_symbol("n1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_n1);
      sf_debug_symbol_scope_add_symbol("n2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_n2);
      sf_debug_symbol_scope_add_symbol("e1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_e1);
      sf_debug_symbol_scope_add_symbol("e2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_e2);
      sf_debug_symbol_scope_add_symbol("e4", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_e4);
      sf_debug_symbol_scope_add_symbol("e5", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_e5);
      sf_debug_symbol_scope_add_symbol("s1", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_s1);
      sf_debug_symbol_scope_add_symbol("s2", 0, 0U, 0U, 0U, 0U, 1.0, 0, 0.0, 0U,
       0, &c2_s2);
      CV_EML_FCN(0, 0);
      /*  change these values for diff plant parameters */
      /*  also change in the plant model */
      /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,6);
      c2_m1 = 1.0;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,6);
      c2_m2 = 1.0;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,6);
      c2_L1 = 1.0;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,6);
      c2_L2 = 1.0;
      /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,9);
      c2_g = 9.8;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,10);
      c2_ref1 = c2_b_u[0];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,11);
      c2_ref2 = c2_b_u[1];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,12);
      c2_ref3 = c2_b_u[2];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,13);
      c2_ref4 = c2_b_u[3];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,14);
      c2_ref5 = c2_b_u[4];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,15);
      c2_ref6 = c2_b_u[5];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,16);
      c2_t1 = c2_b_u[6];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,17);
      c2_t2 = c2_b_u[7];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,18);
      c2_t1d = c2_b_u[8];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,19);
      c2_t2d = c2_b_u[9];
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,20);
      c2_thita[0] = c2_t1;
      c2_thita[1] = c2_t2;
      c2_thita[2] = c2_t1d;
      c2_thita[3] = c2_t2d;
      /* PD-computed torque control input */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,22);
      c2_kp = 60.0;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,22);
      c2_kd = 7.0;
      /* some defined parameters  */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,24);
      c2_alpha = (c2_m1 + c2_m2) * c2_mpower(1.0, 2.0);
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,25);
      c2_beta = c2_m2 * c2_mpower(1.0, 2.0);
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,26);
      c2_nita = c2_m2 * c2_L1 * c2_L2;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,27);
      c2_x1 = 9.8;
      c2_x2 = 1.0;
      {
        real_T c2_em_d8;
        real_T c2_h_x;
        real_T c2_y;
        c2_h_x = c2_x1;
        c2_y = c2_x2;
        c2_em_d8 = c2_h_x / c2_y;
        c2_em_d0 = c2_em_d8;
      }
      c2_iita = c2_em_d0;
      /* compute desired trajectory */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,29);
      c2_thita1D = c2_ref1;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,30);
      c2_thita1Ddot = c2_ref2;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,31);
      c2_thita1Dddot = c2_ref3;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,32);
      c2_thita2D = c2_ref4;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,33);
      c2_thita2Ddot = c2_ref5;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,34);
      c2_thita2Dddot = c2_ref6;
      /* matrix M */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,36);
      c2_x = c2_thita[1];
      c2_em_d1 = cos(c2_x);
      c2_M11 = (c2_alpha + c2_beta) + 2.0 * c2_nita * c2_em_d1;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,37);
      c2_b_x = c2_thita[1];
      c2_em_d2 = cos(c2_b_x);
      c2_M12 = c2_beta + c2_nita * c2_em_d2;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,38);
      c2_M22 = c2_beta;
      /* matrix V */
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,40);
      c2_c_x = c2_thita[1];
      c2_em_d3 = sin(c2_c_x);
      c2_v1 = -c2_nita * (2.0 * c2_thita[2] * c2_thita[3] +
        c2_mpower(c2_thita[3], 2.0)) * c2_em_d3;
      _SFD_EML_CALL(STATE_DURING_DURING_ACTION_TAG,0,41);
      c2_d_x = c2_thita[1];
      c2_em_d4 = sin(c2_d_x);
      c2_v2 = c2_nita * c2_mpower(c2_thita[2], 2.0) + c2_em_d4;