diff --git a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 16b3edb..11867ad 100644 --- a/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp @@ -198,42 +198,40 @@ int32_t XYRobotCtrlModule::module_reset_reg() { int32_t XYRobotCtrlModule::module_set_reg(int32_t regindex, int32_t val) { int32_t ret = 0; switch (regindex) { - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_robot_type,m_cfg.robot_type); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_one_circle_pulse,m_cfg.one_circle_pulse); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_one_circle_pulse_denominator,m_cfg.one_circle_pulse_denominator); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_ihold,m_cfg.ihold); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_irun,m_cfg.irun); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_iholddelay,m_cfg.iholddelay); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_iglobalscaler,m_cfg.iglobalscaler); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_vstart,m_cfg.vstart); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_a1,m_cfg.a1); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_amax,m_cfg.amax); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_v1,m_cfg.v1); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_dmax,m_cfg.dmax); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_d1,m_cfg.d1); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_vstop,m_cfg.vstop); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_tzerowait,m_cfg.tzerowait); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_enc_resolution,m_cfg.enc_resolution); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_enable_enc,m_cfg.enable_enc); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_x_shaft,m_cfg.x_shaft); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_y_shaft,m_cfg.y_shaft); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_default_velocity,m_cfg.default_velocity); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_shift_x,m_cfg.shift_x); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_shift_y,m_cfg.shift_y); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_min_x,m_cfg.min_x); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_min_y,m_cfg.min_y); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_max_x,m_cfg.max_x); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_max_y,m_cfg.max_y); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_run_to_zero_speed,m_cfg.run_to_zero_speed); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_look_zero_edge_speed,m_cfg.look_zero_edge_speed); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_pos_devi_tolerance,m_cfg.pos_devi_tolerance); - MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_io_trigger_append_distance,m_cfg.io_trigger_append_distance); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_robot_type, m_cfg.robot_type); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_one_circle_pulse, m_cfg.one_circle_pulse); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_one_circle_pulse_denominator, m_cfg.one_circle_pulse_denominator); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_ihold, m_cfg.ihold); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_irun, m_cfg.irun); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_iholddelay, m_cfg.iholddelay); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_iglobalscaler, m_cfg.iglobalscaler); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_vstart, m_cfg.vstart); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_a1, m_cfg.a1); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_amax, m_cfg.amax); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_v1, m_cfg.v1); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_dmax, m_cfg.dmax); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_d1, m_cfg.d1); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_vstop, m_cfg.vstop); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_tzerowait, m_cfg.tzerowait); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_enc_resolution, m_cfg.enc_resolution); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_enable_enc, m_cfg.enable_enc); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_x_shaft, m_cfg.x_shaft); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_y_shaft, m_cfg.y_shaft); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_default_velocity, m_cfg.default_velocity); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_shift_x, m_cfg.shift_x); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_shift_y, m_cfg.shift_y); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_min_x, m_cfg.min_x); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_min_y, m_cfg.min_y); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_max_x, m_cfg.max_x); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_max_y, m_cfg.max_y); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_run_to_zero_speed, m_cfg.run_to_zero_speed); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_look_zero_edge_speed, m_cfg.look_zero_edge_speed); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_pos_devi_tolerance, m_cfg.pos_devi_tolerance); + MODULE_REG_CASE__SET_REG_TO(kreg_xyrobot_io_trigger_append_distance, m_cfg.io_trigger_append_distance); MODULE_REG_CASE__NOT_SUPPORT(kreg_xyrobot_has_move_to_zero); MODULE_REG_CASE__NOT_SUPPORT(kreg_xyrobot_is_enable); - - default: return err::kmodule_not_find_reg; break; @@ -244,43 +242,41 @@ int32_t XYRobotCtrlModule::module_set_reg(int32_t regindex, int32_t val) { return ret; } int32_t XYRobotCtrlModule::module_get_reg(int32_t regindex, int32_t* val) { - int32_t ret = 0; switch (regindex) { - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_robot_type,m_cfg.robot_type); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_one_circle_pulse,m_cfg.one_circle_pulse); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_one_circle_pulse_denominator,m_cfg.one_circle_pulse_denominator); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_ihold,m_cfg.ihold); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_irun,m_cfg.irun); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_iholddelay,m_cfg.iholddelay); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_iglobalscaler,m_cfg.iglobalscaler); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_vstart,m_cfg.vstart); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_a1,m_cfg.a1); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_amax,m_cfg.amax); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_v1,m_cfg.v1); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_dmax,m_cfg.dmax); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_d1,m_cfg.d1); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_vstop,m_cfg.vstop); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_tzerowait,m_cfg.tzerowait); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_enc_resolution,m_cfg.enc_resolution); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_enable_enc,m_cfg.enable_enc); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_x_shaft,m_cfg.x_shaft); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_y_shaft,m_cfg.y_shaft); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_default_velocity,m_cfg.default_velocity); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_shift_x,m_cfg.shift_x); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_shift_y,m_cfg.shift_y); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_min_x,m_cfg.min_x); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_min_y,m_cfg.min_y); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_max_x,m_cfg.max_x); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_max_y,m_cfg.max_y); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_run_to_zero_speed,m_cfg.run_to_zero_speed); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_look_zero_edge_speed,m_cfg.look_zero_edge_speed); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_pos_devi_tolerance,m_cfg.pos_devi_tolerance); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_io_trigger_append_distance,m_cfg.io_trigger_append_distance); - - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_has_move_to_zero,m_state.has_move_to_zero); - MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_is_enable,m_state.is_enable); - + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_robot_type, m_cfg.robot_type); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_one_circle_pulse, m_cfg.one_circle_pulse); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_one_circle_pulse_denominator, m_cfg.one_circle_pulse_denominator); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_ihold, m_cfg.ihold); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_irun, m_cfg.irun); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_iholddelay, m_cfg.iholddelay); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_iglobalscaler, m_cfg.iglobalscaler); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_vstart, m_cfg.vstart); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_a1, m_cfg.a1); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_amax, m_cfg.amax); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_v1, m_cfg.v1); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_dmax, m_cfg.dmax); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_d1, m_cfg.d1); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_vstop, m_cfg.vstop); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_tzerowait, m_cfg.tzerowait); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_enc_resolution, m_cfg.enc_resolution); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_enable_enc, m_cfg.enable_enc); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_x_shaft, m_cfg.x_shaft); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_y_shaft, m_cfg.y_shaft); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_default_velocity, m_cfg.default_velocity); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_shift_x, m_cfg.shift_x); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_shift_y, m_cfg.shift_y); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_min_x, m_cfg.min_x); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_min_y, m_cfg.min_y); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_max_x, m_cfg.max_x); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_max_y, m_cfg.max_y); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_run_to_zero_speed, m_cfg.run_to_zero_speed); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_look_zero_edge_speed, m_cfg.look_zero_edge_speed); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_pos_devi_tolerance, m_cfg.pos_devi_tolerance); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_io_trigger_append_distance, m_cfg.io_trigger_append_distance); + + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_has_move_to_zero, m_state.has_move_to_zero); + MODULE_CFG_CASE__GET_REG_FROM(kreg_xyrobot_is_enable, m_state.is_enable); default: return err::kmodule_not_find_reg; @@ -329,7 +325,7 @@ int32_t XYRobotCtrlModule::do_xymotor_move_to_zero() { int32_t ecode = exec_move_to_zero_task(); after_motor_move(); if (ecode == 0) { - setnowpos(0, 0); + setnowpos(0 + m_cfg.shift_x, 0 + m_cfg.shift_y); m_state.has_move_to_zero = 1; } return; @@ -451,12 +447,12 @@ void XYRobotCtrlModule::inverse_kinematics(int32_t m1pos, int32_t m2pos, int32_t if (m_cfg.x_shaft) x = -x; if (m_cfg.y_shaft) y = -y; - x += m_cfg.shift_x; - y += m_cfg.shift_y; + // x += m_cfg.shift_x; + // y += m_cfg.shift_y; } void XYRobotCtrlModule::forward_kinematics(int32_t x, int32_t y, int32_t& m1pos, int32_t& m2pos) { - x -= m_cfg.shift_x; - y -= m_cfg.shift_y; + // x -= m_cfg.shift_x; + // y -= m_cfg.shift_y; if (m_cfg.x_shaft) x = -x; if (m_cfg.y_shaft) y = -y; diff --git a/usrc/version.h b/usrc/version.h index aeaddff..e21d1fb 100644 --- a/usrc/version.h +++ b/usrc/version.h @@ -1,2 +1,2 @@ #pragma once -#define APP_VERSION 1201 +#define APP_VERSION 1202