update

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) {

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

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

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;
-
   });
 }