diff --git a/components/eq_20_asb_motor/eq20_servomotor.cpp b/components/eq_20_asb_motor/eq20_servomotor.cpp index 2c7cc7d..8333750 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/eq20_servomotor.cpp @@ -73,10 +73,18 @@ write_pn_bit 1 1501 0 1 DO(write_pn_bit(1501, 0, 1)); return 0; } + +static int32_t abs(int32_t v) { + if (v < 0) return -v; + return v; +} int32_t Eq20ServoMotor::rotate(int32_t rpm, int32_t acctime) { DO(stop()); if (rpm == 0) return 0; - return move_by(100000 * 10000, rpm, acctime); + int32_t direction = 1; + if (rpm < 0) direction = -1; + rpm = abs(rpm); + return move_by(direction * 100000 * 10000, rpm, acctime); } int32_t Eq20ServoMotor::move_to_zero_forward(int32_t lookzeropoint_rpm, int32_t findzero_edge_rpm, int32_t lookzeropoint_acc_time) { @@ -132,6 +140,12 @@ int32_t Eq20ServoMotor::stop() { DO(write_pn_bit(1501, 1, 0)); return 0; } + +int32_t Eq20ServoMotor::get_io_state(int32_t &io_state) { + DO(read_pn(1008, io_state)); + return 0; +} + // 2110 int32_t Eq20ServoMotor::get_servo_internal_state(servo_internal_status_t &state) { diff --git a/components/eq_20_asb_motor/eq20_servomotor.hpp b/components/eq_20_asb_motor/eq20_servomotor.hpp index c8e3e2a..cb35c2b 100644 --- a/components/eq_20_asb_motor/eq20_servomotor.hpp +++ b/components/eq_20_asb_motor/eq20_servomotor.hpp @@ -86,6 +86,7 @@ class Eq20ServoMotor { int32_t get_servo_internal_state(servo_internal_status_t &state); int32_t get_pos(int32_t &pos); + int32_t get_io_state(int32_t &io_state); public: int32_t write_reg(uint32_t regadd, int32_t value); diff --git a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp index dde3e4d..6488dad 100644 --- a/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp +++ b/components/eq_20_asb_motor/script_cmder_eq20_servomotor.cpp @@ -66,7 +66,7 @@ void ScriptCmderEq20Servomotor::regcmd() { REG_CMD___NO_ACK("eq20_", stop, "(id)", 1); REG_CMD_WITH_ACK("eq20_", get_pos, "(id)", 1, int32_t, ack); REG_CMD_WITH_ACK("eq20_", get_servo_internal_state, "(id)", 1, Eq20ServoMotor::servo_internal_status_t, ack); - + REG_CMD_WITH_ACK("eq20_", get_io_state, "(id)", 1, int32_t, ack); REG_CMD_WITH_ACK("eq20_", read_pn, "(id,pnadd)", 2, int32_t, con->getInt(2), ack); REG_CMD___NO_ACK("eq20_", write_pn, "(id,pnadd,value)", 3, con->getInt(2), con->getInt(3)); diff --git a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp index 0229362..c464ec2 100644 --- a/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp +++ b/components/xy_robot_ctrl_module/xy_robot_script_cmder_module.cpp @@ -25,18 +25,20 @@ static void cmd_dump_ack(I_XYRobotCtrlModule::version_t& ack) { // ZLOGI(TAG, "version:%d", ack.version); } +#define BIT(x, n) (((x) >> (n)) & 1) + static void cmd_dump_ack(I_XYRobotCtrlModule::status_t& ack) { // ZLOGI(TAG, "status :%d", ack.status); ZLOGI(TAG, "x :%d", ack.x); ZLOGI(TAG, "y :%d", ack.y); - ZLOGI(TAG, "iostate :%d", ack.iostate); + ZLOGI(TAG, "iostate :%d %d %d %d - %d %d %d %d", BIT(ack.iostate,0), BIT(ack.iostate,1), BIT(ack.iostate,2), BIT(ack.iostate,3), BIT(ack.iostate,4), BIT(ack.iostate,5), BIT(ack.iostate,6), BIT(ack.iostate,7)); } static void cmd_dump_ack(I_XYRobotCtrlModule::detailed_status_t& ack) { // ZLOGI(TAG, "status :%d", ack.status); ZLOGI(TAG, "x :%d", ack.x); ZLOGI(TAG, "y :%d", ack.y); - ZLOGI(TAG, "iostate :%d", ack.iostate); + ZLOGI(TAG, "iostate :%d %d %d %d - %d %d %d %d", BIT(ack.iostate,0), BIT(ack.iostate,1), BIT(ack.iostate,2), BIT(ack.iostate,3), BIT(ack.iostate,4), BIT(ack.iostate,5), BIT(ack.iostate,6), BIT(ack.iostate,7)); } static void cmd_dump_ack(I_XYRobotCtrlModule::base_param_t& ack) { @@ -95,7 +97,7 @@ void XYRobotScriptCmderModule::regcmd() { // const char* paramName = con->getString(2); int32_t paramVal = con->getInt(3); - DO_CMD(findmodule(con->getInt(0), &module)); + DO_CMD(findmodule(con->getInt(1), &module)); I_XYRobotCtrlModule::base_param_t status; DO_CMD(module->get_base_param(status)); @@ -144,11 +146,10 @@ void XYRobotScriptCmderModule::regcmd() { // status.run_to_zero_dec = paramVal; } else { ZLOGE(TAG, "invalid param name:%s", paramName); - return(int32_t) err::kce_param_out_of_range; + return (int32_t)err::kce_param_out_of_range; } DO_CMD(module->set_base_param(status)); return (int32_t)0; - }); }