From 45fc39c8751dafe79ecafaf176a5ba484a26b1c8 Mon Sep 17 00:00:00 2001
From: huangxiang <155373492@qq.com>
Date: Tue, 29 Apr 2025 11:43:27 +0800
Subject: [PATCH] =?UTF-8?q?feat:=E5=AE=9E=E7=8E=B0=E7=94=B5=E6=9C=BA?=
 =?UTF-8?q?=E8=AE=BE=E7=BD=AE=E5=92=8C=E8=AF=BB=E5=8F=96=E5=AF=84=E5=AD=98?=
 =?UTF-8?q?=E5=99=A8=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../infrastructure/devices/StandardStepMotor.java  | 15 ++++++++++----
 .../com/iflytop/gd/system/devices/StepMotor.java   | 23 ++++++++++++++++++++--
 .../com/iflytop/gd/system/models/DataPacket.java   |  4 ++++
 3 files changed, 36 insertions(+), 6 deletions(-)

diff --git a/src/main/java/com/iflytop/gd/infrastructure/devices/StandardStepMotor.java b/src/main/java/com/iflytop/gd/infrastructure/devices/StandardStepMotor.java
index d8a0d1d..aa4091e 100644
--- a/src/main/java/com/iflytop/gd/infrastructure/devices/StandardStepMotor.java
+++ b/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);
     }
 }
diff --git a/src/main/java/com/iflytop/gd/system/devices/StepMotor.java b/src/main/java/com/iflytop/gd/system/devices/StepMotor.java
index 003789f..00c4685 100644
--- a/src/main/java/com/iflytop/gd/system/devices/StepMotor.java
+++ b/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;
 }
diff --git a/src/main/java/com/iflytop/gd/system/models/DataPacket.java b/src/main/java/com/iflytop/gd/system/models/DataPacket.java
index f2fac00..90a0c3e 100644
--- a/src/main/java/com/iflytop/gd/system/models/DataPacket.java
+++ b/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);
+    }
 }