基于模型的NXT SCARA机器手臂设计（11）

旭日东升

11 读者挑战

对于读者我提出以下挑战，感兴趣可以尝试。

z   控制器性能提升（增益校准，控制器逻辑改进，等等）创建新轨迹

z   计算和仿真NXT SCARA 运动方程

z   NXT SCARA 硬件结构再优化

附录生成代码

该附录完成nxtscara_controller.mdl生成的任务执行代码，为了减少篇幅，减少注释部分。

#include "nxtscara_app.h"
#include "nxtscara_app_private.h"

BlockIO rtB;
D_Work rtDWork;
void task_init(void)
{
  rtDWork.motor_number = 1U;
  rtDWork.operation_mode = 1U;
  rtDWork.timer_trj = MAX_uint32_T;
  rtDWork.pen_idx = 0U;
}

uint8_T rt_modu8_f_pw(uint8_T u0, uint8_T u1)
{
  uint8_T y;
  if (u1 == 0) {
    y = u0;
  } else {
    y = (uint8_T)(u0 % u1);
  }

  return y;
}

void task_ts1_Start(void)
{
  rtDWork.ud_theta1m_ref = -5.28738965E-5F;
  rtDWork.ud_engage1 = 1;
  rtDWork.ud_theta2m_ref = 9.87497842E-5F;
  rtDWork.ud_engage2 = 1;
}

void task_ts1(void)
{
  int16_T rtb_DataTypeConversion3_e;
  int16_T rtb_DataTypeConversion4_f;
  int16_T rtb_DataTypeConversion2;
  int16_T rtb_DataTypeConversion1;
  boolean_T rtb_DataTypeConversion5;
  boolean_T rtb_DataTypeConversion1_i;
  uint32_T rtb_DataStoreRead3_dz;
  uint32_T rtb_Switch1_o;
  real32_T rtb_Sum7;
  real32_T rtb_Switch2_j;
  real32_T rtb_Divide;
  real32_T rtb_gear_ratio1;
  real32_T rtb_Sum2;
  uint32_T rtb_Sum1_h;
  uint32_T rtb_Sum1_k;
  uint8_T rtb_DataStoreRead1_l;
  uint32_T rtb_Sum1_g;
  int8_T rtb_Switch2_m;
  int8_T rtb_Switch2_o;
  real32_T rtb_Switch1_m;
  int32_T rtb_Switch1;
  int8_T rtb_Switch2_k;
  int32_T rtb_Sum1_h_0;
  uint8_T rtb_DataStoreRead3_dz_0;
  int8_T rtb_Switch1_m_0;
  rtb_DataTypeConversion5 = (ecrobot_get_touch_sensor(NXT_PORT_S1) != 0);
  rtb_DataTypeConversion1_i = (ecrobot_get_touch_sensor(NXT_PORT_S2) != 0);
  if (rtDWork.operation_mode == 1) {
    if (rtDWork.timer_trj > 91500U) {
      if (rtb_DataTypeConversion5) {
        rtDWork.timer_trj = 0U;
      } else {
        rtDWork.timer_trj = MAX_uint32_T;
      }

      rtDWork.pen_idx = 0U;
      rtb_Switch2_m = 0;
      rtb_Switch2_o = 0;
      rtb_Switch2_k = 0;
    } else {
      rtb_DataStoreRead3_dz = rtDWork.timer_trj;
      rtb_Switch1_o = rtDWork.timer_trj / 50U;
      rtb_Sum1_g = rtb_Switch1_o <= 1819U ? rtb_Switch1_o : 1819U;
      rtb_Switch2_j = rtConstP.Constant1_Value[rtb_Switch1_o <= 1819U ?
        rtb_Switch1_o : 1819U] * rtConstP.Constant1_Value[rtb_Switch1_o <= 1819U
        ? rtb_Switch1_o : 1819U] + rtConstP.Constant3_Value[rtb_Sum1_g] *
        rtConstP.Constant3_Value[rtb_Sum1_g];
      rtb_Divide = 4.0F * rtb_Switch2_j * 0.013924F;
      rtb_Switch2_j -= 0.0184960011F;
      rtb_gear_ratio1 = (atan2f(rtConstP.Constant1_Value[rtb_Switch1_o <= 1819U ?
        rtb_Switch1_o : 1819U], rtConstP.Constant3_Value[rtb_Sum1_g]) - atan2f
                         (sqrtf(fabsf(rtb_Divide - (rtb_Switch2_j + 0.013924F) *
                            (rtb_Switch2_j + 0.013924F))), rtb_Switch2_j +
                          0.013924F)) * 57.2957802F * 84.0F;
      rtb_Sum2 = rtb_gear_ratio1 + rtDWork.ud_backlash1;
      rtDWork.theta1m_ref_bl = rtb_Sum2;
      if (rtDWork.timer_trj < 91000U) {
        rtb_Divide = rtb_gear_ratio1 - rtDWork.ud_theta1m_ref;
        rtb_DataTypeConversion5 = (fabsf(rtb_Divide) > 3.0F);
        if ((rtDWork.ud_engage1 == -1) && (rtb_Divide > 0.0F) &&
            rtb_DataTypeConversion5) {
          rtb_Switch1_m = 440.0F;
        } else if (rtb_DataTypeConversion5 && (rtb_Divide < 0.0F) &&
                   (rtDWork.ud_engage1 == 1)) {
          rtb_Switch1_m = -440.0F;
        } else {
          rtb_Switch1_m = 0.0F;
        }
      } else if (rtDWork.ud_engage1 == 1) {
        rtb_Switch1_m = 0.0F;
      } else {
        rtb_Switch1_m = 440.0F;
      }

      rtb_Sum7 = atan2f(sqrtf(fabsf(0.00103015325F - (rtb_Switch2_j - 0.013924F)
        * (rtb_Switch2_j - 0.013924F))), rtb_Switch2_j - 0.013924F) *
        57.2957802F * 84.0F;
      rtb_Switch2_j = rtb_Sum7 + rtDWork.ud_backlash2;
      rtDWork.theta2m_ref_bl = rtb_Switch2_j;
      if (rtDWork.timer_trj < 91000U) {
        rtb_Divide = rtb_Sum7 - rtDWork.ud_theta2m_ref;
        rtb_DataTypeConversion5 = (fabsf(rtb_Divide) > 3.0F);
        if ((rtDWork.ud_engage2 == -1) && (rtb_Divide > 0.0F) &&
            rtb_DataTypeConversion5) {
          rtb_Switch1 = 140;
        } else if (rtb_DataTypeConversion5 && (rtb_Divide < 0.0F) &&
                   (rtDWork.ud_engage2 == 1)) {
          rtb_Switch1 = -140;
        } else {
          rtb_Switch1 = 0;
        }
      } else if (rtDWork.ud_engage2 == 1) {
        rtb_Switch1 = 0;
      } else {
        rtb_Switch1 = 140;
      }

      if (((real32_T)ecrobot_get_motor_rev(NXT_PORT_C) < 7560.0F) && ((real32_T)
           ecrobot_get_motor_rev(NXT_PORT_C) > -7560.0F)) {
        rtb_Divide = (rtb_Sum2 - (real32_T)ecrobot_get_motor_rev(NXT_PORT_C)) /
          50.0F;
        rtb_Divide = (rtb_Divide < 0.0F ? -1.0F : rtb_Divide > 0.0F ? 1.0F :
                      rtb_Divide) * 4.40295219F + 107.505577F * rtb_Divide;
        rtb_Sum2 = rtb_Divide >= 100.0F ? 100.0F : rtb_Divide <= -100.0F ?
          -100.0F : rtb_Divide;
      } else {
        rtb_Sum2 = ((real32_T)ecrobot_get_motor_rev(NXT_PORT_C) < 0.0F ? -1.0F :
                    (real32_T)ecrobot_get_motor_rev(NXT_PORT_C) > 0.0F ? 1.0F :
                    (real32_T)ecrobot_get_motor_rev(NXT_PORT_C)) * -100.0F;
      }

      if (((real32_T)ecrobot_get_motor_rev(NXT_PORT_B) < 11760.0F) && ((real32_T)
           ecrobot_get_motor_rev(NXT_PORT_B) > -11760.0F)) {
        rtb_Divide = (rtb_Switch2_j - (real32_T)ecrobot_get_motor_rev(NXT_PORT_B))
          / 50.0F;
        rtb_Divide = (rtb_Divide < 0.0F ? -1.0F : rtb_Divide > 0.0F ? 1.0F :
                      rtb_Divide) * 2.39175367F + 106.748F * rtb_Divide;
        rtb_Switch2_j = rtb_Divide >= 100.0F ? 100.0F : rtb_Divide <= -100.0F ?
          -100.0F : rtb_Divide;
      } else {
        rtb_Switch2_j = ((real32_T)ecrobot_get_motor_rev(NXT_PORT_B) < 0.0F ?
                         -1.0F : (real32_T)ecrobot_get_motor_rev(NXT_PORT_B) >
                         0.0F ? 1.0F : (real32_T)ecrobot_get_motor_rev
                         (NXT_PORT_B)) * -100.0F;
      }

      rtb_Sum1_h = ecrobot_get_systick_ms() + 800U;
      if (rtb_Switch1_m != 0.0F) {
        rtB.timer_en1_0 = rtb_Sum1_h;
      }

      rtb_Sum1_k = ecrobot_get_systick_ms() + 600U;
      if (rtb_Switch1 != 0) {
        rtB.timer_en2_0 = rtb_Sum1_k;
      }

      rtb_Sum1_g = ecrobot_get_systick_ms() + 2500U;
      rtb_DataStoreRead1_l = rtDWork.pen_idx;
      if (rtDWork.pen_idx <= 7) {
        rtb_Switch1_o = rtConstP.Constant2_Value_b[rtDWork.pen_idx];
      } else {
        rtb_Switch1_o = MAX_uint32_T;
      }

      rtb_DataTypeConversion5 = (rtb_Switch1_o == rtDWork.timer_trj);
      if (rtb_DataTypeConversion5 && (!rtDWork.ud_flag_pen)) {
        rtB.timer_pen_0 = rtb_Sum1_g;
      }

      rtb_Sum1_g -= rtB.timer_pen_0;
      if ((rtb_Sum1_h - rtB.timer_en1_0 >= 800U) && (rtb_Sum1_k -
           rtB.timer_en2_0 >= 600U) && (rtb_Sum1_g >= 2500U)) {
        rtb_Sum1_h_0 = 50;
      } else {
        rtb_Sum1_h_0 = 0;
      }

      rtDWork.timer_trj = (uint32_T)rtb_Sum1_h_0 + rtDWork.timer_trj;
      if (rtb_DataStoreRead3_dz == rtb_Switch1_o) {
        rtb_DataStoreRead3_dz_0 = 1U;
      } else {
        rtb_DataStoreRead3_dz_0 = 0U;
      }

      rtDWork.pen_idx = (uint8_T)((uint32_T)rtb_DataStoreRead3_dz_0 + (uint32_T)
        rtDWork.pen_idx);
      rtb_Switch2_m = (int8_T)floorf(rtb_Sum2 + 0.5F);
      rtb_Switch2_o = (int8_T)floorf(rtb_Switch2_j + 0.5F);
      if (rtb_Sum1_g <= 500U) {
        rtb_Switch2_k = 0;
      } else if (rtb_Sum1_g <= 2000U) {
        if (rt_modu8_f_pw(rtb_DataStoreRead1_l, 2U) == 0) {
          rtb_Switch2_k = 50;
        } else {
          rtb_Switch2_k = -50;
        }
      } else {
        rtb_Switch2_k = 0;
      }

      rtDWork.ud_backlash1 = rtb_Switch1_m + rtDWork.ud_backlash1;
      rtDWork.ud_theta1m_ref = rtb_gear_ratio1;
      if (rtb_Switch1_m == 0.0F) {
        rtb_Switch1_m_0 = 1;
      } else {
        rtb_Switch1_m_0 = -1;
      }

      rtDWork.ud_engage1 = (int8_T)(rtb_Switch1_m_0 * rtDWork.ud_engage1);
      rtDWork.ud_backlash2 = (real32_T)rtb_Switch1 + rtDWork.ud_backlash2;
      rtDWork.ud_theta2m_ref = rtb_Sum7;
      if (rtb_Switch1 == 0) {
        rtb_Switch1_m_0 = 1;
      } else {
        rtb_Switch1_m_0 = -1;
      }

      rtDWork.ud_engage2 = (int8_T)(rtb_Switch1_m_0 * rtDWork.ud_engage2);
      rtDWork.ud_flag_pen = rtb_DataTypeConversion5;
    }
  } else if (rtDWork.motor_number == 1) {
    if (rtb_DataTypeConversion5) {
      rtb_Switch2_m = 50;
    } else if (rtb_DataTypeConversion1_i) {
      rtb_Switch2_m = -50;
    } else {
      rtb_Switch2_m = 0;
    }

    rtb_Switch2_o = 0;
    rtb_Switch2_k = 0;
  } else if (rtDWork.motor_number == 2) {
    rtb_Switch2_m = 0;
    if (rtb_DataTypeConversion5) {
      rtb_Switch2_o = 50;
    } else if (rtb_DataTypeConversion1_i) {
      rtb_Switch2_o = -50;
    } else {
      rtb_Switch2_o = 0;
    }

    rtb_Switch2_k = 0;
  } else {
    rtb_Switch2_m = 0;
    rtb_Switch2_o = 0;
    if (rtb_DataTypeConversion5) {
      rtb_Switch2_k = 50;
    } else if (rtb_DataTypeConversion1_i) {
      rtb_Switch2_k = -50;
    } else {
      rtb_Switch2_k = 0;
    }
  }

  ecrobot_set_motor_mode_speed(NXT_PORT_C, 1, rtb_Switch2_m);
  ecrobot_set_motor_mode_speed(NXT_PORT_B, 1, rtb_Switch2_o);
  ecrobot_set_motor_mode_speed(NXT_PORT_A, 1, rtb_Switch2_k);
  rtb_DataTypeConversion3_e = ecrobot_get_touch_sensor(NXT_PORT_S1);
  rtb_DataTypeConversion4_f = (int16_T)(rtDWork.timer_trj <= 32767U ?
    rtDWork.timer_trj : 32767U);
  rtb_DataTypeConversion2 = (int16_T)floorf(rtDWork.theta1m_ref_bl + 0.5F);
  rtb_DataTypeConversion1 = (int16_T)floorf(rtDWork.theta2m_ref_bl + 0.5F);
  ecrobot_bt_adc_data_logger(0, 0, rtb_DataTypeConversion3_e,
    rtb_DataTypeConversion4_f, rtb_DataTypeConversion2, rtb_DataTypeConversion1);
}

void task_ts2(void)
{
  uint8_T rtb_RunButton_;
  boolean_T rtb_DataTypeConversion2_g;
  boolean_T rtb_LogicalOperator2_a;
  uint8_T rtb_DataStoreRead2_p;
  boolean_T rtb_DataTypeConversion1_e;
  boolean_T rtb_LogicalOperator2_e;
  rtb_DataTypeConversion2_g = (ecrobot_is_ENTER_button_pressed() != 0);
  rtb_LogicalOperator2_a = (rtb_DataTypeConversion2_g && (!rtDWork.ud_flag_enter)
    && (rtDWork.timer_trj == MAX_uint32_T));
  rtb_DataStoreRead2_p = rtDWork.operation_mode;
  rtb_RunButton_ = (uint8_T)((uint32_T)rtb_LogicalOperator2_a + (uint32_T)
    rtDWork.operation_mode);
  if (3 == rtb_RunButton_) {
    rtDWork.operation_mode = 1U;
  } else {
    rtDWork.operation_mode = rtb_RunButton_;
  }

  rtb_DataTypeConversion1_e = (ecrobot_is_RUN_button_pressed() != 0);
  rtb_RunButton_ = rtDWork.motor_number;
  if (rtb_DataStoreRead2_p == 2) {
    rtb_LogicalOperator2_e = (rtb_DataTypeConversion1_e && (!rtDWork.ud_flag_run));
    if (rtb_LogicalOperator2_e) {
      ecrobot_sound_tone(880U, 200U, 70);
    }

    rtb_RunButton_ = (uint8_T)((uint32_T)rtb_LogicalOperator2_e + (uint32_T)
      rtb_RunButton_);
    if (4 == rtb_RunButton_) {
      rtb_RunButton_ = 1U;
    }

    rtDWork.ud_flag_run = rtb_DataTypeConversion1_e;
  } else {
    rtb_RunButton_ = 1U;
  }

  rtDWork.motor_number = rtb_RunButton_;
  if (rtb_LogicalOperator2_a) {
    ecrobot_sound_tone(440U, 600U, 70);
  }

  rtDWork.ud_flag_enter = rtb_DataTypeConversion2_g;
}

void nxtscara_app_initialize(void)
{
  task_ts1_Start();
}

参考

[1] Philo’s Home Page  LEGO Mindstorms NXT

   http://www.philohome.com/

[2] Inverse Kinematics (Wikipedia)

  http://en.wikipedia.org/wiki/Inverse_kinematics

[3] Real-Time Workshop Embedded Coder document
    http://www.mathworks.com/access/helpdesk/help/toolbox/ecoder/

捞屎人

软件能说明什么

Leon0420

这是神马意思?!