diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 7f62e8b..08c9439 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/README.md b/README.md
index 409e940..3f9c1d3 100644
--- a/README.md
+++ b/README.md
@@ -40,3 +40,88 @@ step_motor_rotate 2 0
舵机指令
+
+```
+eq20_enable (id,en)
+eq20_get_io_state (id)
+eq20_get_pos (id)
+eq20_get_servo_internal_state (id)
+eq20_move_by (id,pos,rpm,acctime)
+eq20_move_to (id,pos,rpm,acctime)
+eq20_move_to_zero_backward (id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)
+eq20_move_to_zero_forward (id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)
+eq20_read_pn (id,pnadd)
+eq20_read_pn_bit (id,pnadd,off)
+eq20_read_reg (id,regadd)
+eq20_read_reg_bit (id,regadd,off)
+eq20_rotate (id,rpm,acctime)
+eq20_stop (id)
+eq20_write_pn (id,pnadd,value)
+eq20_write_pn_bit (id,pnadd,off,value)
+eq20_write_reg (id,regadd,value)
+eq20_write_reg_bit (id,regadd,off,value)
+
+mini_servo_enable (id,en)
+mini_servo_get_basic_param (id)
+mini_servo_move_backward (id,torque)
+mini_servo_move_by (id,pos,speed,torque)
+mini_servo_move_forward (id,torque)
+mini_servo_move_to (id,pos,speed,torque)
+mini_servo_position_calibrate (id,pos)
+mini_servo_read_detailed_status (id)
+mini_servo_read_status (id)
+mini_servo_read_version (id)
+mini_servo_rotate (id,speed,torque,time)
+mini_servo_stop (id,stop_type)
+
+sleep_ms (ms)
+
+step_motor_45_get_default_speed (id)
+step_motor_45_get_pos (id)
+step_motor_45_get_zero_pin_state (id,pos)
+step_motor_45_is_reach_target_pos (id)
+step_motor_45_move_by (id,pos)
+step_motor_45_move_to (id,pos)
+step_motor_45_rotate (id,direction)
+step_motor_45_set_default_speed (id,speed)
+step_motor_45_set_pos (id,pos)
+step_motor_45_stop (id)
+step_motor_45_zero_calibration (id)
+step_motor_ctrl_enable (id,en)
+step_motor_ctrl_factory_reset (id)
+step_motor_ctrl_flush (id)
+step_motor_ctrl_force_change_current_pos (id,x)
+step_motor_ctrl_get_base_param (id)
+step_motor_ctrl_get_last_exec_status (id)
+step_motor_ctrl_get_logic_point (id,logic_point_id)
+step_motor_ctrl_is_busy (id)
+step_motor_ctrl_move_by (id,dx,speed)
+step_motor_ctrl_move_to (id,x,speed)
+step_motor_ctrl_move_to_logic_point (id,logic_point_id)
+step_motor_ctrl_move_to_zero (id)
+step_motor_ctrl_move_to_zero_with_calibrate (id,x)
+step_motor_ctrl_read_detailed_status (id)
+step_motor_ctrl_read_status (id)
+step_motor_ctrl_read_version (id)
+step_motor_ctrl_rotate (id,speed,lastforms)
+step_motor_ctrl_set_base_param (id,paramName,val)
+step_motor_ctrl_set_logic_point (id,logic_point_id,x,vel,acc,dec)
+step_motor_ctrl_set_logic_point_simplify (id,logic_point_id)
+step_motor_ctrl_stop (id,stop_type)
+
+xy_robot_ctrl_enable (id,en)
+xy_robot_ctrl_factory_reset (id)
+xy_robot_ctrl_flush (id)
+xy_robot_ctrl_force_change_current_pos (id,x,y)
+xy_robot_ctrl_get_base_param (id)
+xy_robot_ctrl_move_by (id,dx,dy,v)
+xy_robot_ctrl_move_by_no_limit (id,dx,dy,v)
+xy_robot_ctrl_move_to (id,x,y,speed)
+xy_robot_ctrl_move_to_zero (id)
+xy_robot_ctrl_move_to_zero_with_calibrate (id,nowx,nowy)
+xy_robot_ctrl_read_detailed_status (id)
+xy_robot_ctrl_read_status (id)
+xy_robot_ctrl_read_version (id)
+xy_robot_ctrl_set_base_param (id,paramName,val)
+xy_robot_ctrl_stop (id,stop_type)
+```
\ No newline at end of file
diff --git a/sdk b/sdk
index 1a752ec..ef7a7f2 160000
--- a/sdk
+++ b/sdk
@@ -1 +1 @@
-Subproject commit 1a752ece8b739fe743348afe09463ed8b09565be
+Subproject commit ef7a7f2a43038c8655ccfac5d80bab8984a3963a
diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp
index 275bb68..7637780 100644
--- a/usrc/intelligent_winding_robot_ctrl.cpp
+++ b/usrc/intelligent_winding_robot_ctrl.cpp
@@ -140,9 +140,9 @@ int32_t IntelligentWindingRobotCtrl::scissors_move_reset_pos() {
return 0;
} // block
int32_t IntelligentWindingRobotCtrl::scissors_cut() {
- ZLOGI(TAG, "scissors_cut %d", 24);
- DO(m_dm->motor_move_by(24, 4095, 0, 0));
- wait_module_idle(24);
+ ZLOGI(TAG, "scissors_cut %d", 22);
+ DO(m_dm->motor_move_by(22, 4095, 0, 0));
+ wait_module_idle(22);
return 0;
} // block
/**
@@ -182,6 +182,17 @@ int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_full_pos() {
wait_module_idle(21);
return 0;
}
+
+int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
+ ZLOGI(TAG, "main_shaft_run");
+ DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000));
+ return 0;
+}
+int32_t IntelligentWindingRobotCtrl::main_shaft_stop() {
+ ZLOGI(TAG, "main_shaft_stop");
+ DO(m_dm->module_stop(2));
+ return 0;
+}
/**
* @brief XY平台
*/
@@ -198,6 +209,11 @@ int32_t IntelligentWindingRobotCtrl::xy_take_clip(int32_t index) { return 0; }
int32_t IntelligentWindingRobotCtrl::xy_take_line() { return 0; } // 取线
int32_t IntelligentWindingRobotCtrl::xy_take_back_clip() { return 0; } // 放弹夹
int32_t IntelligentWindingRobotCtrl::xy_remove_line() { return 0; } // 移除线
+
+int32_t IntelligentWindingRobotCtrl::do_reset_device() {
+
+}
+int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {}
/**
* @brief
*/
@@ -284,45 +300,47 @@ int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdPa
m_dm = dm;
m_cmdparse = cmdparse;
- cmdparse->regCMD("paifei_duoji_moveto_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_reset(); });
- cmdparse->regCMD("paifei_duoji_moveto_press", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_press(); });
- cmdparse->regCMD("raoxiantance_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_reset(); });
- cmdparse->regCMD("raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ cmdparse->regCMD("app_paifei_duoji_moveto_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_reset(); });
+ cmdparse->regCMD("app_paifei_duoji_moveto_press", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_press(); });
+ cmdparse->regCMD("app_raoxiantance_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_reset(); });
+ cmdparse->regCMD("app_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
int32_t thickness = 0;
int32_t err = raoxiantance_duoji_move_to_get_thickness(ack->getAck(1));
ack->acktype = ICmdParserACK::kAckType_int32;
ack->rawlen = sizeof(int32_t);
});
- cmdparse->regCMD("yaxian_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_reset(); });
- cmdparse->regCMD("yaxian_duoji_move_to_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_press_pos(); });
- cmdparse->regCMD("yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_wait_for_press_pos(); });
- cmdparse->regCMD("xianlajin_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_reset(); });
- cmdparse->regCMD("xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_line_entry_pos(); });
- cmdparse->regCMD("xianlajin_duoji_move_to_tight_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_tight_line_pos(); });
- cmdparse->regCMD("xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_loose_line_pos(); });
- cmdparse->regCMD("jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_reset_pos(); });
- cmdparse->regCMD("jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_clamp_pos(); });
- cmdparse->regCMD("scissors_move_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_move_reset_pos(); });
- cmdparse->regCMD("scissors_cut", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_cut(); });
- cmdparse->regCMD("arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_reset_pos(); });
- cmdparse->regCMD("arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_clamp_pos(); });
- cmdparse->regCMD("arm_hook_claws_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_reset(); });
- cmdparse->regCMD("arm_hook_claws_move_to_half_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_half_pos(); });
- cmdparse->regCMD("arm_hook_claws_move_to_full_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_full_pos(); });
- cmdparse->regCMD("xy_platform_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_platform_reset(); });
- cmdparse->regCMD("z_axis_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_reset(); });
- cmdparse->regCMD("z_axis_move_to", "(int32_t pos)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_move_to(atoi(paraV[0])); });
- cmdparse->regCMD("xy_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_reset(); });
- cmdparse->regCMD("xy_move_to_zero", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_move_to_zero(); });
- cmdparse->regCMD("xy_take_clip", "(int32_t index)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_clip(atoi(paraV[0])); });
- cmdparse->regCMD("xy_take_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_line(); });
- cmdparse->regCMD("xy_take_back_clip", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_back_clip(); });
- cmdparse->regCMD("xy_remove_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_remove_line(); });
- cmdparse->regCMD("start_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return start_winding(); });
- cmdparse->regCMD("stop_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return stop_winding(); });
- cmdparse->regCMD("reset_and_check_device", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return reset_and_check_device(); });
- cmdparse->regCMD("setcfg", "(const char* cfgname, int32_t cfgvalue)", 2, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return setcfg(paraV[0], atoi(paraV[1])); });
- cmdparse->regCMD("dumpcfg", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return dumpcfg(); });
+ cmdparse->regCMD("app_yaxian_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_reset(); });
+ cmdparse->regCMD("app_yaxian_duoji_move_to_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_press_pos(); });
+ cmdparse->regCMD("app_yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_wait_for_press_pos(); });
+ cmdparse->regCMD("app_xianlajin_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_reset(); });
+ cmdparse->regCMD("app_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_line_entry_pos(); });
+ cmdparse->regCMD("app_xianlajin_duoji_move_to_tight_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_tight_line_pos(); });
+ cmdparse->regCMD("app_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_loose_line_pos(); });
+ cmdparse->regCMD("app_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_reset_pos(); });
+ cmdparse->regCMD("app_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_clamp_pos(); });
+ cmdparse->regCMD("app_scissors_move_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_move_reset_pos(); });
+ cmdparse->regCMD("app_scissors_cut", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_cut(); });
+ cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_reset_pos(); });
+ cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_clamp_pos(); });
+ cmdparse->regCMD("app_arm_hook_claws_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_reset(); });
+ cmdparse->regCMD("app_arm_hook_claws_move_to_half_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_half_pos(); });
+ cmdparse->regCMD("app_arm_hook_claws_move_to_full_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_full_pos(); });
+ cmdparse->regCMD("app_main_shaft_run", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return main_shaft_run(); });
+ cmdparse->regCMD("app_main_shaft_stop", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return main_shaft_stop(); });
+ cmdparse->regCMD("app_xy_platform_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_platform_reset(); });
+ cmdparse->regCMD("app_z_axis_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_reset(); });
+ cmdparse->regCMD("app_z_axis_move_to", "(int32_t pos)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_move_to(atoi(paraV[0])); });
+ cmdparse->regCMD("app_xy_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_reset(); });
+ cmdparse->regCMD("app_xy_move_to_zero", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_move_to_zero(); });
+ cmdparse->regCMD("app_xy_take_clip", "(int32_t index)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_clip(atoi(paraV[0])); });
+ cmdparse->regCMD("app_xy_take_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_line(); });
+ cmdparse->regCMD("app_xy_take_back_clip", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_back_clip(); });
+ cmdparse->regCMD("app_xy_remove_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_remove_line(); });
+ cmdparse->regCMD("app_start_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return start_winding(); });
+ cmdparse->regCMD("app_stop_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return stop_winding(); });
+ cmdparse->regCMD("app_reset_and_check_device", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return reset_and_check_device(); });
+ cmdparse->regCMD("app_setcfg", "(const char* cfgname, int32_t cfgvalue)", 2, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return setcfg(paraV[0], atoi(paraV[1])); });
+ cmdparse->regCMD("app_dumpcfg", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return dumpcfg(); });
return 0;
}
diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp
index f8818c7..4129588 100644
--- a/usrc/intelligent_winding_robot_ctrl.hpp
+++ b/usrc/intelligent_winding_robot_ctrl.hpp
@@ -132,6 +132,10 @@ class IntelligentWindingRobotCtrl {
int32_t arm_hook_claws_reset();
int32_t arm_hook_claws_move_to_half_pos();
int32_t arm_hook_claws_move_to_full_pos();
+
+ int32_t main_shaft_run();
+ int32_t main_shaft_stop();
+
/**
* @brief XY平台
*/
@@ -148,6 +152,7 @@ class IntelligentWindingRobotCtrl {
int32_t xy_take_line(); // 取线
int32_t xy_take_back_clip(); // 放弹夹
int32_t xy_remove_line(); // 移除线
+
/**
* @brief
*/
@@ -158,6 +163,10 @@ class IntelligentWindingRobotCtrl {
int32_t setcfg(const char* cfgname, int32_t cfgvalue);
int32_t dumpcfg();
+
+ int32_t do_reset_device();
+ int32_t do_winding(int32_t index);
+
void processError(int32_t err);
void wait_module_idle(int32_t moduleid);
diff --git a/usrc/main.cpp b/usrc/main.cpp
index 2a9b391..326f26c 100644
--- a/usrc/main.cpp
+++ b/usrc/main.cpp
@@ -178,6 +178,227 @@ extern "C" {}
extern DMA_HandleTypeDef hdma_usart2_rx;
extern DMA_HandleTypeDef hdma_usart2_tx;
+void regfn() {
+#if 0
+ eq20_enable (id,en)
+ eq20_get_io_state (id)
+ eq20_get_pos (id)
+ eq20_get_servo_internal_state (id)
+ eq20_move_by (id,pos,rpm,acctime)
+ eq20_move_to (id,pos,rpm,acctime)
+ eq20_move_to_zero_backward (id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)
+ eq20_move_to_zero_forward (id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)
+ eq20_rotate (id,rpm,acctime)
+ eq20_stop (id)
+
+ mini_servo_enable (id,en)
+ mini_servo_move_backward (id,torque)
+ mini_servo_move_by (id,pos,speed,torque)
+ mini_servo_move_forward (id,torque)
+ mini_servo_move_to (id,pos,speed,torque)
+ mini_servo_position_calibrate (id,pos)
+ mini_servo_read_status (id)
+ mini_servo_rotate (id,speed,torque,time)
+ mini_servo_stop (id,stop_type)
+#endif
+ g_cmdScheduler.regCMD("eq20_enable", "(id,en)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t en = atoi(paraV[1]);
+ int32_t ecode = g_zmodule_device_manager.motor_enable(2, en);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("eq20_get_io_state", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t en = atoi(paraV[1]);
+ int32_t iostate = 0;
+ int32_t ecode = g_zmodule_device_manager.module_readio(2, &iostate);
+ ack->setInt32Ack(ecode, iostate);
+ });
+
+ g_cmdScheduler.regCMD("eq20_get_pos", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t pos = 0;
+ int32_t ecode = g_zmodule_device_manager.motor_read_pos(2, &pos);
+ ack->setInt32Ack(ecode, pos);
+ });
+
+ g_cmdScheduler.regCMD("eq20_get_servo_internal_state", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t pos = 0;
+ int32_t ecode = g_zmodule_device_manager.motor_read_pos(2, &pos);
+ ack->setInt32Ack(ecode, pos);
+ });
+
+ g_cmdScheduler.regCMD("eq20_move_by", "(id,pos,rpm,acctime)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t pos = atoi(paraV[1]);
+ int32_t rpm = atoi(paraV[2]);
+ int32_t acctime = atoi(paraV[3]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_by_acctime(2, pos, rpm, acctime);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("eq20_move_to", "(id,pos,rpm,acctime)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t pos = atoi(paraV[1]);
+ int32_t rpm = atoi(paraV[2]);
+ int32_t acctime = atoi(paraV[3]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_to_acctime(2, pos, rpm, acctime);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("eq20_move_to_zero_backward", "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t lookzeropoint_rpm = atoi(paraV[1]);
+ int32_t findzero_edge_rpm = atoi(paraV[2]);
+ int32_t lookzeropoint_acc_time = atoi(paraV[3]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_to_zero_backward(2, lookzeropoint_rpm, findzero_edge_rpm, 0, 0);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("eq20_move_to_zero_forward", "(id,lookzeropoint_rpm,findzero_edge_rpm,lookzeropoint_acc_time)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t lookzeropoint_rpm = atoi(paraV[1]);
+ int32_t findzero_edge_rpm = atoi(paraV[2]);
+ int32_t lookzeropoint_acc_time = atoi(paraV[3]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_to_zero_forward(2, lookzeropoint_rpm, findzero_edge_rpm, 0, 0);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("eq20_rotate", "(id,rpm,acctime)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t rpm = atoi(paraV[1]);
+ int32_t acctime = atoi(paraV[2]);
+
+ int32_t absrpm = abs(rpm);
+ int32_t direction = rpm > 0 ? 1 : -1;
+
+ int32_t ecode = g_zmodule_device_manager.motor_rotate_acctime(2, direction, absrpm, acctime);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("eq20_stop", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t ecode = g_zmodule_device_manager.module_stop(2);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_enable", "(id,en)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t en = atoi(paraV[1]);
+ int32_t ecode = g_zmodule_device_manager.motor_enable(id + 10, en);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_move_backward", "(id,torque)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t torque = atoi(paraV[1]);
+ torque = abs(torque);
+ int32_t ecode = g_zmodule_device_manager.motor_move_to_with_torque(id + 10, 0, torque);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_move_by", "(id,pos,speed,torque)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t pos = atoi(paraV[1]);
+ int32_t speed = atoi(paraV[2]);
+ int32_t torque = atoi(paraV[3]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_by(id + 10, pos, speed, 0);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_move_forward", "(id,torque)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t torque = atoi(paraV[1]);
+ torque = abs(torque);
+ int32_t ecode = g_zmodule_device_manager.motor_move_to_with_torque(id + 10, 0, torque);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_move_to", "(id,pos,speed,torque)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t pos = atoi(paraV[1]);
+ int32_t speed = atoi(paraV[2]);
+ int32_t torque = atoi(paraV[3]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_to(id + 10, pos, speed, 0);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_position_calibrate", "(id,pos)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t pos = atoi(paraV[1]);
+ int32_t ecode = g_zmodule_device_manager.motor_set_current_pos_by_change_shift(id + 10, pos);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_read_status", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t pos = 0;
+ int32_t ecode = g_zmodule_device_manager.motor_read_pos(id + 10, &pos);
+ ack->setInt32Ack(ecode, pos);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_rotate", "(id,speed,torque,time)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t speed = atoi(paraV[1]);
+ int32_t torque = atoi(paraV[2]);
+ int32_t time = atoi(paraV[3]);
+
+ int direction = speed > 0 ? 1 : -1;
+ int32_t ecode = g_zmodule_device_manager.motor_rotate(id + 10, direction, speed, time);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("mini_servo_stop", "(id,stop_type)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t id = atoi(paraV[0]);
+ int32_t stop_type = atoi(paraV[1]);
+ int32_t ecode = g_zmodule_device_manager.module_stop(id + 10);
+ ack->setNoneAck(ecode);
+ });
+
+#if 0
+ step_motor_45_get_pos (id)
+ step_motor_45_move_by (id,pos)
+ step_motor_45_move_to (id,pos)
+ step_motor_45_rotate (id,direction)
+ step_motor_45_stop (id)
+ step_motor_45_zero_calibration (id)
+#endif
+
+ g_cmdScheduler.regCMD("step_motor_45_get_pos", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t pos = 0;
+ int id = atoi(paraV[0]);
+ int32_t ecode = g_zmodule_device_manager.motor_read_pos(id + 20, &pos);
+ ack->setInt32Ack(ecode, pos);
+ });
+
+ g_cmdScheduler.regCMD("step_motor_45_move_by", "(id,pos)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t pos = atoi(paraV[1]);
+ int id = atoi(paraV[0]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_by(id + 20, pos,0,0);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("step_motor_45_move_to", "(id,pos)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t pos = atoi(paraV[1]);
+ int id = atoi(paraV[0]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_to(id + 20, pos,0,0);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("step_motor_45_rotate", "(id,direction)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int32_t direction = atoi(paraV[1]);
+ int id = atoi(paraV[0]);
+ int32_t ecode = g_zmodule_device_manager.motor_rotate(id + 20, direction,0,0);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("step_motor_45_stop", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int id = atoi(paraV[0]);
+ int32_t ecode = g_zmodule_device_manager.module_stop(id + 20);
+ ack->setNoneAck(ecode);
+ });
+
+ g_cmdScheduler.regCMD("step_motor_45_zero_calibration", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
+ int id = atoi(paraV[0]);
+ int32_t ecode = g_zmodule_device_manager.motor_move_to_zero_backward(id + 20,0,0,0,0);
+ ack->setNoneAck(ecode);
+ });
+
+
+}
+
extern void step_motor_cmd_reg();
void Main::run() {
/*******************************************************************************
@@ -251,7 +472,7 @@ void Main::run() {
* @brief 陶晶驰消息解析
*/
[this](uint8_t* data, size_t len) {});
-
+ regfn();
#if 0
step_motor_cmd_reg();
#endif