project_boditech_vidas_a8000_v3
/
a8000_subboard
2
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

调整hbot零点偏移实现方案

master
zhaohe 3 months ago
parent
9ef4b470a8
commit
03daf2c8ed
2 changed files with 69 additions and 73 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 140
      sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
  2. 2
      usrc/version.h

140
sdk/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp
View File

@ -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;

2
usrc/version.h
View File

@ -1,2 +1,2 @@
#pragma once
#define APP_VERSION 1201
#define APP_VERSION 1202
Write Preview
Loading…
Cancel
Save