add: 初步实现除modbus，相机 设备以外其他设备的控制

src/main/java/com/iflytop/gd/common/cmd/DeviceCommandParams.java

@@ -9,7 +9,7 @@ import lombok.Data;
 @Data
 public class DeviceCommandParams {
     private String device;
-    private Double current;
+    private Integer current;
     private CmdDirection direction;
     private Double position;
     private Double speed;

src/main/java/com/iflytop/gd/common/enums/cmd/CmdAction.java

@@ -1,6 +1,6 @@
 package com.iflytop.gd.common.enums.cmd;
 
 public enum CmdAction {
-    open, close, stop, start, origin, move, move_x_joint, move_y_joint, move_joint, set, get, tight, loose,
-    open_power, close_power, open_circle, close_circle, open_heart, close_heart, take_photo
+    open, close, stop, start, origin, move, move_x_joint, move_y_joint, move_joint, move_point, set, get, tight, loose,
+    open_power, close_power, open_circle, close_circle, open_heart, close_heart, open_cool, close_cool, take_photo
 }

src/main/java/com/iflytop/gd/hardware/comm/can/A8kCanBusService.java

@@ -93,10 +93,10 @@ public class A8kCanBusService {
 		return connection.callcmd(id, CmdId.module_get_version).getContentI32(0);
 	}
 
-//	public ModuleType moduleReadType(MId id) throws HardwareException {
-//		var packet = connection.callcmd(id, CmdId.module_get_type);
-//		return ModuleType.of(packet.getContentI32(0));
-//	}
+	public ModuleType moduleReadType(MId id) throws HardwareException {
+		var packet = connection.callcmd(id, CmdId.module_get_type);
+		return ModuleType.of(packet.getContentI32(0));
+	}
 
 //	public byte[] a8kIdcardReaderReadRaw() throws HardwareException {
 //		int    i      = 0;

src/main/java/com/iflytop/gd/hardware/command/CommandHandler.java

@@ -1,16 +1,72 @@
 package com.iflytop.gd.hardware.command;
 
+import cn.hutool.core.util.StrUtil;
 import com.iflytop.gd.common.cmd.DeviceCommand;
-import com.iflytop.gd.common.cmd.DeviceCommandParams;
 import com.iflytop.gd.common.enums.cmd.CmdAction;
-import com.iflytop.gd.hardware.type.A8kPacket;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.type.MId;
 
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+/**
+ * 命令处理器
+ * H - 硬件 C - 命令
+ */
 public abstract class CommandHandler {
-    protected abstract void checkAction(CmdAction action) throws Exception;
-    public abstract boolean sendCommand(DeviceCommand command) throws Exception;
 
-    protected A8kPacket sendPacket(A8kPacket packet) throws Exception {
-        // 发送数据包
-        return packet;
+    abstract protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap();
+    abstract protected Set<CmdAction> getSupportActions();
+
+    /**
+     * 发送指令
+     */
+    public void sendCommand(DeviceCommand command) throws Exception
+    {
+        // 校验动作是否合法
+        checkAction(command.getAction());
+        // 校验参数是否合法
+        checkParams(command);
+
+        // 处理指令
+        handleCommand(command);
+    }
+
+    abstract protected void handleCommand(DeviceCommand command) throws Exception;
+
+    /**
+     * 获取支持的设备
+     */
+    protected Set<CmdDevice> getSupportedDevices() {
+        Map<CmdDevice, MId> cmdDeviceMIdMap = getSupportCmdDeviceMIdMap();
+        return cmdDeviceMIdMap.keySet();
     }
+
+
+
+    /**
+     * 检查Action 是否合法
+     * @param action
+     */
+    protected void checkAction(CmdAction action) throws Exception
+    {
+        Set<CmdAction> supportedActions = getSupportActions();
+        if (!supportedActions.contains(action)) {
+            // 生成支持的动作列表字符串
+            String supported = supportedActions.stream()
+                    .map(Enum::name)
+                    .collect(Collectors.joining(", "));
+
+            throw new IllegalArgumentException(
+                    StrUtil.format("action [{}] not supported! Supported actions: {}",
+                            action.name(), supported));
+        }
+    }
+
+    /**
+     * 检查参数是否合法
+     */
+    protected void checkParams(DeviceCommand command) throws Exception
+    {}
 }

src/main/java/com/iflytop/gd/hardware/command/SupportDevice.java → src/main/java/com/iflytop/gd/hardware/command/checker/SupportDevice.java

@@ -1,4 +1,4 @@
-package com.iflytop.gd.hardware.command;
+package com.iflytop.gd.hardware.command.checker;
 
 public class SupportDevice {
 //    public enum CmdDevice {

src/main/java/com/iflytop/gd/hardware/command/SupportMethod.java → src/main/java/com/iflytop/gd/hardware/command/checker/SupportMethod.java

@@ -1,4 +1,4 @@
-package com.iflytop.gd.hardware.command;
+package com.iflytop.gd.hardware.command.checker;
 
 public class SupportMethod {
     public enum CommandMethod {

src/main/java/com/iflytop/gd/hardware/command/command_handler/DoorHandler.java

@@ -1,36 +0,0 @@
-package com.iflytop.gd.hardware.command.command_handler;
-
-import com.iflytop.gd.common.cmd.DeviceCommand;
-import com.iflytop.gd.common.enums.cmd.CmdAction;
-import com.iflytop.gd.hardware.command.CommandHandler;
-
-public class DoorHandler extends CommandHandler  {
-    @Override
-    public boolean sendCommand(DeviceCommand command) throws Exception {
-        // 校验动作
-        checkAction(command.getAction());
-        // 发送命令
-        if (command.getAction() == CmdAction.open) {
-            // 组包
-            // 发送命令
-            return true;
-        } else if (command.getAction() == CmdAction.close) {
-            return true;
-        }
-        else if (command.getAction() == CmdAction.stop) {
-            return true;
-        }
-
-        return false;
-    }
-    @Override
-    public void checkAction(CmdAction action) throws Exception {
-        if (!action.equals(CmdAction.open) && !action.equals(CmdAction.close)) {
-            throw new IllegalArgumentException("action must be 'open' or 'close'");
-        }
-    }
-}
-
-
-
-

src/main/java/com/iflytop/gd/hardware/command/command_handler/FanHandler.java

@@ -1,61 +0,0 @@
-package com.iflytop.gd.hardware.command.command_handler;
-
-import com.iflytop.gd.common.cmd.DeviceCommand;
-import com.iflytop.gd.common.cmd.DeviceCommandParams;
-import com.iflytop.gd.common.enums.cmd.CmdAction;
-import com.iflytop.gd.hardware.command.CommandHandler;
-
-public class FanHandler extends CommandHandler {
-
-    private final int fandId_;
-
-    public FanHandler(int fanId) {
-        fandId_ = fanId;
-    }
-
-    @Override
-    public boolean sendCommand(DeviceCommand command) throws Exception {
-        // 校验动作
-        checkAction(command.getAction());
-        // 发送命令
-        if (command.getAction() == CmdAction.open) {
-            // 校验参数
-            checkParams(command.getParam());
-            // get 参数值
-            Double speed = command.getParam().getSpeed();
-
-            return openFan(speed);
-        } else if (command.getAction() == CmdAction.close) {
-            return closeFan();
-        }
-        return false;
-    }
-
-    private boolean openFan(Double speed) {
-        // 组包
-        // 发送命令
-        return true;
-    }
-
-    private boolean closeFan() {
-        // 组包
-        // 发送命令
-        return true;
-    }
-
-    @Override
-    protected void checkAction(CmdAction action) throws Exception {
-        if (!action.equals(CmdAction.open) && !action.equals(CmdAction.close)) {
-            throw new IllegalArgumentException("action must be 'open' or 'close'");
-        }
-    }
-
-    private void checkParams(DeviceCommandParams params) throws Exception {
-        Double speed = params.getSpeed();
-        if (speed < 0 || speed > 100) {
-        {
-            throw new IllegalArgumentException("speed must be between 0 and 100");
-        }
-        }
-    }
-}

src/main/java/com/iflytop/gd/hardware/command/handlers/AcidPumpHandler.java

@@ -0,0 +1,75 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.AcidPumpDriver;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class AcidPumpHandler extends CommandHandler {
+    private final AcidPumpDriver acidPumpDriver_;
+
+    private final Map<CmdDevice, StepMotorMId> stepMotorMIdMap_ = Map.ofEntries(
+            Map.entry(CmdDevice.acid_pump1, StepMotorMId.ACID_PUMP_1_MOTOR_MID),
+            Map.entry(CmdDevice.acid_pump2, StepMotorMId.ACID_PUMP_2_MOTOR_MID),
+            Map.entry(CmdDevice.acid_pump3, StepMotorMId.ACID_PUMP_3_MOTOR_MID),
+            Map.entry(CmdDevice.acid_pump4, StepMotorMId.ACID_PUMP_4_MOTOR_MID),
+            Map.entry(CmdDevice.acid_pump5, StepMotorMId.ACID_PUMP_5_MOTOR_MID),
+            Map.entry(CmdDevice.acid_pump6, StepMotorMId.ACID_PUMP_6_MOTOR_MID),
+            Map.entry(CmdDevice.acid_pump7, StepMotorMId.ACID_PUMP_7_MOTOR_MID),
+            Map.entry(CmdDevice.acid_pump8, StepMotorMId.ACID_PUMP_8_MOTOR_MID)
+    );
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+        if (command.getAction() == CmdAction.origin) {
+            acidPumpDriver_.moveToHome(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.stop) {
+            acidPumpDriver_.stop(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.move) {
+            double position = command.getParam().getPosition();
+            double speed = command.getParam().getSpeed();
+            acidPumpDriver_.moveTo(stepMotorMId, position, speed);
+        }
+        else if(command.getAction() == CmdAction.set) {
+            Integer current = command.getParam().getCurrent();
+            if(current != null) {
+                acidPumpDriver_.setCurrent(stepMotorMId, current);
+            }
+            Double speed = command.getParam().getSpeed();
+            if(speed != null) {
+                acidPumpDriver_.setSpeed(stepMotorMId, speed);
+            }
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/CameraHandler.java

@@ -0,0 +1,53 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdColor;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.CameraDriver;
+import com.iflytop.gd.hardware.drivers.TricolorLightDriver;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+import java.util.Map;
+import java.util.Set;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class CameraHandler extends CommandHandler {
+    private final CameraDriver cameraDriver_;
+
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = Map.ofEntries(
+            Map.entry(CmdDevice.photo, MId.NotSet)
+    );
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.open, CmdAction.close);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    MId getMId(CmdDevice cmdDevice){
+        return supportCmdDeviceMIdMap.get(cmdDevice);
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.take_photo) {
+            cameraDriver_.takePhoto();
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/ClawHandler.java

@@ -0,0 +1,67 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.MiniServoDriver.ClawDriver;
+import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoMId;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class ClawHandler extends CommandHandler {
+    private final ClawDriver clawDriver_;
+
+    private final Map<CmdDevice, MiniServoMId> servoMIdMap_ = Map.ofEntries(
+            Map.entry(CmdDevice.claw, MiniServoMId.CLAW_MID)
+    );
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = servoMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        MiniServoMId servoMId = servoMIdMap_.get(command.getDevice());
+        if (command.getAction() == CmdAction.origin) {
+            clawDriver_.moveToHome(servoMId);
+        }
+        else if(command.getAction() == CmdAction.stop) {
+            clawDriver_.stop(servoMId);
+        }
+        else if(command.getAction() == CmdAction.move) {
+            double position = command.getParam().getPosition();
+            double speed = command.getParam().getSpeed();
+            clawDriver_.moveTo(servoMId, position, speed);
+        }
+        else if(command.getAction() == CmdAction.set) {
+            Integer current = command.getParam().getCurrent();
+            Double speed = command.getParam().getSpeed();
+            if(speed != null) {
+                clawDriver_.setSpeed(servoMId, speed);
+            }
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/ColdTrapHandler.java

@@ -0,0 +1,81 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdColor;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.ColdTrapDriver;
+import com.iflytop.gd.hardware.drivers.TricolorLightDriver;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class ColdTrapHandler extends CommandHandler {
+    private final ColdTrapDriver coldTrapDriver_;
+
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = Map.ofEntries(
+            Map.entry(CmdDevice.cold_trap, MId.NotSet)
+    );
+
+    private final Set<CmdAction> supportActions = Set.of(
+            CmdAction.open_power, CmdAction.close_power,
+            CmdAction.set,
+            CmdAction.open_circle, CmdAction.close_circle,
+            CmdAction.open_heart, CmdAction.close_heart,
+            CmdAction.open_cool, CmdAction.close_cool);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    MId getMId(CmdDevice cmdDevice){
+        return supportCmdDeviceMIdMap.get(cmdDevice);
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.open_power) {
+            coldTrapDriver_.openPower();
+        } else if (command.getAction() == CmdAction.close_power) {
+            coldTrapDriver_.closePower();
+        }
+        else if (command.getAction() == CmdAction.set) {
+            double temperature = command.getParam().getTemperature();
+            coldTrapDriver_.setTemperature(temperature);
+        }
+        else if (command.getAction() == CmdAction.open_circle) {
+            coldTrapDriver_.openCircle();
+        }
+        else if (command.getAction() == CmdAction.close_circle) {
+            coldTrapDriver_.closeCircle();
+        }
+        else if (command.getAction() == CmdAction.open_heart) {
+            coldTrapDriver_.openHeart();
+        }
+        else if (command.getAction() == CmdAction.close_heart) {
+            coldTrapDriver_.closeHeart();
+        }
+        else if (command.getAction() == CmdAction.open_cool) {
+            coldTrapDriver_.openCool();
+        }
+        else if (command.getAction() == CmdAction.close_cool) {
+            coldTrapDriver_.closeCool();
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/DoorHandler.java

@@ -0,0 +1,74 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.DoorDriver;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorCtrlDriver;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class DoorHandler extends CommandHandler  {
+    private final DoorDriver doorDriver_;
+
+    private final Map<CmdDevice, StepMotorMId> stepMotorMIdMap_ = Map.ofEntries(
+            Map.entry(CmdDevice.door, StepMotorMId.DOOR_MOTOR_MID)
+    );
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+        if (command.getAction() == CmdAction.origin) {
+            doorDriver_.moveToHome(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.stop) {
+            doorDriver_.stop(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.move) {
+            double position = command.getParam().getPosition();
+            double speed = command.getParam().getSpeed();
+            doorDriver_.moveTo(stepMotorMId, position, speed);
+        }
+        else if(command.getAction() == CmdAction.set) {
+            Integer current = command.getParam().getCurrent();
+            if(current != null) {
+                doorDriver_.setCurrent(stepMotorMId, current);
+            }
+            Double speed = command.getParam().getSpeed();
+            if(speed != null) {
+                doorDriver_.setSpeed(stepMotorMId, speed);
+            }
+        }
+    }
+}
+
+
+
+

src/main/java/com/iflytop/gd/hardware/command/handlers/DualRobotHandler.java

@@ -0,0 +1,76 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.cmd.DeviceCommandParams;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.MiniServoDriver.ClawDriver;
+import com.iflytop.gd.hardware.drivers.MiniServoDriver.DualRobotDriver;
+import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoMId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class DualRobotHandler extends CommandHandler {
+    private final DualRobotDriver dualRobotDriver_;
+
+    private final Map<CmdDevice, MiniServoMId> servoMIdMap_ = Map.ofEntries(
+            Map.entry(CmdDevice.dual_robot, MiniServoMId.NotSet)
+    );
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = servoMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    private DualRobotDriver.Joint getJoint(DeviceCommandParams param) {
+        return DualRobotDriver.Joint.valueOf(param.toString());
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.origin) {
+            dualRobotDriver_.moveToHome(MiniServoMId.DUAL_ROBOT_AXIS_1_MID);
+        }
+        else if(command.getAction() == CmdAction.stop) {
+            dualRobotDriver_.stop(getJoint(command.getParam()));
+        }
+        else if(command.getAction() == CmdAction.move) {
+            double position = command.getParam().getPosition();
+            double speed = command.getParam().getSpeed();
+            dualRobotDriver_.moveTo(getJoint(command.getParam()), position, speed);
+        }
+        else if(command.getAction() == CmdAction.move) {
+            double pos_x = command.getParam().getPosition();
+            double pos_y = command.getParam().getPosition();
+            double speed = command.getParam().getSpeed();
+            dualRobotDriver_.moveToPoint(pos_x, pos_y);
+        }
+        else if(command.getAction() == CmdAction.set) {
+            Double speed = command.getParam().getSpeed();
+            if(speed != null) {
+                dualRobotDriver_.setSpeed(getJoint(command.getParam()), speed);
+            }
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/FanHandler.java

@@ -0,0 +1,60 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.DODriver.FanDriver;
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver;
+import com.iflytop.gd.hardware.type.MId;
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOMId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+//
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class FanHandler extends CommandHandler {
+    private final FanDriver fanDriver_;
+    private final Map<CmdDevice, OutputIOMId>  supportCmdDeviceIOOutputMap = Map.ofEntries(
+            Map.entry(CmdDevice.fan1, OutputIOMId.DO_FAN1),
+            Map.entry(CmdDevice.fan2, OutputIOMId.DO_FAN2),
+            Map.entry(CmdDevice.fan3, OutputIOMId.DO_FAN3),
+            Map.entry(CmdDevice.fan4, OutputIOMId.DO_FAN4),
+            Map.entry(CmdDevice.fan5, OutputIOMId.DO_FAN5),
+            Map.entry(CmdDevice.fan6, OutputIOMId.DO_FAN6)
+    );
+
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = supportCmdDeviceIOOutputMap.entrySet().stream()
+            .collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.open, CmdAction.close);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.open) {
+            fanDriver_.open(supportCmdDeviceIOOutputMap.get(command.getDevice()));
+        } else if (command.getAction() == CmdAction.close) {
+            fanDriver_.close(supportCmdDeviceIOOutputMap.get(command.getDevice()));
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/FillLightHandler.java

@@ -0,0 +1,63 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdColor;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.FillLightDriver;
+import com.iflytop.gd.hardware.drivers.TricolorLightDriver;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+import java.util.Map;
+import java.util.Set;
+
+/**
+ * 补光灯处理
+ */
+@Slf4j
+@Component
+@RequiredArgsConstructor
+class FillLightHandler extends CommandHandler {
+    private final FillLightDriver fillLightDriver_;
+
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = Map.ofEntries(
+            Map.entry(CmdDevice.fill_light, MId.PWMLight)
+    );
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.open, CmdAction.close);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    MId getMId(CmdDevice cmdDevice){
+        return supportCmdDeviceMIdMap.get(cmdDevice);
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.open) {
+            Double d_brightness = command.getParam().getBrightness();
+            int i_brightnessInt = d_brightness.intValue();
+
+            if (d_brightness == null) {
+                fillLightDriver_.open(getMId(command.getDevice()), i_brightnessInt);
+            }
+        } else if (command.getAction() == CmdAction.close) {
+            fillLightDriver_.close(getMId(command.getDevice()));
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java

@@ -0,0 +1,71 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.HBotDriver;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class HBotHandler extends CommandHandler {
+    private final HBotDriver hBotDriver_;
+
+    private final Map<CmdDevice, StepMotorMId> stepMotorMIdMap_ = Map.ofEntries(
+            Map.entry(CmdDevice.hbot_x, StepMotorMId.HBOT_X_MOTOR_MID),
+            Map.entry(CmdDevice.hbot_y, StepMotorMId.HBOT_Y_MOTOR_MID),
+            Map.entry(CmdDevice.hbot_z, StepMotorMId.HBOT_Z_MOTOR_MID)
+    );
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+        if (command.getAction() == CmdAction.origin) {
+            hBotDriver_.moveToHome(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.stop) {
+            hBotDriver_.stop(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.move) {
+            double position = command.getParam().getPosition();
+            double speed = command.getParam().getSpeed();
+            hBotDriver_.moveTo(stepMotorMId, position, speed);
+        }
+        else if(command.getAction() == CmdAction.set) {
+            Integer current = command.getParam().getCurrent();
+            if(current != null) {
+                hBotDriver_.setCurrent(stepMotorMId, current);
+            }
+            Double speed = command.getParam().getSpeed();
+            if(speed != null) {
+                hBotDriver_.setSpeed(stepMotorMId, speed);
+            }
+        }
+    }
+}
+

src/main/java/com/iflytop/gd/hardware/command/handlers/HeatRodHandler.java

@@ -0,0 +1,80 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdColor;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.TricolorLightDriver;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+import java.util.Map;
+import java.util.Set;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class HeatRodHandler extends CommandHandler {
+    private final TricolorLightDriver tricolorLightDriver;
+
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = Map.ofEntries(
+            Map.entry(CmdDevice.tricolor_light, MId.TriColorLight)
+    );
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.open, CmdAction.close);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    protected void checkParams(DeviceCommand command) throws Exception {
+        // 检查参数
+        if (command.getAction() == CmdAction.open) {
+            // 检查参数
+            CmdColor cmdColor = command.getParam().getColor();
+            getCmdColor(cmdColor);
+        }
+
+    }
+
+    TricolorLightDriver.Color getCmdColor(CmdColor cmdColor){
+        switch (cmdColor){
+            case red:
+                return TricolorLightDriver.Color.RED;
+            case green:
+                return TricolorLightDriver.Color.GREEN;
+            case blue:
+                return TricolorLightDriver.Color.BLUE;
+            default:
+                throw new InvalidParameterException("颜色参数错误");
+        }
+    }
+
+    MId getMId(CmdDevice cmdDevice){
+        return supportCmdDeviceMIdMap.get(cmdDevice);
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.open) {
+            CmdColor cmdColor = command.getParam().getColor();
+            TricolorLightDriver.Color color = getCmdColor(cmdColor);
+            tricolorLightDriver.open(getMId(command.getDevice()), color);
+        } else if (command.getAction() == CmdAction.close) {
+            tricolorLightDriver.close(getMId(command.getDevice()));
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/HeaterMotorHandler.java

@@ -0,0 +1,73 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.HeaterMotorDriver;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class HeaterMotorHandler extends CommandHandler {
+    private final HeaterMotorDriver heaterMotorCtrlDriver_;
+
+    private final Map<CmdDevice, StepMotorMId> stepMotorMIdMap_ = Map.ofEntries(
+            Map.entry(CmdDevice.heater_motor_1, StepMotorMId.HEATER_1_MOTOR_MID),
+            Map.entry(CmdDevice.heater_motor_2, StepMotorMId.HEATER_2_MOTOR_MID),
+            Map.entry(CmdDevice.heater_motor_3, StepMotorMId.HEATER_3_MOTOR_MID),
+            Map.entry(CmdDevice.heater_motor_4, StepMotorMId.HEATER_4_MOTOR_MID),
+            Map.entry(CmdDevice.heater_motor_5, StepMotorMId.HEATER_5_MOTOR_MID),
+            Map.entry(CmdDevice.heater_motor_6, StepMotorMId.HEATER_6_MOTOR_MID)
+    );
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+        if (command.getAction() == CmdAction.origin) {
+            heaterMotorCtrlDriver_.moveToHome(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.stop) {
+            heaterMotorCtrlDriver_.stop(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.move) {
+            double position = command.getParam().getPosition();
+            double speed = command.getParam().getSpeed();
+            heaterMotorCtrlDriver_.moveTo(stepMotorMId, position, speed);
+        }
+        else if(command.getAction() == CmdAction.set) {
+            Integer current = command.getParam().getCurrent();
+            if(current != null) {
+                heaterMotorCtrlDriver_.setCurrent(stepMotorMId, current);
+            }
+            Double speed = command.getParam().getSpeed();
+            if(speed != null) {
+                heaterMotorCtrlDriver_.setSpeed(stepMotorMId, speed);
+            }
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java

@@ -0,0 +1,67 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.ShakeMotorDriver;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class ShakeMotorHandler extends CommandHandler {
+    private final ShakeMotorDriver shakeMotorDriver_;
+
+    private final Map<CmdDevice, StepMotorMId> stepMotorMIdMap_ = Map.ofEntries(
+            Map.entry(CmdDevice.shake_motor, StepMotorMId.SHAKE_MOTOR_MID)
+    );
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.start, CmdAction.stop);
+
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+        if (command.getAction() == CmdAction.start) {
+            shakeMotorDriver_.start(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.stop) {
+            shakeMotorDriver_.stop(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.set) {
+            Integer current = command.getParam().getCurrent();
+            if(current != null) {
+                shakeMotorDriver_.setCurrent(stepMotorMId, current);
+            }
+            Double speed = command.getParam().getSpeed();
+            if(speed != null) {
+                shakeMotorDriver_.setSpeed(stepMotorMId, speed);
+            }
+        }
+    }
+}
+

src/main/java/com/iflytop/gd/hardware/command/handlers/TrayMotorHandler.java

@@ -0,0 +1,69 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.TrayMotorDriver;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class TrayMotorHandler extends CommandHandler {
+    private final TrayMotorDriver trayMotorDriver_;
+
+    private final Map<CmdDevice, StepMotorMId> stepMotorMIdMap_ = Map.ofEntries(
+            Map.entry(CmdDevice.tray_motor, StepMotorMId.SHAKE_MOTOR_MID)
+    );
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream().
+            collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.stop);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+        if (command.getAction() == CmdAction.origin) {
+            trayMotorDriver_.moveToHome(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.stop) {
+            trayMotorDriver_.stop(stepMotorMId);
+        }
+        else if(command.getAction() == CmdAction.move) {
+            double position = command.getParam().getPosition();
+            double speed = command.getParam().getSpeed();
+            trayMotorDriver_.moveTo(stepMotorMId, position, speed);
+        }
+        else if(command.getAction() == CmdAction.set) {
+            Integer current = command.getParam().getCurrent();
+            if(current != null) {
+                trayMotorDriver_.setCurrent(stepMotorMId, current);
+            }
+            Double speed = command.getParam().getSpeed();
+            if(speed != null) {
+                trayMotorDriver_.setSpeed(stepMotorMId, speed);
+            }
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/TricolorLightHandler.java

@@ -0,0 +1,83 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdColor;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.TricolorLightDriver;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+import java.util.Map;
+import java.util.Set;
+
+/**
+ * 三色灯
+ */
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class TricolorLightHandler extends CommandHandler {
+    private final TricolorLightDriver tricolorLightDriver;
+
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = Map.ofEntries(
+            Map.entry(CmdDevice.tricolor_light, MId.TriColorLight)
+    );
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.open, CmdAction.close);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    protected void checkParams(DeviceCommand command) throws Exception {
+        // 检查参数
+        if (command.getAction() == CmdAction.open) {
+            // 检查参数
+            CmdColor cmdColor = command.getParam().getColor();
+            getCmdColor(cmdColor);
+        }
+
+    }
+
+    TricolorLightDriver.Color getCmdColor(CmdColor cmdColor){
+        switch (cmdColor){
+            case red:
+                return TricolorLightDriver.Color.RED;
+            case green:
+                return TricolorLightDriver.Color.GREEN;
+            case blue:
+                return TricolorLightDriver.Color.BLUE;
+            default:
+                throw new InvalidParameterException("颜色参数错误");
+        }
+    }
+
+    MId getMId(CmdDevice cmdDevice){
+        return supportCmdDeviceMIdMap.get(cmdDevice);
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.open) {
+            CmdColor cmdColor = command.getParam().getColor();
+            TricolorLightDriver.Color color = getCmdColor(cmdColor);
+            tricolorLightDriver.open(getMId(command.getDevice()), color);
+        } else if (command.getAction() == CmdAction.close) {
+            tricolorLightDriver.close(getMId(command.getDevice()));
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/VentilatorHandler.java

@@ -0,0 +1,53 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.DODriver.FanDriver;
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOMId;
+import com.iflytop.gd.hardware.drivers.DODriver.VentilatorDriver;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class VentilatorHandler extends CommandHandler {
+    private final VentilatorDriver ventilatorDriver_;
+    private final Map<CmdDevice, OutputIOMId> supportCmdDeviceIOOutputMap = Map.ofEntries(
+            Map.entry(CmdDevice.ventilator_power, OutputIOMId.DO_VENTILATOR)
+    );
+
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = supportCmdDeviceIOOutputMap.entrySet().stream()
+            .collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.open_power, CmdAction.close_power);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.open) {
+            ventilatorDriver_.openPower(supportCmdDeviceIOOutputMap.get(command.getDevice()));
+        } else if (command.getAction() == CmdAction.close) {
+            ventilatorDriver_.closePower(supportCmdDeviceIOOutputMap.get(command.getDevice()));
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/command/handlers/WaterPumpHandler.java

@@ -0,0 +1,53 @@
+package com.iflytop.gd.hardware.command.handlers;
+
+import com.iflytop.gd.common.cmd.DeviceCommand;
+import com.iflytop.gd.common.enums.cmd.CmdAction;
+import com.iflytop.gd.common.enums.cmd.CmdDevice;
+import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOMId;
+import com.iflytop.gd.hardware.drivers.DODriver.VentilatorDriver;
+import com.iflytop.gd.hardware.drivers.DODriver.WaterPumpDriver;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.util.Map;
+import java.util.Set;
+import java.util.stream.Collectors;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class WaterPumpHandler extends CommandHandler {
+    private final WaterPumpDriver waterPumpDriver_;
+    private final Map<CmdDevice, OutputIOMId> supportCmdDeviceIOOutputMap = Map.ofEntries(
+            Map.entry(CmdDevice.ventilator_power, OutputIOMId.DO_VENTILATOR)
+    );
+
+    private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = supportCmdDeviceIOOutputMap.entrySet().stream()
+            .collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
+
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.open_power, CmdAction.close_power);
+
+    @Override
+    protected Map<CmdDevice, MId> getSupportCmdDeviceMIdMap()
+    {
+        return supportCmdDeviceMIdMap;
+    }
+
+    @Override
+    protected Set<CmdAction> getSupportActions() {
+        return supportActions;
+    }
+
+    @Override
+    public void handleCommand(DeviceCommand command) throws Exception {
+        // 发送命令
+        if (command.getAction() == CmdAction.open) {
+            waterPumpDriver_.openPower(supportCmdDeviceIOOutputMap.get(command.getDevice()));
+        } else if (command.getAction() == CmdAction.close) {
+            waterPumpDriver_.closePower(supportCmdDeviceIOOutputMap.get(command.getDevice()));
+        }
+    }
+}

src/main/java/com/iflytop/gd/hardware/constants/ActionOvertimeConstant.java

@@ -0,0 +1,71 @@
+package com.iflytop.gd.hardware.constants;
+
+
+import com.iflytop.gd.hardware.type.CmdId;
+import com.iflytop.gd.hardware.type.MId;
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import jakarta.annotation.PostConstruct;
+import org.springframework.stereotype.Component;
+
+import java.util.ArrayList;
+import java.util.List;
+
+@Component
+public class ActionOvertimeConstant {
+	final Integer defaultOvertime = 10 * 1000;
+
+	static class OvertimeConfigItem {
+		public MId mid;
+		public CmdId cmdId;
+		public Integer overtime;
+	}
+
+	List<OvertimeConfigItem> overtimeConfigItems = new ArrayList<>();
+
+	@PostConstruct
+	void init() {
+//		pushNewConfig(StepMotorMId.IncubatorRotateCtrlM, CmdId.step_motor_easy_move_to_zero, 20 * 1000);
+//		pushNewConfig(StepMotorMId.IncubatorRotateCtrlM, CmdId.step_motor_easy_move_to_end_point, 20 * 1000);
+//		pushNewConfig(StepMotorMId.PlatesBoxYM, CmdId.step_motor_easy_move_to_zero, 20 * 1000);
+//		pushNewConfig(StepMotorMId.PlatesBoxYM, CmdId.step_motor_easy_move_to_end_point, 20 * 1000);
+//
+//		pushNewConfig(StepMotorMId.OptModPullM, CmdId.step_motor_easy_move_to_end_point, 15 * 1000);
+//		pushNewConfig(StepMotorMId.OptModPullM, CmdId.step_motor_easy_move_to_zero, 15 * 1000);
+//
+//		pushNewConfig(StepMotorMId.FeedingModXM, CmdId.step_motor_easy_move_to_zero, 15 * 1000);
+//		pushNewConfig(StepMotorMId.FeedingModXM, CmdId.step_motor_easy_move_to_end_point, 15 * 1000);
+
+
+	}
+
+	public Integer get(MId mid, CmdId cmdId) {
+		return priGetOvertime(mid, cmdId);
+	}
+
+	public Integer get(StepMotorMId mid, CmdId cmdId) {
+		return priGetOvertime(mid.mid, cmdId);
+	}
+
+
+	private void pushNewConfig(StepMotorMId mid, CmdId cmdId, Integer overtime) {
+		pushNewConfig(mid.mid, cmdId, overtime);
+	}
+
+	private void pushNewConfig(MId mid, CmdId cmdId, Integer overtime) {
+		OvertimeConfigItem item = new OvertimeConfigItem();
+		item.mid      = mid;
+		item.cmdId    = cmdId;
+		item.overtime = overtime;
+		overtimeConfigItems.add(item);
+	}
+
+	private Integer priGetOvertime(MId mid, CmdId cmdId) {
+		for (OvertimeConfigItem item : overtimeConfigItems) {
+			if (item.mid == mid && item.cmdId == cmdId) {
+				return item.overtime;
+			}
+		}
+		return defaultOvertime;
+	}
+
+}

src/main/java/com/iflytop/gd/hardware/constants/Dim.java

@@ -1,5 +0,0 @@
-package com.iflytop.gd.hardware.constants;
-
-public enum Dim {
-    X, Y, Z
-}

src/main/java/com/iflytop/gd/hardware/constants/DistanceUnit.java

@@ -1,30 +0,0 @@
-package com.iflytop.gd.hardware.constants;
-
-public enum DistanceUnit {
-    MM, CM;
-
-    public Integer toMM(Integer value) {
-        if (this == MM) {
-            return value;
-        }
-
-        if (this == CM) {
-            return value * 10;
-        }
-
-        return value;
-    }
-
-
-    public static DistanceUnit toDistanceUnit(String unitString) {
-        if ("mm".equals(unitString)) {
-            return MM;
-        }
-
-        if ("cm".equals(unitString)) {
-            return CM;
-        }
-
-        throw new IllegalArgumentException("Unknown distance unit " + unitString);
-    }
-}

src/main/java/com/iflytop/gd/hardware/constants/LiquidFillArmMotorIndex.java

@@ -1,5 +0,0 @@
-package com.iflytop.gd.hardware.constants;
-
-public enum LiquidFillArmMotorIndex {
-    LargeArm, SmallArm
-}

src/main/java/com/iflytop/gd/hardware/constants/MiniServoConstant.java

@@ -0,0 +1,52 @@
+package com.iflytop.gd.hardware.constants;
+
+import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoMId;
+import org.springframework.util.Assert;
+
+import java.util.Map;
+
+public class MiniServoConstant {
+	static public final Integer actionOvertime = 10000;
+	static class MiniServoFixPosConfig {
+		public Integer zeroPos;
+		public Integer refPos;
+	}
+
+	static Map<MiniServoMId, MiniServoFixPosConfig> map = Map.of(
+//			//摇匀模组Y轴
+//			MiniServoMId.ShakeModGripperYSV, new MiniServoFixPosConfig() {{
+//				zeroPos = 110;
+//				refPos  = 100;//Y轴最外侧的位置
+//			}},
+//			//摇匀模组夹爪
+//			MiniServoMId.ShakeModGripperSV, new MiniServoFixPosConfig() {{
+//				zeroPos = 1600;
+//				refPos  = 1800;//完全张开的位置
+//			}},
+//			//试管架夹紧
+//			MiniServoMId.ShakeModTubeScanerClampingSV, new MiniServoFixPosConfig() {{
+//				zeroPos = 1800;
+//				refPos  = 1800;//水平位置
+//			}},
+//			//试管顶升舵机
+//			MiniServoMId.ShakeModLiftingSV, new MiniServoFixPosConfig() {{
+//				zeroPos = 644;//底部 644/3000 = 220/1024
+//				refPos  = 644;//底部
+//			}}
+
+	);
+
+	static public Integer getZeroPos(MiniServoMId mid) {
+		var config = map.get(mid);
+		Assert.notNull(config, "getZeroPos fail");
+		return config.zeroPos;
+	}
+
+	static public Integer getRefPos(MiniServoMId mid) {
+		var config = map.get(mid);
+		Assert.notNull(config, "getRefPos fail");
+		return config.refPos;
+	}
+
+
+}

src/main/java/com/iflytop/gd/hardware/constants/RotationDirection.java

@@ -1,18 +0,0 @@
-package com.iflytop.gd.hardware.constants;
-
-import lombok.Getter;
-
-/**
- * 电机转动方向
- */
-@Getter
-public enum RotationDirection {
-    Forward(1),Backward(-1);
-
-    private Integer value;
-
-    RotationDirection(Integer value) {
-        this.value = value;
-    }
-
-}

src/main/java/com/iflytop/gd/hardware/constants/VelocityUnit.java

@@ -1,28 +0,0 @@
-package com.iflytop.gd.hardware.constants;
-
-/**
- * 移动速度单位
- */
-public enum VelocityUnit {
-    MM_PER_SEC, CM_PER_SEC;
-
-    public Integer toMM_PER_SEC(Integer value) {
-        if (this == VelocityUnit.CM_PER_SEC) {
-            return value * 10;
-        }
-        return value;
-    }
-
-
-    public static VelocityUnit toVelocityUnit(String unitString) {
-        if ("mm/s".equals(unitString)) {
-            return VelocityUnit.MM_PER_SEC;
-        }
-
-        if ("cm/s".equals(unitString)) {
-            return VelocityUnit.CM_PER_SEC;
-        }
-
-        throw new IllegalArgumentException("Unrecognized SpeedUnit: " + unitString);
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/CameraDriver.java

@@ -0,0 +1,13 @@
+package com.iflytop.gd.hardware.drivers;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class CameraDriver {
+    public void takePhoto(){
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/ColdTrapDriver.java

@@ -0,0 +1,54 @@
+package com.iflytop.gd.hardware.drivers;
+
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver;
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOMId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import org.springframework.stereotype.Component;
+
+import static com.iflytop.gd.hardware.drivers.DODriver.OutputIOMId.DO_COLD_TRAP_POWER;
+
+/**
+ * 物理冷阱
+ */
+@Component
+@RequiredArgsConstructor
+public class ColdTrapDriver {
+    private final OutputIOCtrlDriver outputIOCtrlDriver_;
+
+    public void setTemperature(Double temperature) throws Exception {
+
+    }
+
+    public void openPower()  throws Exception{
+        outputIOCtrlDriver_.setIOState(DO_COLD_TRAP_POWER, true);
+    }
+
+    public void closePower() throws Exception{
+        outputIOCtrlDriver_.setIOState(DO_COLD_TRAP_POWER, false);
+    }
+
+    public boolean openCircle() {
+        return false;
+    }
+
+    public boolean closeCircle() {
+        return false;
+    }
+
+    public boolean openHeart() {
+        return false;
+    }
+
+    public boolean closeHeart() {
+        return false;
+    }
+
+    public boolean openCool() {
+        return false;
+    }
+
+    public boolean closeCool() {
+        return false;
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/ColdTray.java

@@ -1,34 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-/**
- * 物理冷阱
- */
-public class ColdTray {
-    public boolean setTemperature(Double temperature) {
-        return false;
-    }
-
-    public boolean startRecycle() {
-        return false;
-    }
-
-    public boolean stopRecycle() {
-        return false;
-    }
-
-    public boolean startHeating() {
-        return false;
-    }
-
-    public boolean stopHeating() {
-        return false;
-    }
-
-    public boolean startRefrigeration() {
-        return false;
-    }
-
-    public boolean stopRefrigeration() {
-        return false;
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/DIDriver/InputDetectDriver.java

@@ -0,0 +1,44 @@
+package com.iflytop.gd.hardware.drivers.DIDriver;
+
+import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
+import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.ModuleType;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class InputDetectDriver {
+
+	private final A8kCanBusService a8kCanBusService;
+
+	public Boolean getIOState(InputIOId ioid) throws HardwareException {
+		while (true) {
+			Boolean firstReadIO  = priGetIOState(ioid);
+			Boolean secondReadIO = priGetIOState(ioid);
+			if (firstReadIO == secondReadIO) {
+				if (ioid.mirror) {
+					return !firstReadIO;
+				}
+				return firstReadIO;
+			} else {
+				log.warn("getIOState {} not match, retry", ioid);
+			}
+		}
+	}
+
+
+	private Boolean priGetIOState(InputIOId ioid) throws HardwareException {
+		if (ioid.mtype == ModuleType.Board) {
+			return a8kCanBusService.callcmd(ioid.mid, CmdId.extboard_read_inio, ioid.ioIndex).getContentI32(0) != 0;
+		} else if (ioid.mtype == ModuleType.TMCStepMotor) {
+			return a8kCanBusService.callcmd(ioid.mid, CmdId.step_motor_read_io_state, ioid.ioIndex).getContentI32(0) != 0;
+		} else if (ioid.mtype == ModuleType.MiniServo) {
+			return a8kCanBusService.callcmd(ioid.mid, CmdId.mini_servo_read_io_state, ioid.ioIndex).getContentI32(0) != 0;
+		} else {
+			throw new HardwareException(new AECodeError(String.format("IOID MODULE TYPE %s NOT SUPPORT", ioid.mtype)));
+		}
+	}
+}

src/main/java/com/iflytop/gd/hardware/drivers/DODriver/FanDriver.java

@@ -0,0 +1,21 @@
+package com.iflytop.gd.hardware.drivers.DODriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class FanDriver {
+    private final OutputIOCtrlDriver outputIOCtrlDriver_;
+
+    public void open(OutputIOMId outputIOMId) throws Exception
+    {
+        outputIOCtrlDriver_.setIOState(outputIOMId, true);
+    }
+    public void close(OutputIOMId outputIOMId) throws Exception
+    {
+        outputIOCtrlDriver_.setIOState(outputIOMId, false);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/DODriver/OutputIOCtrlDriver.java

@@ -0,0 +1,19 @@
+package com.iflytop.gd.hardware.drivers.DODriver;
+
+
+import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
+import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.CmdId;
+import lombok.RequiredArgsConstructor;
+import org.springframework.stereotype.Component;
+
+@Component
+@RequiredArgsConstructor
+public class OutputIOCtrlDriver {
+
+	private final A8kCanBusService canBus;
+
+	public void setIOState(OutputIOMId IOOutput, Boolean state) throws HardwareException {
+		canBus.callcmd(IOOutput.mid, CmdId.extboard_write_outio, IOOutput.ioIndex, state ? 1 : 0);
+	}
+}

src/main/java/com/iflytop/gd/hardware/drivers/DODriver/OutputIOMId.java

@@ -0,0 +1,30 @@
+package com.iflytop.gd.hardware.drivers.DODriver;
+
+import com.iflytop.gd.hardware.type.MId;
+
+public enum OutputIOMId {
+	DO_FAN1("FAN1", MId.IO1Board, 0,false),
+	DO_FAN2("FAN2", MId.IO1Board, 1,false),
+	DO_FAN3("FAN3", MId.IO1Board, 2,false),
+	DO_FAN4("FAN4", MId.IO1Board, 3,false),
+	DO_FAN5("FAN5", MId.IO1Board, 4,false),
+	DO_FAN6("FAN6", MId.IO1Board, 5,false),
+	DO_VENTILATOR("VENTILATOR", MId.IO1Board, 6,false),
+	DO_WATER_PUMP("WATER_PUMP", MId.IO1Board, 7,false),
+	DO_COLD_TRAP_POWER("COLD_TRAP_POWER", MId.IO1Board, 8,false),
+	;
+
+	final public String description;
+	final public MId        mid;
+	final public int        ioIndex;
+	final public boolean    mirror;
+
+	OutputIOMId(String description, MId mid, int ioIndex, boolean mirror) {
+		this.description = description;
+		this.mid     = mid;
+		this.ioIndex = ioIndex;
+		this.mirror  = mirror;
+
+	}
+
+}

src/main/java/com/iflytop/gd/hardware/drivers/DODriver/VentilatorDriver.java

@@ -0,0 +1,21 @@
+package com.iflytop.gd.hardware.drivers.DODriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class VentilatorDriver {
+    private final OutputIOCtrlDriver outputIOCtrlDriver_;
+
+    public void openPower(OutputIOMId outputIOMId) throws Exception
+    {
+        outputIOCtrlDriver_.setIOState(outputIOMId, true);
+    }
+    public void closePower(OutputIOMId outputIOMId) throws Exception
+    {
+        outputIOCtrlDriver_.setIOState(outputIOMId, false);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/DODriver/WaterPumpDriver.java

@@ -0,0 +1,21 @@
+package com.iflytop.gd.hardware.drivers.DODriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class WaterPumpDriver {
+    private final OutputIOCtrlDriver outputIOCtrlDriver_;
+
+    public void openPower(OutputIOMId outputIOMId) throws Exception
+    {
+        outputIOCtrlDriver_.setIOState(outputIOMId, true);
+    }
+    public void closePower(OutputIOMId outputIOMId) throws Exception
+    {
+        outputIOCtrlDriver_.setIOState(outputIOMId, false);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/Door.java

@@ -1,17 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-/**
- * 物理门
- */
-
-public class Door {
-    public void open() {
-    }
-
-    public void close() {
-    }
-
-    public void stop() {
-    }
-
-}

src/main/java/com/iflytop/gd/hardware/drivers/Fan.java

@@ -1,13 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-/**
- * 物理风扇
- */
-public class Fan {
-    public boolean open() {
-        return false;
-    }
-    public boolean close() {
-        return false;
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/FillLightDriver.java

@@ -0,0 +1,23 @@
+package com.iflytop.gd.hardware.drivers;
+
+import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
+import com.iflytop.gd.hardware.type.CmdId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class FillLightDriver {
+    private final A8kCanBusService canBus;
+
+    public void open(MId mId, Integer brightness) throws Exception {
+        // 模拟打开
+        canBus.callcmd(mId, CmdId.module_pwm_light_on, brightness);
+    }
+    public void close(MId mId) throws Exception {
+        canBus.callcmd(mId, CmdId.module_pwm_light_off);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/HeaterRodDriver.java

@@ -0,0 +1,24 @@
+package com.iflytop.gd.hardware.drivers;
+
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class HeaterRodDriver {
+    public double getTemperature(MId mId) {
+        // 模拟获取温度
+        return 25.0;
+    }
+
+    public void open(MId mId, double temperature) throws Exception {
+        // 模拟加热
+    }
+
+    public void close(MId mId) throws Exception {
+        // 模拟关闭
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/HoldingJaw.java

@@ -1,22 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-/**
- * 物理夹爪
- */
-public class HoldingJaw{
-    public void open() {
-
-    }
-    public void close() {
-
-    }
-    public void pause() {
-
-    }
-    public void resume() {
-
-    }
-    public void setSpeed(int speed) {
-
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/LiquidFillingArm.java

@@ -1,24 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-
-import com.iflytop.gd.hardware.constants.LiquidFillArmMotorIndex;
-import com.iflytop.gd.hardware.constants.RotationDirection;
-import com.iflytop.gd.hardware.constants.VelocityUnit;
-import com.iflytop.gd.app.model.bo.Point3D;
-
-/**
- * 物理液体加注机械臂
- */
-public class LiquidFillingArm {
-    public void moveTo(Point3D point) {
-
-    }
-
-    public void rotateTo(LiquidFillArmMotorIndex liquidFillArmMotorIndex, Integer angle, RotationDirection direction) {
-
-    }
-
-    public void setRotationSpeed(Integer speed, VelocityUnit speedUnit) {
-
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/ClawDriver.java

@@ -0,0 +1,66 @@
+package com.iflytop.gd.hardware.drivers.MiniServoDriver;
+
+import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorMId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class ClawDriver {
+    private final MiniServoDriver miniServoDriver;
+
+    private double speed_ = 10.0;         // mm/s
+
+    // ==== ==== ==== ==== ==== ==== convert ==== ==== ==== ==== ==== ====
+    private int getServoPosition(double position) {
+        int servoPosition = 0;
+        servoPosition = (int)position;
+        return servoPosition;
+    }
+
+    private int getServoSpeed(double speed) {
+        int servoSpeed = 0;
+        servoSpeed = (int)speed;
+        return servoSpeed;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
+    private double getSpeed(MiniServoMId sevoMid)
+    {
+        // TODO: 从数据库中获取门的速度
+        return speed_;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
+    public void setSpeed(MiniServoMId sevoMid, double speed) {
+        // 检查速度是否合法
+        speed_ = speed;
+        // TODO: 写入数据库
+    }
+
+    // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
+    public void moveToHome(MiniServoMId sevoMid) throws Exception
+    {
+        double speed = getSpeed(sevoMid);
+        int motorSpeed = getServoSpeed(speed);
+        miniServoDriver.miniServoMoveToZero(sevoMid);
+    }
+
+    public void moveTo(MiniServoMId sevoMid, double position, double speed) throws Exception
+    {
+        // 检查位置是否合法
+        if (position < 0) {
+            throw new InvalidParameterException("Position is out of range");
+        }
+        this.setSpeed(sevoMid, speed);
+        miniServoDriver.miniServoMoveToBlock(sevoMid, getServoPosition(position));
+    }
+
+    public void stop(MiniServoMId sevoMid) throws Exception {
+        miniServoDriver.miniServoStop(sevoMid);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DualRobotDriver.java

@@ -0,0 +1,122 @@
+package com.iflytop.gd.hardware.drivers.MiniServoDriver;
+
+import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.awt.geom.Point2D;
+import java.security.InvalidParameterException;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class DualRobotDriver {
+    public enum Joint {
+        JOINT_1,
+        JOINT_2
+    }
+
+    private final MiniServoDriver miniServoDriver;
+
+    private double speed_ = 10.0;         // mm/s
+
+
+
+    // ==== ==== ==== ==== ==== ==== convert ==== ==== ==== ==== ==== ====
+    private int getServoPosition(double position) {
+        int servoPosition = 0;
+        servoPosition = (int)position;
+        return servoPosition;
+    }
+
+    private int getServoSpeed(double speed) {
+        int servoSpeed = 0;
+        servoSpeed = (int)speed;
+        return servoSpeed;
+    }
+
+    private Point2D getServoPosition(Point2D point) {
+        Point2D servoPosition = new Point2D.Double(0, 0);
+        servoPosition.setLocation(point.getX(), point.getY());
+        return servoPosition;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
+    private double getSpeed(MiniServoMId sevoMid)
+    {
+        // TODO: 从数据库中获取门的速度
+        return speed_;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
+    public void setSpeed(MiniServoMId sevoMid, double speed) {
+        // 检查速度是否合法
+        speed_ = speed;
+        // TODO: 写入数据库
+    }
+
+    public MiniServoMId getMiniServoMId(Joint joint) {
+        switch (joint) {
+            case JOINT_1:
+                return MiniServoMId.DUAL_ROBOT_AXIS_1_MID;
+            case JOINT_2:
+                return MiniServoMId.DUAL_ROBOT_AXIS_2_MID;
+            default:
+                return null;
+        }
+    }
+
+    // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
+    public void moveToHome(MiniServoMId sevoMid) throws HardwareException
+    {
+        double speed = getSpeed(sevoMid);
+        int motorSpeed = getServoSpeed(speed);
+        miniServoDriver.miniServoMoveToZero(sevoMid);
+    }
+
+    public void moveTo(MiniServoMId sevoMid, double position, double speed) throws HardwareException
+    {
+        // 检查位置是否合法
+        if (position < 0) {
+            throw new InvalidParameterException("Position is out of range");
+        }
+        this.setSpeed(sevoMid, speed);
+        miniServoDriver.miniServoMoveToBlock(sevoMid, getServoPosition(position));
+    }
+
+    public void stop(MiniServoMId sevoMid) throws HardwareException {
+        miniServoDriver.miniServoStop(sevoMid);
+    }
+
+    // ==== ==== ==== ==== ==== ==== Application ==== ==== ==== ==== ==== ====
+
+    public void setSpeed(Joint joint, double speed) throws HardwareException {
+        MiniServoMId servoMId = getMiniServoMId(joint);
+        setSpeed(servoMId, speed);
+    }
+
+    public void moveToPoint(double pointX, double pointY) throws HardwareException {
+        Point2D point = getServoPosition(new Point2D.Double(pointX, pointY));
+
+        miniServoDriver.miniServoMoveToBlock(MiniServoMId.DUAL_ROBOT_AXIS_1_MID, (int)point.getX());
+        miniServoDriver.miniServoMoveToBlock(MiniServoMId.DUAL_ROBOT_AXIS_2_MID, (int)point.getY());
+    }
+
+    public void moveToHome(Joint joint) throws HardwareException {
+        MiniServoMId servoMId = getMiniServoMId(joint);
+        moveToHome(servoMId);
+    }
+
+    public void moveTo(Joint joint, double position, double speed) throws HardwareException {
+        MiniServoMId servoMId = getMiniServoMId(joint);
+        moveTo(servoMId, position, speed);
+    }
+
+    public void stop(Joint joint) throws HardwareException {
+        MiniServoMId servoMId = getMiniServoMId(joint);
+        stop(servoMId);
+    }
+
+}

src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/MiniServoDriver.java

@@ -0,0 +1,161 @@
+package com.iflytop.gd.hardware.drivers.MiniServoDriver;
+
+import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
+import com.iflytop.gd.hardware.constants.MiniServoConstant;
+import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.A8kEcode;
+import com.iflytop.gd.hardware.type.AEHardwareError;
+import com.iflytop.gd.hardware.type.CmdId;
+import com.iflytop.gd.hardware.type.RegIndex;
+import com.iflytop.gd.hardware.utils.OS;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+
+@Component
+@Slf4j
+@RequiredArgsConstructor
+public class MiniServoDriver {
+	final private A8kCanBusService canBus;
+
+	public void moduleStop(MiniServoMId id) throws HardwareException {
+		canBus.moduleStop(id.mid);
+	}
+
+	public void miniServoEnable(MiniServoMId id, int enable) throws HardwareException {
+		log.debug("{} miniServoEnable {}", id.mid, enable);
+		canBus.callcmd(id.mid, CmdId.mini_servo_enable, enable);
+	}
+
+	public void miniServoForceEnable(MiniServoMId id, int enable) {
+		try {
+			miniServoEnable(id, enable);
+		} catch (HardwareException e) {
+			log.error("miniServoForceEnable error", e);
+		}
+	}
+
+	public Boolean miniServoPing(MiniServoMId id) {
+		try {
+			miniServoReadPos(id);
+		} catch (HardwareException e) {
+			return false;
+		}
+		return true;
+	}
+
+	public int miniServoReadPos(MiniServoMId id) throws HardwareException {
+		var packet = canBus.callcmd(id.mid, CmdId.mini_servo_read_pos);
+		return packet.getContentI32(0);
+	}
+
+	public void miniServoMoveToZeroBlock(MiniServoMId id) throws HardwareException {
+		log.debug("{} miniServoMoveToZeroBlock", id.mid);
+		miniServoMoveToBlock(id, MiniServoConstant.getZeroPos(id));
+	}
+
+	public void miniServoMoveToZero(MiniServoMId id) throws HardwareException {
+		log.debug("{} miniServoMoveToZero", id.mid);
+		miniServoMoveTo(id, MiniServoConstant.getZeroPos(id));
+	}
+
+
+	public void miniServoActiveCfg(MiniServoMId id) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.mini_servo_active_cfg);
+	}
+
+	public void miniServoStop(MiniServoMId id) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.mini_servo_stop);
+	}
+
+	public void miniServoSetMidPoint(MiniServoMId id) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.mini_servo_set_mid_point);
+	}
+
+	public int miniServoReadIoState(MiniServoMId id) throws HardwareException {
+		var packet = canBus.callcmd(id.mid, CmdId.mini_servo_read_io_state);
+		return packet.getContentI32(0);
+	}
+
+	public void miniServoMoveTo(MiniServoMId id, int pos) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.mini_servo_move_to, pos);
+	}
+
+	public void miniServoMoveToBlock(MiniServoMId id, int pos) throws HardwareException {
+		log.debug("{} miniServoMoveTo {}", id.mid, pos);
+		miniServoMoveTo(id, pos);
+		canBus.waitForMod(id.mid, MiniServoConstant.actionOvertime);
+	}
+
+
+	public void miniServoRotate(MiniServoMId id, int direction) throws HardwareException {
+		log.debug("{} miniServoRotate {}", id.mid, direction);
+		canBus.callcmd(id.mid, CmdId.mini_servo_rotate, direction);
+	}
+
+	public void miniServoRotateBlock(MiniServoMId id, int direction) throws HardwareException {
+		miniServoRotate(id, direction);
+		canBus.waitForMod(id.mid, MiniServoConstant.actionOvertime);
+	}
+
+	public void miniServoRotateWithTorque(MiniServoMId id, int torque) throws HardwareException {
+		log.debug("{} miniServoRotateWithTorque {}", id.mid, torque);
+		canBus.callcmd(id.mid, CmdId.mini_servo_rotate_with_torque, torque);
+	}
+
+	public void waitForMod(MiniServoMId mid) throws HardwareException {
+		canBus.waitForMod(mid.mid, MiniServoConstant.actionOvertime);
+	}
+
+	public void waitForMod(MiniServoMId... mid) throws HardwareException {
+		for (MiniServoMId m : mid) {
+			waitForMod(m);
+		}
+	}
+
+
+	//	kmini_servo_set_cur_pos
+	private void miniServoSetCurPos(MiniServoMId id, Integer pos) throws HardwareException {
+		log.debug("{} miniServoSetCurPos {}", id.mid, pos);
+		canBus.callcmd2(id.mid, CmdId.mini_servo_set_cur_pos, 5000, pos);
+	}
+
+	public Integer miniServoSetRefPos(MiniServoMId id) throws HardwareException {
+		miniServoSetCurPos(id, MiniServoConstant.getRefPos(id));
+		return MiniServoConstant.getRefPos(id);
+	}
+
+	public void setReg(MiniServoMId id, MiniServoRegIndex regindex, int val) throws HardwareException {
+		log.debug("{} setReg {} {}", id.mid, regindex, val);
+		canBus.moduleSetReg(id.mid, regindex.regIndex, val);
+	}
+
+	public Integer getReg(MiniServoMId id, MiniServoRegIndex regindex) throws HardwareException {
+		return canBus.moduleGetReg(id.mid, regindex.regIndex);
+	}
+
+	public Integer getRegNoEx(MiniServoMId id, MiniServoRegIndex regindex) {
+		try {
+			return canBus.moduleGetReg(id.mid, regindex.regIndex);
+		} catch (HardwareException e) {
+			log.error("readRegNoEx error", e);
+			return null;
+		}
+	}
+
+	public void miniServoWaitIsNotMove(MiniServoMId id, int acitionOvertime) throws HardwareException {
+		long startedAt = System.currentTimeMillis();
+		do {
+			var isMove = canBus.moduleGetReg(id.mid, RegIndex.kreg_mini_servo_is_move);
+			if (isMove != 0) {
+				break;
+			}
+			long now = System.currentTimeMillis();
+			if (now - startedAt > acitionOvertime) {
+				throw HardwareException.of(new AEHardwareError(A8kEcode.LOW_ERROR_OVERTIME, id.mid, null));
+			}
+
+		} while (true);
+	}
+}

src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/MiniServoMId.java

@@ -0,0 +1,20 @@
+package com.iflytop.gd.hardware.drivers.MiniServoDriver;
+
+import com.iflytop.gd.hardware.type.MId;
+import org.springframework.util.Assert;
+
+public enum MiniServoMId {
+	NotSet(MId.NotSet),//
+	DUAL_ROBOT_AXIS_1_MID(MId.DualRobotAxis1M),//
+	DUAL_ROBOT_AXIS_2_MID(MId.DualRobotAxis2M),
+	CLAW_MID(MId.HBotClawS)
+	;
+
+	final public MId mid;
+
+	MiniServoMId(MId mid) {
+		Assert.isTrue(this.name().equals(mid.name()),"MiniServoMid Init fail");
+		this.mid = mid;
+	}
+
+}

src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/MiniServoRegIndex.java

@@ -0,0 +1,71 @@
+package com.iflytop.gd.hardware.drivers.MiniServoDriver;
+
+import com.iflytop.gd.hardware.type.RegIndex;
+
+public enum MiniServoRegIndex {
+	//	kreg_module_version(RegIndex.kreg_module_version), // 模块版本
+	//	kreg_module_type(RegIndex.kreg_module_type), // 模块类型
+	//	kreg_module_status(RegIndex.kreg_module_status), // 0idle,1busy,2error
+	//	kreg_module_errorcode(RegIndex.kreg_module_errorcode), // inited_flag
+
+	kreg_mini_servo_pos(RegIndex.kreg_mini_servo_pos), // 位置
+	kreg_mini_servo_limit_velocity(RegIndex.kreg_mini_servo_limit_velocity), // 限制速度
+	kreg_mini_servo_limit_torque(RegIndex.kreg_mini_servo_limit_torque), // 限制扭矩
+	kreg_mini_servo_protective_torque(RegIndex.kreg_mini_servo_protective_torque), // 保护扭矩
+	kreg_mini_servo_is_move(RegIndex.kreg_mini_servo_is_move), // 是否在运动
+
+
+	kreg_mini_servo_status(RegIndex.kreg_mini_servo_status), // 舵机状态
+	kreg_mini_servo_voltage(RegIndex.kreg_mini_servo_voltage), // 电压 反馈当前舵机工作电压,反馈精度为0.1V,即120*0.1=12V
+	kreg_mini_servo_current(RegIndex.kreg_mini_servo_current), // 电流 反馈当前工作电流值,单位为ma
+	kreg_mini_servo_temperature(RegIndex.kreg_mini_servo_temperature), // 温度 反馈当前舵机内部工作温度,反馈精度为1摄氏度
+	kreg_mini_servo_loadvalue(RegIndex.kreg_mini_servo_loadvalue), // 负载值 输出驱动电机的当前占空比电压,单位为0.1%,取值0-1000
+	kreg_mini_servo_target_pos_tolerance(RegIndex.kreg_mini_servo_target_pos_tolerance), // 目标位置容忍度
+
+
+	kreg_mini_servo_firmware_main_version(RegIndex.kreg_mini_servo_firmware_main_version),     // 固件主版本号
+	kreg_mini_servo_firmware_sub_version(RegIndex.kreg_mini_servo_firmware_sub_version),     // 固件次版本号
+	kreg_mini_servo_servo_main_version(RegIndex.kreg_mini_servo_servo_main_version),     // 舵机主版本号
+	kreg_mini_servo_servo_sub_version(RegIndex.kreg_mini_servo_servo_sub_version),     // 舵机次版本号
+	kreg_mini_servo_servo_min_angle(RegIndex.kreg_mini_servo_servo_min_angle),     // 最小角度限制
+	kreg_mini_servo_servo_max_angle(RegIndex.kreg_mini_servo_servo_max_angle),     // 最大角度限制
+	kreg_mini_servo_servo_max_temp(RegIndex.kreg_mini_servo_servo_max_temp),     // 最高温度上限
+	kreg_mini_servo_servo_max_voltage(RegIndex.kreg_mini_servo_servo_max_voltage),     // 最高输入电压
+	kreg_mini_servo_servo_min_voltage(RegIndex.kreg_mini_servo_servo_min_voltage),     // 最低输入电压
+	kreg_mini_servo_servo_max_torque(RegIndex.kreg_mini_servo_servo_max_torque),     // 最大扭矩
+	kreg_mini_servo_servo_unload_condition(RegIndex.kreg_mini_servo_servo_unload_condition),     // 卸载条件
+	kreg_mini_servo_servo_p(RegIndex.kreg_mini_servo_servo_p),     // P 比例系
+	kreg_mini_servo_servo_d(RegIndex.kreg_mini_servo_servo_d),     // D 微分系
+	kreg_mini_servo_servo_i(RegIndex.kreg_mini_servo_servo_i),     // I
+	kreg_mini_servo_servo_min_start(RegIndex.kreg_mini_servo_servo_min_start),     // 最小启动
+	kreg_mini_servo_servo_cw_dead_zone(RegIndex.kreg_mini_servo_servo_cw_dead_zone),    // 顺时针不灵敏区
+	kreg_mini_servo_servo_ccw_dead_zone(RegIndex.kreg_mini_servo_servo_ccw_dead_zone),    // 逆时针不灵敏
+	kreg_mini_servo_servo_protect_current(RegIndex.kreg_mini_servo_servo_protect_current),    // 保护电流
+	kreg_mini_servo_servo_protect_torque(RegIndex.kreg_mini_servo_servo_protect_torque),    // 保护扭矩 0->100 ,触发后，需要写入与组转方向相反的位置指令，进行解除
+	kreg_mini_servo_servo_protect_time(RegIndex.kreg_mini_servo_servo_protect_time),    // 保护时间
+	kreg_mini_servo_servo_overload_torque(RegIndex.kreg_mini_servo_servo_overload_torque),    // 过载扭矩
+	kreg_mini_servo_servo_speed_p(RegIndex.kreg_mini_servo_servo_speed_p),    // 速度闭环P比例参数
+	kreg_mini_servo_servo_overload_time(RegIndex.kreg_mini_servo_servo_overload_time),    // 过流保护时间
+	kreg_mini_servo_servo_speed_i(RegIndex.kreg_mini_servo_servo_speed_i),    // 速度闭环I积分参数
+	kreg_mini_servo_servo_torque_switch(RegIndex.kreg_mini_servo_servo_torque_switch),    // 扭矩开关
+	kreg_mini_servo_servo_acc(RegIndex.kreg_mini_servo_servo_acc),    // 加速度
+	kreg_mini_servo_servo_target_pos(RegIndex.kreg_mini_servo_servo_target_pos),    // 目标位置
+	kreg_mini_servo_servo_run_time(RegIndex.kreg_mini_servo_servo_run_time),    // 运行时间
+	kreg_mini_servo_servo_run_speed(RegIndex.kreg_mini_servo_servo_run_speed),    // 运行速度
+	kreg_mini_servo_servo_torque_limit(RegIndex.kreg_mini_servo_servo_torque_limit),    // 转矩限制
+	kreg_mini_servo_servo_lock_flag(RegIndex.kreg_mini_servo_servo_lock_flag),    // 锁标志
+	kreg_mini_servo_servo_current_pos(RegIndex.kreg_mini_servo_servo_current_pos),    // 当前位置
+	kreg_mini_servo_servo_current_speed(RegIndex.kreg_mini_servo_servo_current_speed),    // 当前速度
+	kreg_mini_servo_servo_current_load(RegIndex.kreg_mini_servo_servo_current_load),    // 当前负载 bit10为方向位 输出驱动电机的当前占空比电压,单位为0.1%,取值0-1000
+	kreg_mini_servo_servo_current_voltage(RegIndex.kreg_mini_servo_servo_current_voltage),    // 当前电压 反馈当前舵机工作电压,反馈精度为0.1V,即120*0.1=12V
+	kreg_mini_servo_servo_current_temp(RegIndex.kreg_mini_servo_servo_current_temp),    // 当前温度 反馈当前舵机内部工作温度,反馈精度为1摄氏度
+	kreg_mini_servo_servo_status(RegIndex.kreg_mini_servo_servo_status),    // 舵机状态
+	kreg_mini_servo_servo_move_flag(RegIndex.kreg_mini_servo_servo_move_flag),    // 移动标志
+	kreg_mini_servo_servo_current_current(RegIndex.kreg_mini_servo_servo_current_current),    // 当前电流 反馈当前工作电流值,单位为6.26.5mA,最大可反馈电流为500*6.5mA=3250mA
+	;
+	public final RegIndex regIndex;
+
+	MiniServoRegIndex(RegIndex regIndex) {
+		this.regIndex = regIndex;
+	}
+}

src/main/java/com/iflytop/gd/hardware/drivers/Pump.java

@@ -1,7 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-/**
- * 物理泵
- */
-public class Pump {
-}

src/main/java/com/iflytop/gd/hardware/drivers/Relay.java

@@ -1,13 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-/**
- * 物理继电器
- */
-public class Relay{
-    public boolean open() {
-        return false;
-    }
-    public boolean close() {
-        return false;
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/ServoMotor.java

@@ -1,53 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-import com.iflytop.gd.hardware.type.RegIndex;
-
-
-/**
- * 物理伺服电机
- */
-public class ServoMotor {
-    public void enable() {
-
-    }
-
-    public void disable() {
-
-    }
-
-    public void stop() {
-
-    }
-
-    public Integer getCurrentPosition() {
-        return 0;
-    }
-
-    public void moveToZero() {
-
-    }
-
-    public void moveTo(Integer position) {
-
-    }
-
-    public void setMaxVelocity(Integer maxVelocity) {
-
-    }
-
-    public void setMaxTorque(Integer maxTorque) {
-
-    }
-
-    public void setProtectiveTorque(Integer protectiveTorque) {
-
-    }
-
-    public Integer readReg(RegIndex regIndex) {
-        return 0;
-    }
-
-    public void writeReg(RegIndex regIndex, Integer value) {
-
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotor.java

@@ -1,118 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-import com.iflytop.gd.hardware.type.RegIndex;
-import com.iflytop.gd.hardware.constants.DistanceUnit;
-import com.iflytop.gd.hardware.constants.RotationDirection;
-
-import java.util.Map;
-
-/**
- * 物理步进电机实现
- */
-public class StepMotor {
-    public void easyMoveTo(Integer value, DistanceUnit unit) {
-    }
-
-    public void easyMoveBy(Integer value, DistanceUnit unit) {
-    }
-
-    public void easyMoveToZero() {
-    }
-
-    public void easyMoveToZeroPointQuick() {
-
-    }
-
-    public void enable() {
-
-    }
-
-    public void disable() {
-
-    }
-
-    public void moveForward(RotationDirection direction, Integer distance, DistanceUnit unit) {
-
-    }
-
-    public void moveBackward(RotationDirection direction, Integer distance, DistanceUnit unit) {
-
-    }
-
-    public Integer readPosition() {
-        return 0;
-    }
-
-    public Integer readEncoderPosition() {
-        return 0;
-    }
-
-    public void stop() {
-
-    }
-
-    public void rotateForward() {
-
-    }
-
-    public void rotateBackward() {
-
-    }
-
-
-    public Map<String, Boolean> readIOState() {
-        return null;
-    }
-
-
-    public void setReg(RegIndex regIndex, Integer value) {
-    }
-
-    public Integer readReg(RegIndex regIndex) {
-        return 0;
-    }
-
-    public void setMRes(Integer value) {
-
-    }
-
-    public void setIRun(Integer value) {
-
-    }
-
-    public void setIHold(Integer value) {
-
-    }
-
-    public void setStartAndStopVelocity(Integer value) {
-
-    }
-
-    public void setV1(Integer value) {
-
-    }
-
-    public void setA1AndD1(Integer value) {
-
-    }
-
-    public void setAmaxAndDmax(Integer value) {
-
-    }
-
-    public void setDefaultVelocity(Integer value) {
-
-    }
-
-    public void setVelocity(Integer low, Integer mid, Integer high) {
-
-    }
-
-    public void setOneCyclePulse(Integer pause, Integer denominator) {
-
-    }
-
-    public void setDZero(Integer value) {
-
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/AcidPumpDriver.java

@@ -0,0 +1,70 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class AcidPumpDriver {
+    private final StepMotorCtrlDriver stepMotorCtrlDriver_;
+
+    private double speed_ = 10.0;         // mm/s
+    private int current_ = 5;             // [0-31]
+
+    // ==== ==== ==== ==== ==== ==== convert ==== ==== ==== ==== ==== ====
+    private int getMotorPosition(double position) {
+        int motorPosition = 0;
+        motorPosition = (int)position;
+        return motorPosition;
+    }
+
+    private int getMotorSpeed(double speed) {
+        int motorSpeed = 0;
+        motorSpeed = (int)speed;
+        return motorSpeed;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
+    private double getSpeed(StepMotorMId stepMotorMId)
+    {
+        // TODO: 从数据库中获取门的速度
+        return speed_;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
+    public void setCurrent(StepMotorMId stepMotorMId, int current) {
+        current_ = current;
+        // TODO: 写入数据库
+    }
+    public void setSpeed(StepMotorMId stepMotorMId, double speed) {
+        // 检查速度是否合法
+        speed_ = speed;
+        // TODO: 写入数据库
+    }
+
+    // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
+    public void moveToHome(StepMotorMId stepMotorMId) throws Exception
+    {
+        double speed = getSpeed(stepMotorMId);
+        int motorSpeed = getMotorSpeed(speed);
+        stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed);
+    }
+
+    public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception
+    {
+        // 检查位置是否合法
+        if (position < 0) {
+            throw new InvalidParameterException("Position is out of range");
+        }
+        this.setSpeed(stepMotorMId, speed);
+        stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed));
+    }
+
+    public void stop(StepMotorMId stepMotorMId) throws Exception {
+        stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DoorDriver.java

@@ -0,0 +1,70 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class DoorDriver{
+    private final StepMotorCtrlDriver stepMotorCtrlDriver_;
+
+    private double speed_ = 10.0;         // mm/s
+    private int current_ = 5;             // [0-31]
+
+    // ==== ==== ==== ==== ==== ==== convert ==== ==== ==== ==== ==== ====
+    private int getMotorPosition(double position) {
+        int motorPosition = 0;
+        motorPosition = (int)position;
+        return motorPosition;
+    }
+
+    private int getMotorSpeed(double speed) {
+        int motorSpeed = 0;
+        motorSpeed = (int)speed;
+        return motorSpeed;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
+    private double getSpeed(StepMotorMId stepMotorMId)
+    {
+        // TODO: 从数据库中获取门的速度
+        return speed_;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
+    public void setCurrent(StepMotorMId stepMotorMId, int current) {
+        current_ = current;
+        // TODO: 写入数据库
+    }
+    public void setSpeed(StepMotorMId stepMotorMId, double speed) {
+        // 检查速度是否合法
+        speed_ = speed;
+        // TODO: 写入数据库
+    }
+
+    // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
+    public void moveToHome(StepMotorMId stepMotorMId) throws Exception
+    {
+        double speed = getSpeed(stepMotorMId);
+        int motorSpeed = getMotorSpeed(speed);
+        stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed);
+    }
+
+    public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception
+    {
+        // 检查位置是否合法
+        if (position < 0) {
+            throw new InvalidParameterException("Position is out of range");
+        }
+        this.setSpeed(stepMotorMId, speed);
+        stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed));
+    }
+
+    public void stop(StepMotorMId stepMotorMId) throws Exception {
+        stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java

@@ -0,0 +1,70 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class HBotDriver {
+    private final StepMotorCtrlDriver stepMotorCtrlDriver_;
+
+    private double speed_ = 10.0;         // mm/s
+    private int current_ = 5;             // [0-31]
+
+    // ==== ==== ==== ==== ==== ==== convert ==== ==== ==== ==== ==== ====
+    private int getMotorPosition(double position) {
+        int motorPosition = 0;
+        motorPosition = (int)position;
+        return motorPosition;
+    }
+
+    private int getMotorSpeed(double speed) {
+        int motorSpeed = 0;
+        motorSpeed = (int)speed;
+        return motorSpeed;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
+    private double getSpeed(StepMotorMId stepMotorMId)
+    {
+        // TODO: 从数据库中获取门的速度
+        return speed_;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
+    public void setCurrent(StepMotorMId stepMotorMId, int current) {
+        current_ = current;
+        // TODO: 写入数据库
+    }
+    public void setSpeed(StepMotorMId stepMotorMId, double speed) {
+        // 检查速度是否合法
+        speed_ = speed;
+        // TODO: 写入数据库
+    }
+
+    // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
+    public void moveToHome(StepMotorMId stepMotorMId) throws Exception
+    {
+        double speed = getSpeed(stepMotorMId);
+        int motorSpeed = getMotorSpeed(speed);
+        stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed);
+    }
+
+    public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception
+    {
+        // 检查位置是否合法
+        if (position < 0) {
+            throw new InvalidParameterException("Position is out of range");
+        }
+        this.setSpeed(stepMotorMId, speed);
+        stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed));
+    }
+
+    public void stop(StepMotorMId stepMotorMId) throws Exception {
+        stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HeaterMotorDriver.java

@@ -0,0 +1,70 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class HeaterMotorDriver {
+    private final StepMotorCtrlDriver stepMotorCtrlDriver_;
+
+    private double speed_ = 10.0;         // mm/s
+    private int current_ = 5;             // [0-31]
+
+    // ==== ==== ==== ==== ==== ==== convert ==== ==== ==== ==== ==== ====
+    private int getMotorPosition(double position) {
+        int motorPosition = 0;
+        motorPosition = (int)position;
+        return motorPosition;
+    }
+
+    private int getMotorSpeed(double speed) {
+        int motorSpeed = 0;
+        motorSpeed = (int)speed;
+        return motorSpeed;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
+    private double getSpeed(StepMotorMId stepMotorMId)
+    {
+        // TODO: 从数据库中获取门的速度
+        return speed_;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
+    public void setCurrent(StepMotorMId stepMotorMId, int current) {
+        current_ = current;
+        // TODO: 写入数据库
+    }
+    public void setSpeed(StepMotorMId stepMotorMId, double speed) {
+        // 检查速度是否合法
+        speed_ = speed;
+        // TODO: 写入数据库
+    }
+
+    // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
+    public void moveToHome(StepMotorMId stepMotorMId) throws Exception
+    {
+        double speed = getSpeed(stepMotorMId);
+        int motorSpeed = getMotorSpeed(speed);
+        stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed);
+    }
+
+    public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception
+    {
+        // 检查位置是否合法
+        if (position < 0) {
+            throw new InvalidParameterException("Position is out of range");
+        }
+        this.setSpeed(stepMotorMId, speed);
+        stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed));
+    }
+
+    public void stop(StepMotorMId stepMotorMId) throws Exception {
+        stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java

@@ -0,0 +1,56 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class ShakeMotorDriver {
+    private final StepMotorCtrlDriver stepMotorCtrlDriver_;
+    private double speed_ = 10.0;         // mm/s
+    private int current_ = 5;             // [0-31]
+
+    // ==== ==== ==== ==== ==== ==== convert ==== ==== ==== ==== ==== ====
+    private int getMotorSpeed(double speed) {
+        int motorSpeed = 0;
+        motorSpeed = (int)speed;
+        return motorSpeed;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
+    private double getSpeed(StepMotorMId stepMotorMId)
+    {
+        // TODO: 从数据库中获取门的速度
+        return speed_;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
+    public void setCurrent(StepMotorMId stepMotorMId, int current) {
+        current_ = current;
+        // TODO: 写入数据库
+    }
+    public void setSpeed(StepMotorMId stepMotorMId, double speed) {
+        // 检查速度是否合法
+        speed_ = speed;
+        // TODO: 写入数据库
+    }
+
+    // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
+    public void start(StepMotorMId stepMotorMId) throws Exception
+    {
+        // TODO: 从数据库中获取门的速度
+        double speed = getSpeed(stepMotorMId);
+        int motorSpeed = getMotorSpeed(speed);
+        int direction = 1;
+        stepMotorCtrlDriver_.rotate(stepMotorMId, direction, motorSpeed);
+    }
+
+    public void stop(StepMotorMId stepMotorMId) throws Exception {
+        stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
+    }
+
+}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorCtrlDriver.java

@@ -0,0 +1,192 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
+import com.iflytop.gd.hardware.constants.ActionOvertimeConstant;
+import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.*;
+import lombok.RequiredArgsConstructor;
+import org.slf4j.Logger;
+import org.springframework.stereotype.Component;
+
+@Component
+@RequiredArgsConstructor
+public class StepMotorCtrlDriver {
+	static Logger logger = org.slf4j.LoggerFactory.getLogger(StepMotorCtrlDriver.class);
+
+	final private A8kCanBusService canBus;
+
+	private final ActionOvertimeConstant actionOvertimeConstant;
+
+	public void stepMotorEasyMoveBy(StepMotorMId id, Integer dpos) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_easy_move_by, dpos);
+	}
+
+	public void stepMotorEasyMoveTo(StepMotorMId id, Integer pos) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_easy_move_to, pos);
+	}
+
+	public void stepMotorEasyMoveToZero(StepMotorMId id) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_easy_move_to_zero);
+	}
+
+	public void setCurrentPos(StepMotorMId id, Integer pos) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_easy_set_current_pos, pos);
+	}
+
+	void stepMotorEasyMoveToZeroPointQuick(StepMotorMId id) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_easy_move_to_zero_point_quick);
+	}
+
+	void stepMotorEasyMoveToEndPoint(StepMotorMId id) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_easy_move_to_end_point);
+	}
+
+	public void stepMotorEnable(StepMotorMId mid, Integer enable) throws HardwareException {
+		canBus.callcmd(mid.mid, CmdId.step_motor_enable, enable);
+	}
+
+	public Integer stepMotorReadPos(StepMotorMId id) throws HardwareException {
+		A8kPacket packet = canBus.callcmd(id.mid, CmdId.step_motor_read_pos);
+		return packet.getContentI32(0);
+	}
+
+	public Integer stepMotorReadEncPos(StepMotorMId id) throws HardwareException {
+		A8kPacket packet = canBus.callcmd(id.mid, CmdId.step_motor_read_enc_pos);
+		return packet.getContentI32(0);
+	}
+
+	public void stepMotorEasyRotate(StepMotorMId id, Integer direction) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_easy_rotate, direction);
+	}
+
+	public void stepMotorEasyMoveByBlock(StepMotorMId id, Integer dpos) throws HardwareException {
+		logger.debug("stepMotorEasyMoveByBlock {} {}", id, dpos);
+		stepMotorEasyMoveBy(id, dpos);
+		canBus.waitForMod(id.mid, actionOvertimeConstant.get(id, CmdId.step_motor_easy_move_by));
+	}
+
+	public void waitForMod(StepMotorMId id, Integer overtime) throws HardwareException {
+		canBus.waitForMod(id.mid, overtime);
+	}
+
+	public void stepMotorEasyMoveToBlock(StepMotorMId id, Integer pos) throws HardwareException {
+		logger.debug("stepMotorEasyMoveToBlock {} {}", id, pos);
+		stepMotorEasyMoveTo(id, pos);
+		canBus.waitForMod(id.mid, actionOvertimeConstant.get(id, CmdId.step_motor_easy_move_to));
+	}
+
+	public void stepMotorEasyMoveToZeroBlock(StepMotorMId id) throws HardwareException {
+		logger.debug("stepMotorEasyMoveToZeroBlock {}", id);
+		stepMotorEasyMoveToZero(id);
+		canBus.waitForMod(id.mid, actionOvertimeConstant.get(id, CmdId.step_motor_easy_move_to_zero));
+	}
+
+	public Integer stepMotorReadPosByMoveToZeroBlock(StepMotorMId id) throws HardwareException {
+		stepMotorEnable(id, 1);
+		stepMotorEasyMoveToZeroBlock(id);
+		Integer nowpos     = stepMotorReadPos(id);
+		Integer measurepos = -canBus.moduleGetReg(id.mid, RegIndex.kreg_step_motor_dpos) + nowpos;
+		stepMotorEnable(id, 0);
+		return measurepos;
+	}
+
+	public void stepMotorEasyMoveToZeroPointQuickBlock(StepMotorMId id) throws HardwareException {
+		stepMotorEasyMoveToZeroPointQuick(id);
+		canBus.waitForMod(id.mid, actionOvertimeConstant.get(id, CmdId.step_motor_easy_move_to_zero_point_quick));
+	}
+
+	public void stepMotorStop(StepMotorMId id) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_stop, 0);
+	}
+
+
+	//	step_motor_move_by(0x021d, "STEP_MOTOR_MOVE_BY"),  // (dpos,speedlevel)->null speedlevel=0,1,2,3(default,low,mid,high)
+	//	step_motor_move_to(0x021e, "STEP_MOTOR_MOVE_TO"),  // (pos,speedlevel)->null speedlevel=0,1,2,3(default,low,mid,high)
+	//	step_motor_move_to_zero_point_quick(0x021f, "STEP_MOTOR_MOVE_TO_ZERO_POINT_QUICK"),  // (speedlevel)->null speedlevel=0,1,2,3(default,low,mid,high)
+	//	step_motor_rotate(0x0220, "STEP_MOTOR_ROTATE"),  //
+
+//	public void moveByBlock(StepMotorMId id, Integer dpos, StepMotorSpeedLevel speedLevel) throws HardwareException {
+//		canBus.callcmd(id.mid, CmdId.step_motor_move_by, dpos, speedLevel.ordinal());
+//		waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_by));
+//	}
+//
+//	public void moveToBlock(StepMotorMId id, Integer pos, StepMotorSpeedLevel speedLevel) throws HardwareException {
+//		canBus.callcmd(id.mid, CmdId.step_motor_move_to, pos, speedLevel.ordinal());
+//		waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to));
+//	}
+//
+//	public void moveToZeroQuickBlock(StepMotorMId id, StepMotorSpeedLevel speedLevel) throws HardwareException {
+//		canBus.callcmd(id.mid, CmdId.step_motor_move_to_zero_point_quick, speedLevel.ordinal());
+//		waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to_zero_point_quick));
+//	}
+//
+//	public void rotate(StepMotorMId id, Integer direction, StepMotorSpeedLevel speedLevel) throws HardwareException {
+//		canBus.callcmd(id.mid, CmdId.step_motor_rotate, direction, speedLevel.ordinal());
+//	}
+
+	public Boolean stepMotorReadIoState(StepMotorMId id, Integer ioindex) throws HardwareException {
+		var packet = canBus.callcmd(id.mid, CmdId.step_motor_read_io_state, ioindex);
+		return packet.getContentI32(0) != 0;
+	}
+
+	public void stepMotorEasyMoveToEndPointBlock(StepMotorMId id) throws HardwareException {
+		stepMotorEasyMoveToEndPoint(id);
+		canBus.waitForMod(id.mid, actionOvertimeConstant.get(id, CmdId.step_motor_easy_move_to_end_point));
+	}
+
+	void stepMotorEasyReciprocatingMotion(StepMotorMId id, Integer startpos, Integer endpos, Integer times) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_easy_reciprocating_motion, startpos, endpos, times);
+	}
+
+	public void stepMotorEasyReciprocatingMotionBlock(StepMotorMId id, Integer startpos, Integer endpos, Integer times) throws HardwareException {
+		stepMotorEasyReciprocatingMotion(id, startpos, endpos, times);
+		canBus.waitForMod(id.mid, actionOvertimeConstant.get(id, CmdId.step_motor_easy_reciprocating_motion));
+	}
+
+	// ==== ==== ==== ==== ==== ==== raw ctrl ==== ==== ==== ==== ==== ====
+	public void rotate(StepMotorMId id, Integer direction, Integer speed) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_rotate, direction, speed);
+	}
+
+	public void moveToBlock(StepMotorMId id, Integer pos, Integer speed) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_move_to, pos, speed);
+		waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to));
+	}
+
+	public void moveByBlock(StepMotorMId id, Integer pos, Integer speed) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_move_by, pos, speed);
+		waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_by));
+	}
+
+	public void moveToZeroQuickBlock(StepMotorMId id, Integer speed) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.step_motor_move_to_zero_point_quick, speed);
+		waitForMod(id, actionOvertimeConstant.get(id, CmdId.step_motor_move_to_zero_point_quick));
+	}
+
+
+//	public Boolean isHasMoveToZero(StepMotorMId id) throws HardwareException {
+//		return getReg(id, StepMotorRegIndex.kreg_step_motor_has_move_zero) != 0;
+//	}
+
+//	public void setReg(StepMotorMId id, StepMotorRegIndex regIndex, Integer val) throws HardwareException {
+//		canBus.moduleSetReg(id.mid, regIndex.regIndex, val);
+//	}
+
+//	public Integer getReg(StepMotorMId id, StepMotorRegIndex regIndex) throws HardwareException {
+//		return canBus.moduleGetReg(id.mid, regIndex.regIndex);
+//	}
+
+	public void resetRegs(StepMotorMId id) throws HardwareException {
+		canBus.callcmd(id.mid, CmdId.module_reset_reg);
+	}
+
+//	public Object getAllReg(StepMotorMId id) throws HardwareException {
+//		ObjectNode node = ZJsonHelper.createObjectNode();
+//		for (StepMotorRegIndex regIndex : StepMotorRegIndex.values()) {
+//			Integer regVal = getReg(id, regIndex);
+//			logger.debug("read reg {} -> {}", regIndex, regVal);
+//			node.put(regIndex.name(), getReg(id, regIndex));
+//		}
+//		return node;
+//	}
+}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorMId.java

@@ -0,0 +1,36 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+import com.iflytop.gd.hardware.type.MId;
+import org.springframework.util.Assert;
+
+public enum StepMotorMId {
+	DOOR_MOTOR_MID(MId.DoorM),
+	SHAKE_MOTOR_MID(MId.ShakeM),
+	TRAY_MOTOR_MID(MId.CapStorageM),
+	DUAL_ROBOT_AXIS1_MOTOR_MID(MId.DualRobotAxis1M),
+	DUAL_ROBOT_AXIS2_MOTOR_MID(MId.DualRobotAxis2M),
+	HBOT_X_MOTOR_MID(MId.HBotXM),
+	HBOT_Y_MOTOR_MID(MId.HBotYM),
+	HBOT_Z_MOTOR_MID(MId.HBotZM),
+	HEATER_1_MOTOR_MID(MId.Heater1M),
+	HEATER_2_MOTOR_MID(MId.Heater2M),
+	HEATER_3_MOTOR_MID(MId.Heater3M),
+	HEATER_4_MOTOR_MID(MId.Heater4M),
+	HEATER_5_MOTOR_MID(MId.Heater5M),
+	HEATER_6_MOTOR_MID(MId.Heater6M),
+	ACID_PUMP_1_MOTOR_MID(MId.AcidPump1M),
+	ACID_PUMP_2_MOTOR_MID(MId.AcidPump2M),
+	ACID_PUMP_3_MOTOR_MID(MId.AcidPump3M),
+	ACID_PUMP_4_MOTOR_MID(MId.AcidPump4M),
+	ACID_PUMP_5_MOTOR_MID(MId.AcidPump5M),
+	ACID_PUMP_6_MOTOR_MID(MId.AcidPump6M),
+	ACID_PUMP_7_MOTOR_MID(MId.AcidPump7M),
+	ACID_PUMP_8_MOTOR_MID(MId.AcidPump8M),
+	;
+	final public MId mid;
+
+	StepMotorMId(MId mid) {
+		Assert.isTrue(this.name().equals(mid.name()),"StepMotorMId init fail");
+		this.mid = mid;
+	}
+}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorSpeedLevel.java

@@ -0,0 +1,9 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+public enum StepMotorSpeedLevel {
+	DEFAULT,
+	LOW,
+	MID,
+	HIGH,
+	;
+}

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java

@@ -0,0 +1,90 @@
+package com.iflytop.gd.hardware.drivers.StepMotorDriver;
+
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+import java.security.InvalidParameterException;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class TrayMotorDriver {
+    private final StepMotorCtrlDriver stepMotorCtrlDriver_;
+
+    private double speed_ = 10.0;         // mm/s
+    private int current_ = 5;             // [0-31]
+
+    private int trayLevelCount_ = 10;     // 托盘层数
+    private double trayInterval_ = 10;       // mm
+    private double tray0Position_ = 0;       // 托盘 0 位置 mm
+
+    // ==== ==== ==== ==== ==== ==== convert ==== ==== ==== ==== ==== ====
+    private int getMotorPosition(double position) {
+        int motorPosition = 0;
+        motorPosition = (int)position;
+        return motorPosition;
+    }
+
+    private int getMotorSpeed(double speed) {
+        int motorSpeed = 0;
+        motorSpeed = (int)speed;
+        return motorSpeed;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
+    double getTrayPosition(int trayLevel) {
+        // TODO: 从数据库中获取托盘的位置
+        return tray0Position_ + trayLevel * trayInterval_;
+    }
+
+    private double getSpeed(StepMotorMId stepMotorMId)
+    {
+        // TODO: 从数据库中获取门的速度
+        return speed_;
+    }
+
+    // ==== ==== ==== ==== ==== ==== Set ==== ==== ==== ==== ==== ====
+    public void setCurrent(StepMotorMId stepMotorMId, int current) {
+        current_ = current;
+        // TODO: 写入数据库
+    }
+    public void setSpeed(StepMotorMId stepMotorMId, double speed) {
+        // 检查速度是否合法
+        speed_ = speed;
+        // TODO: 写入数据库
+    }
+
+    // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
+    public void moveToHome(StepMotorMId stepMotorMId) throws Exception
+    {
+        double speed = getSpeed(stepMotorMId);
+        int motorSpeed = getMotorSpeed(speed);
+        stepMotorCtrlDriver_.moveToZeroQuickBlock(stepMotorMId, motorSpeed);
+    }
+
+    public void moveTo(StepMotorMId stepMotorMId, double position, double speed) throws Exception
+    {
+        // 检查位置是否合法
+        if (position < 0) {
+            throw new InvalidParameterException("Position is out of range");
+        }
+        this.setSpeed(stepMotorMId, speed);
+        stepMotorCtrlDriver_.moveToBlock(stepMotorMId, getMotorPosition(position), getMotorSpeed(speed));
+    }
+
+    public void stop(StepMotorMId stepMotorMId) throws Exception {
+        stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
+    }
+
+    // ==== ==== ==== ==== ==== ==== Application ==== ==== ==== ==== ==== ====
+    public void moveToTrayLevel(StepMotorMId stepMotorMId, int trayLevel) throws Exception {
+        double position = getTrayPosition(trayLevel);
+        double speed = getSpeed(stepMotorMId);
+
+        int motorPosition = getMotorPosition(position);
+        int motorSpeed = getMotorSpeed(speed);
+
+        stepMotorCtrlDriver_.moveToBlock(stepMotorMId, motorPosition, motorSpeed);
+    }
+}

src/main/java/com/iflytop/gd/hardware/drivers/SwitchSensor.java

@@ -1,11 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-/**
- * 物理传感器
- */
-
-public class SwitchSensor {
-    public boolean isOpen() {
-        return false;
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/TransportationArm.java

@@ -1,27 +0,0 @@
-package com.iflytop.gd.hardware.drivers;
-
-import com.iflytop.gd.hardware.constants.Dim;
-import com.iflytop.gd.hardware.constants.DistanceUnit;
-import com.iflytop.gd.hardware.constants.VelocityUnit;
-
-/**
- * 物理转移机械臂
- */
-
-public class TransportationArm {
-    public void moveTo(Dim dim, Integer distance, DistanceUnit unit) {
-
-    }
-
-    public void relativelyMove(Dim dim, Integer distance, DistanceUnit unit) {
-
-    }
-
-    public void stop(Dim dim) {
-
-    }
-
-    public void setSpeed(Dim dim, Integer speed, VelocityUnit unit) {
-
-    }
-}

src/main/java/com/iflytop/gd/hardware/drivers/TricolorLightDriver.java

@@ -0,0 +1,28 @@
+package com.iflytop.gd.hardware.drivers;
+
+
+import com.iflytop.gd.hardware.comm.can.A8kCanBusService;
+import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.CmdId;
+import com.iflytop.gd.hardware.type.MId;
+import lombok.RequiredArgsConstructor;
+import lombok.extern.slf4j.Slf4j;
+import org.springframework.stereotype.Component;
+
+@Slf4j
+@Component
+@RequiredArgsConstructor
+public class TricolorLightDriver {
+    private final A8kCanBusService canBus;
+    public  enum Color {
+        RED, GREEN, BLUE
+    }
+
+    public void open(MId mid, Color color) throws HardwareException {
+        canBus.callcmd(mid, CmdId.module_tricolor_light_on, color.ordinal());
+    }
+
+    public void close(MId mid) throws HardwareException{
+        canBus.callcmd(mid, CmdId.module_tricolor_light_off);
+    }
+}

src/main/java/com/iflytop/gd/hardware/factory/A8kPacketFactory.java

@@ -13,7 +13,7 @@ public class A8kPacketFactory {
     //  uint8_t  packetType;
     //  uint16_t cmdid;
     //  uint8_t  moduleId;
-    //  uint8_t  index;
+    //  uint16_t  index;
     //  uint8_t  datalen;
     //  uint8_t  data[];
     //  /* int8_t  checksum;*/

src/main/java/com/iflytop/gd/hardware/service/HardwareService.java

@@ -5,7 +5,8 @@ import cn.hutool.core.util.StrUtil;
 import com.iflytop.gd.common.cmd.DeviceCommand;
 import com.iflytop.gd.common.enums.cmd.CmdDevice;
 import com.iflytop.gd.hardware.command.CommandHandler;
-import com.iflytop.gd.hardware.command.SupportMethod;
+import com.iflytop.gd.hardware.command.checker.SupportMethod;
+import jakarta.annotation.PostConstruct;
 import org.springframework.stereotype.Component;
 
 import java.security.InvalidParameterException;
@@ -17,9 +18,10 @@ import java.util.Map;
 public class HardwareService {
     private final Map<CmdDevice, CommandHandler> cmdHandlers = new HashMap<>();
 
-    public void post_initialize()
+    @PostConstruct
+    public void postInit()
     {
-//        cmdHandlers.put(CmdDevice.door, new FanHandler(0));
+
     }
 
     public boolean sendCommand(DeviceCommand cmd)
@@ -30,17 +32,17 @@ public class HardwareService {
             SupportMethod.checkMethod(strMethod);
 
             if(cmdHandlers.containsKey(cmd.getDevice())) {
-                return cmdHandlers.get(cmd.getDevice()).sendCommand(cmd);
+                cmdHandlers.get(cmd.getDevice()).sendCommand(cmd);
             }
             else {
                 throw new InvalidParameterException(StrUtil.format("[Device]: {}", cmd.getDevice()));
             }
+            return true;
         }
         catch (Exception e) {
             handleException(e);
             return false;
         }
-
     }
 
     void handleException(Exception e)

src/main/java/com/iflytop/gd/hardware/type/CmdId.java

@@ -71,6 +71,17 @@ public enum CmdId {
     step_motor_easy_reciprocating_motion(0x022d, "STEP_MOTOR_EASY_RECIPROCATING_MOTION"),//
     step_motor_easy_move_to_zero_point_quick(0x022e, "STEP_MOTOR_EASY_MOVE_TO_ZERO_POINT_QUICK"),
 
+    mini_servo_enable(0x6601, "MINI_SERVO_ENABLE"),//
+    mini_servo_read_pos(0x6602, "MINI_SERVO_READ_POS"),//
+    mini_servo_active_cfg(0x6603, "MINI_SERVO_ACTIVE_CFG"),//
+    mini_servo_stop(0x6604, "MINI_SERVO_STOP"),//
+    mini_servo_set_mid_point(0x6607, "MINI_SERVO_SET_MID_POINT"),//
+    mini_servo_read_io_state(0x6608, "MINI_SERVO_READ_IO_STATE"),//
+    mini_servo_move_to(0x6609, "MINI_SERVO_MOVE_TO"),//
+    mini_servo_rotate(0x660a, "MINI_SERVO_ROTATE"),//
+    mini_servo_rotate_with_torque(0x660b, "MINI_SERVO_ROTATE_WITH_TORQUE"),//
+    mini_servo_set_cur_pos(0x660c, "MINI_SERVO_SET_CUR_POS"),//
+
 
     ;
 

src/main/java/com/iflytop/gd/hardware/type/MId.java

@@ -5,6 +5,7 @@ public enum MId {
     IO1Board(10, "台面 IO 板模块"),
     PWMLight(11, "PWM 灯"),
     TriColorLight(12, "三色灯"),
+    HBotClawS(13, "夹爪舵机"),//
 
     IO2Board(30, "台下 IO 板模块"),
 
@@ -12,7 +13,7 @@ public enum MId {
     DoorM(81, "门电机"),
 
     ShakeModBoard(90, "摇匀模组板"),    //
-    ShakeModShakeM(91, "加液位摇匀电机"), //
+    ShakeM(91, "加液位摇匀电机"), //
 
     CapStorageBoard(100, "拍子存放板模块"),     //
     CapStorageM(101, "拍子存放电机"),          //

src/main/java/com/iflytop/gd/hardware/type/ModuleType.java

@@ -0,0 +1,34 @@
+//package com.iflytop.gd.hardware.type;
+//
+//public enum ModuleType {
+//	Board(0),  // 板子
+//	HBOT(1),  // hbot模块
+//	WaterCoolingTempCtrl(2),  // 温度控制
+//	FanCtrl(3),  // 风扇控制
+//	CodeScaner(4),  // 扫码器
+//	TMCStepMotor(5),  // 步进电机
+//	MiniServo(6),  // 舵机
+//	PipetteGunCtrl(7),  // 移液体枪控制
+//
+//	A8kOptical(100),  // a8000光学模组
+//	A8KIdCardReader(101),  // id卡读卡器
+//	A8KPlateCodeScaner(102),  // 反应板条扫码器
+//
+//	UNKNOWN(999); // 未知类型
+//	public final int code;
+//
+//	ModuleType(int val) {
+//		this.code = val;
+//	}
+//
+//	static public ModuleType of(int val) {
+//		for (ModuleType type : ModuleType.values()) {
+//			if (type.code == val) {
+//				return type;
+//			}
+//		}
+//		return UNKNOWN;
+//	}
+//
+//
+//}

src/main/java/com/iflytop/gd/hardware/type/RegIndex.java

@@ -51,6 +51,65 @@ public enum RegIndex {
     /***********************************************************************************************************************
      * step_motor end *
      ***********************************************************************************************************************/
+
+
+    /***********************************************************************************************************************
+     * step_motor *
+     ***********************************************************************************************************************/
+
+    kreg_mini_servo_pos(10201), // 位置
+    kreg_mini_servo_limit_velocity(10251), // 限制速度
+    kreg_mini_servo_limit_torque(10252), // 限制扭矩
+    kreg_mini_servo_protective_torque(10253), // 保护扭矩
+    kreg_mini_servo_is_move(10254), // 是否在运动
+    kreg_mini_servo_status(10255), // 舵机状态
+    kreg_mini_servo_voltage(10256), // 电压 反馈当前舵机工作电压,反馈精度为0.1V,即120*0.1=12V
+    kreg_mini_servo_current(10257), // 电流 反馈当前工作电流值,单位为ma
+    kreg_mini_servo_temperature(10258), // 温度 反馈当前舵机内部工作温度,反馈精度为1摄氏度
+    kreg_mini_servo_loadvalue(10259), // 负载值 输出驱动电机的当前占空比电压,单位为0.1%,取值0-1000
+    kreg_mini_servo_target_pos_tolerance(10260), // 目标位置容忍度
+
+
+    kreg_mini_servo_firmware_main_version(10500),     // 固件主版本号
+    kreg_mini_servo_firmware_sub_version(10501),     // 固件次版本号
+    kreg_mini_servo_servo_main_version(10503),     // 舵机主版本号
+    kreg_mini_servo_servo_sub_version(10504),     // 舵机次版本号
+    kreg_mini_servo_servo_min_angle(10509),     // 最小角度限制
+    kreg_mini_servo_servo_max_angle(10511),     // 最大角度限制
+    kreg_mini_servo_servo_max_temp(10513),     // 最高温度上限
+    kreg_mini_servo_servo_max_voltage(10514),     // 最高输入电压
+    kreg_mini_servo_servo_min_voltage(10515),     // 最低输入电压
+    kreg_mini_servo_servo_max_torque(10516),     // 最大扭矩
+    kreg_mini_servo_servo_unload_condition(10519),     // 卸载条件
+    kreg_mini_servo_servo_p(10521),     // P 比例系
+    kreg_mini_servo_servo_d(10522),     // D 微分系
+    kreg_mini_servo_servo_i(10523),     // I
+    kreg_mini_servo_servo_min_start(10524),     // 最小启动
+    kreg_mini_servo_servo_cw_dead_zone(10526),    // 顺时针不灵敏区
+    kreg_mini_servo_servo_ccw_dead_zone(10527),    // 逆时针不灵敏
+    kreg_mini_servo_servo_protect_current(10528),    // 保护电流
+    kreg_mini_servo_servo_protect_torque(10534),    // 保护扭矩 0->100 ,触发后，需要写入与组转方向相反的位置指令，进行解除
+    kreg_mini_servo_servo_protect_time(10535),    // 保护时间
+    kreg_mini_servo_servo_overload_torque(10536),    // 过载扭矩
+    kreg_mini_servo_servo_speed_p(10537),    // 速度闭环P比例参数
+    kreg_mini_servo_servo_overload_time(10538),    // 过流保护时间
+    kreg_mini_servo_servo_speed_i(10539),    // 速度闭环I积分参数
+    kreg_mini_servo_servo_torque_switch(10540),    // 扭矩开关
+    kreg_mini_servo_servo_acc(10541),    // 加速度
+    kreg_mini_servo_servo_target_pos(10542),    // 目标位置
+    kreg_mini_servo_servo_run_time(10544),    // 运行时间
+    kreg_mini_servo_servo_run_speed(10546),    // 运行速度
+    kreg_mini_servo_servo_torque_limit(10548),    // 转矩限制
+    kreg_mini_servo_servo_lock_flag(10555),    // 锁标志
+    kreg_mini_servo_servo_current_pos(10556),    // 当前位置
+    kreg_mini_servo_servo_current_speed(10558),    // 当前速度
+    kreg_mini_servo_servo_current_load(10560),    // 当前负载 bit10为方向位 输出驱动电机的当前占空比电压,单位为0.1%,取值0-1000
+    kreg_mini_servo_servo_current_voltage(10562),    // 当前电压 反馈当前舵机工作电压,反馈精度为0.1V,即120*0.1=12V
+    kreg_mini_servo_servo_current_temp(10563),    // 当前温度 反馈当前舵机内部工作温度,反馈精度为1摄氏度
+    kreg_mini_servo_servo_status(10565),    // 舵机状态
+    kreg_mini_servo_servo_move_flag(10566),    // 移动标志
+    kreg_mini_servo_servo_current_current(10569),    // 当前电流 反馈当前工作电流值,单位为6.26.5mA,最大可反馈电流为500*6.5mA=3250mA
+
     ;
 
     public final int     index;