diff --git a/Intelligent_winding_robot_main_board.launch b/Intelligent_winding_robot_main_board.launch index 0c88f6c..951f0cd 100644 --- a/Intelligent_winding_robot_main_board.launch +++ b/Intelligent_winding_robot_main_board.launch @@ -36,8 +36,8 @@ - - + + diff --git a/sdk b/sdk index ef7a7f2..4257580 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit ef7a7f2a43038c8655ccfac5d80bab8984a3963a +Subproject commit 4257580790dc2dda82d0b03a1323746f321331b3 diff --git a/usrc/main.cpp b/usrc/main.cpp index 326f26c..4670a53 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -152,6 +152,9 @@ StepMotor45Scheduler step_motor45_scheduler; // 45 CmdSchedulerV2 g_cmdScheduler; // 串口字符串指令总线 TaoJingChiScreenService g_taojingchi_screen_service; // 淘精驰屏幕服务 MicroComputerModuleDeviceScriptCmderPaser g_zmodule_device_script_cmder_paser; // 用于解析所有的设备指令 +ScriptCmderEq20Servomotor g_eq20_servomotor_script_cmder; // eq20 +ScriptCmderStepMotor45 g_step_motor45_script_cmder; // 45步进电机 +ScirptCmderMiniServoMotorCtrlModule g_mini_servo_motor_script_cmder; // 小舵机 /******************************************************************************* * 本地设备 * @@ -178,226 +181,8 @@ 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 script_reg_fn(); +void regfn() { script_reg_fn(); } extern void step_motor_cmd_reg(); void Main::run() { @@ -460,6 +245,24 @@ void Main::run() { *******************************************************************************/ g_cmdScheduler.initialize(&DEBUG_UART, 1000); // g_zmodule_device_script_cmder_paser.initialize(&g_cmdScheduler, &g_zmodule_device_manager); + g_eq20_servomotor_script_cmder.initialize(&g_cmdScheduler); + g_eq20_servomotor_script_cmder.regmodule(1, &g_main_servo_motor); + + g_step_motor45_script_cmder.initialize(&g_cmdScheduler); + g_step_motor45_script_cmder.regmodule(1, &g_step_motor45[0]); + g_step_motor45_script_cmder.regmodule(2, &g_step_motor45[1]); + g_step_motor45_script_cmder.regmodule(3, &g_step_motor45[2]); + + g_mini_servo_motor_script_cmder.initialize(&g_cmdScheduler); + g_mini_servo_motor_script_cmder.regmodule(1, &g_mini_servo[0]); + g_mini_servo_motor_script_cmder.regmodule(2, &g_mini_servo[1]); + g_mini_servo_motor_script_cmder.regmodule(3, &g_mini_servo[2]); + g_mini_servo_motor_script_cmder.regmodule(4, &g_mini_servo[3]); + g_mini_servo_motor_script_cmder.regmodule(5, &g_mini_servo[4]); + g_mini_servo_motor_script_cmder.regmodule(6, &g_mini_servo[5]); + + + g_taojingchi_screen_service.initialize( &huart5, 1, /** diff --git a/usrc/script_reg.cpp b/usrc/script_reg.cpp new file mode 100644 index 0000000..c170559 --- /dev/null +++ b/usrc/script_reg.cpp @@ -0,0 +1,244 @@ +#include +#include + +#include "main.hpp" + +// +#include "feite_servo_motor.hpp" +#include "sdk/os/zos.hpp" +// +// #include "sdk\components\cmdscheduler\cmd_scheduler.hpp" +#include "sdk\components\eq_20_asb_motor\eq20_servomotor.hpp" +#include "sdk\components\iflytop_can_slave_module_master_end\stepmotor.hpp" +#include "sdk\components\iflytop_can_slave_v1\iflytop_can_master.hpp" +#include "sdk\components\step_motor_45\step_motor_45.hpp" +#include "sdk\components\step_motor_45\step_motor_45_scheduler.hpp" +#include "sdk\components\zcancmder\zcanreceiver_master.hpp" +// +#include "sdk\components/step_motor_ctrl_module/step_motor_ctrl_script_cmder_module.hpp" +#include "sdk\components/xy_robot_ctrl_module/xy_robot_script_cmder_module.hpp" +#include "sdk\components\cmdscheduler\cmd_scheduler_v2.hpp" +#include "sdk\components\eq_20_asb_motor\script_cmder_eq20_servomotor.hpp" +#include "sdk\components\mini_servo_motor\mini_servo_motor_ctrl_module.hpp" +#include "sdk\components\mini_servo_motor\scirpt_cmder_mini_servo_motor_ctrl_module.hpp" +#include "sdk\components\step_motor_45\script_cmder_step_motor_45.hpp" +// #include "sdk\components\scriptcmder_module\xy_robot_script_cmder_module.hpp" +// #include "sdk\components\zcancmder_master_module/zcan_master_step_motor_ctrl_module.hpp" +// #include "sdk\components\zcancmder_master_module\zcan_xy_robot_master_module.hpp" + +#include "intelligent_winding_robot_ctrl.hpp" +#include "sdk\components\taojingchi_screen\taojingchi_screen_service.hpp" +#include "sdk\components\zprotocol_helper\micro_computer_module_device_script_cmder_paser.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\protocol_proxy.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp" +#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_script_cmder_paser.hpp" +namespace iflytop { +extern Main gmain; +}; + +using namespace iflytop; +using namespace std; + +extern "C" {} +/******************************************************************************* + * 配置 * + *******************************************************************************/ +#if 0 +static StepMotor45::cfg_t cfg4 = { + .max_pos = -1, + .enable_zero_limit = false, + .enable_max_pos_limit = false, + .mirror = true, + + .zeroPin = PinNull, + .zeroPinMirror = false, + + .driverPin = {PE0, PE2, PE4, PE6}, + .driverPinMirror = true, +}; + +static StepMotor45::cfg_t cfg5 = { + .max_pos = -1, + .enable_zero_limit = false, + .enable_max_pos_limit = false, + .mirror = true, + + .zeroPin = PinNull, + .zeroPinMirror = false, + + .driverPin = {PC13, PE5, PE3, PE1}, + .driverPinMirror = true, +}; +static StepMotor45::cfg_t cfg6 = { + .max_pos = -1, + .enable_zero_limit = false, + .enable_max_pos_limit = false, + .mirror = true, + + .zeroPin = PinNull, + .zeroPinMirror = false, + + .driverPin = {PC12, PD3, PD5, PD7}, + .driverPinMirror = true, +}; +#endif + +namespace iflytop { +/******************************************************************************* + * 基础组件 * + *******************************************************************************/ +extern ZCanCommnaderMaster m_zcanCommnaderMaster; // can总线 +extern ModbusBlockHost g_modbusblockhost; // modbus总线 +extern FeiTeServoMotor g_feiteservomotor_bus; // 飞特舵机总线 +extern ZModuleDeviceManager g_zmodule_device_manager; // 用于管理所有的设备 +extern StepMotor45Scheduler step_motor45_scheduler; // 45步进电机调度器 + +extern CmdSchedulerV2 g_cmdScheduler; // 串口字符串指令总线 +extern TaoJingChiScreenService g_taojingchi_screen_service; // 淘精驰屏幕服务 +extern MicroComputerModuleDeviceScriptCmderPaser g_zmodule_device_script_cmder_paser; // 用于解析所有的设备指令 + +/******************************************************************************* + * 本地设备 * + *******************************************************************************/ +extern Eq20ServoMotor g_main_servo_motor; +extern StepMotor45 g_step_motor45[7]; +extern MiniRobotCtrlModule g_mini_servo[6]; +/******************************************************************************* + * CAN设备 * + *******************************************************************************/ +extern ZIProtocolProxy g_xyrobotctrlmodule; +extern ZIProtocolProxy g_z_step_motor; + +extern IntelligentWindingRobotCtrl g_intelligent_winding_robot_ctrl; + +} // namespace iflytop + +extern "C" { +extern DMA_HandleTypeDef hdma_usart3_rx; +extern DMA_HandleTypeDef hdma_usart3_tx; +} + +extern "C" {} +extern DMA_HandleTypeDef hdma_usart2_rx; +extern DMA_HandleTypeDef hdma_usart2_tx; + +void script_reg_fn() { +#if 0 + 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_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_stop (id,stop_type) +#endif +#define XY_ROBOT_ID 3 + + g_cmdScheduler.regCMD("xy_robot_ctrl_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.xymotor_enable(XY_ROBOT_ID, en); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("xy_robot_ctrl_factory_reset", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t ecode = g_zmodule_device_manager.module_factory_reset(XY_ROBOT_ID); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("xy_robot_ctrl_flush", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t ecode = g_zmodule_device_manager.module_flush_cfg(XY_ROBOT_ID); + ack->setNoneAck(ecode); + }); +#if 0 + g_cmdScheduler.regCMD("xy_robot_ctrl_force_change_current_pos", "(id,x,y)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t x = atoi(paraV[1]); + int32_t y = atoi(paraV[2]); + int32_t ecode = g_zmodule_device_manager.motor_set_current_pos_by_change_shift(XY_ROBOT_ID, x, y); + ack->setNoneAck(ecode); + }); +#endif + + g_cmdScheduler.regCMD("xy_robot_ctrl_get_base_param", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { g_zmodule_device_script_cmder_paser.do_dumpconfig(paramN, paraV, ack); }); + + g_cmdScheduler.regCMD("xy_robot_ctrl_move_by", "(id,dx,dy,v)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t dx = atoi(paraV[1]); + int32_t dy = atoi(paraV[2]); + int32_t v = atoi(paraV[3]); + int32_t ecode = g_zmodule_device_manager.xymotor_move_by(XY_ROBOT_ID, dx, dy, v); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("xy_robot_ctrl_move_to", "(id,x,y,speed)", 4, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t x = atoi(paraV[1]); + int32_t y = atoi(paraV[2]); + int32_t speed = atoi(paraV[3]); + int32_t ecode = g_zmodule_device_manager.xymotor_move_to(XY_ROBOT_ID, x, y, speed); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("xy_robot_ctrl_move_to_zero", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t ecode = g_zmodule_device_manager.xymotor_move_to_zero(XY_ROBOT_ID); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("xy_robot_ctrl_move_to_zero_with_calibrate", "(id,nowx,nowy)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t nowx = atoi(paraV[1]); + int32_t nowy = atoi(paraV[2]); + int32_t ecode = g_zmodule_device_manager.xymotor_move_to_zero_and_calculated_shift(XY_ROBOT_ID); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("xy_robot_ctrl_stop", "(id,stop_type)", 2, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t stop_type = atoi(paraV[1]); + int32_t ecode = g_zmodule_device_manager.module_stop(XY_ROBOT_ID); + ack->setNoneAck(ecode); + }); +#if 0 +00102227 INFO [CMD ] step_motor_ctrl_enable (id,en) +00102251 INFO [CMD ] step_motor_ctrl_get_base_param (id) +00102277 INFO [CMD ] step_motor_ctrl_move_by (id,dx,speed) +00102283 INFO [CMD ] step_motor_ctrl_move_to (id,x,speed) +00102298 INFO [CMD ] step_motor_ctrl_move_to_zero (id) +00102304 INFO [CMD ] step_motor_ctrl_move_to_zero_with_calibrate (id,x) +00102311 INFO [CMD ] step_motor_ctrl_read_detailed_status (id) +00102318 INFO [CMD ] step_motor_ctrl_read_status (id) +00102330 INFO [CMD ] step_motor_ctrl_rotate (id,speed,lastforms) +00102362 INFO [CMD ] step_motor_ctrl_stop (id,stop_type) +#endif + + g_cmdScheduler.regCMD("step_motor_ctrl_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(4, en); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("step_motor_ctrl_get_base_param", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { // + g_zmodule_device_script_cmder_paser.do_dumpconfig(4); + }); + + g_cmdScheduler.regCMD("step_motor_ctrl_move_by", "(id,dx,speed)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t dx = atoi(paraV[1]); + int32_t speed = atoi(paraV[2]); + int32_t ecode = g_zmodule_device_manager.motor_move_by(4, dx, speed, 0); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("step_motor_ctrl_move_to", "(id,x,speed)", 3, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t x = atoi(paraV[1]); + int32_t speed = atoi(paraV[2]); + int32_t ecode = g_zmodule_device_manager.motor_move_to(4, x, speed, 0); + ack->setNoneAck(ecode); + }); + + g_cmdScheduler.regCMD("step_motor_ctrl_move_to_zero", "(id)", 1, [](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t ecode = g_zmodule_device_manager.motor_move_to_zero_backward(4, 0, 0, 0, 0); + ack->setNoneAck(ecode); + }); + + +}