feat:实现电机设置和读取寄存器功能

src/main/java/com/iflytop/gd/infrastructure/devices/StandardStepMotor.java

@@ -2,6 +2,7 @@ package com.iflytop.gd.infrastructure.devices;
 
 import com.iflytop.gd.infrastructure.drivers.CmdId;
 import com.iflytop.gd.infrastructure.drivers.ModuleId;
+import com.iflytop.gd.infrastructure.drivers.RegIndex;
 import com.iflytop.gd.system.constants.DistanceUnit;
 import com.iflytop.gd.system.constants.RotationDirection;
 import com.iflytop.gd.system.devices.StepMotor;
@@ -13,6 +14,9 @@ import com.iflytop.gd.system.models.DataPacket;
 import java.io.IOException;
 import java.util.concurrent.TimeUnit;
 
+/**
+ * 标准电机实现
+ */
 public class StandardStepMotor implements StepMotor {
     protected final ModuleId moduleId;
     protected final CommandBus commandBus;
@@ -143,12 +147,15 @@ public class StandardStepMotor implements StepMotor {
     }
 
     @Override
-    public void setReg() {
-
+    public void setReg(RegIndex regIndex, Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        DataPacket dataPacket = DataPacket.createCommandDataPacket(moduleId.index, CmdId.module_set_reg.index, value);
+        commandBus.waitForCommandExec(dataPacket, DEFAULT_COMMAND_EXEC_TIMEOUT_SECONDS, TimeUnit.SECONDS);
     }
 
     @Override
-    public Integer readReg() {
-        return 0;
+    public Integer readReg(RegIndex regIndex) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException {
+        DataPacket commandDataPacket = DataPacket.createCommandDataPacket(moduleId.index, CmdId.module_get_reg.index);
+        DataPacket ackDataPacket = commandBus.waitForCommandExec(commandDataPacket, DEFAULT_COMMAND_EXEC_TIMEOUT_SECONDS, TimeUnit.SECONDS);
+        return ackDataPacket.getContentI32(0);
     }
 }

src/main/java/com/iflytop/gd/system/devices/StepMotor.java

@@ -1,5 +1,6 @@
 package com.iflytop.gd.system.devices;
 
+import com.iflytop.gd.infrastructure.drivers.RegIndex;
 import com.iflytop.gd.system.constants.DistanceUnit;
 import com.iflytop.gd.system.constants.RotationDirection;
 import com.iflytop.gd.system.exceptions.CommandExecTimeoutException;
@@ -74,8 +75,26 @@ public interface StepMotor {
 
     Boolean isReturnToZero();
 
-    void setReg();
+    /**
+     * 设置模块寄存器值
+     * @param regIndex
+     * @param value
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    void setReg(RegIndex regIndex, Integer value) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 
-    Integer readReg();
 
+    /**
+     * 读取模块寄存器值
+     * @param regIndex
+     * @return
+     * @throws HardwareErrorException
+     * @throws CommandExecTimeoutException
+     * @throws IOException
+     * @throws InterruptedException
+     */
+    Integer readReg(RegIndex regIndex) throws HardwareErrorException, CommandExecTimeoutException, IOException, InterruptedException;
 }

src/main/java/com/iflytop/gd/system/models/DataPacket.java

@@ -102,4 +102,8 @@ public class DataPacket {
     public int getDataLen() {
         return ByteArray.readU8bit(raw, DATA_LEN_OFFSET);
     }
+
+    public int getContentI32(int index) {
+        return ByteArray.read32bit(raw, DATA_OFFSET + index * 4);
+    }
 }