add:托盘电机 Hbot Z 轴电机增加抱轴控制

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

@@ -6,6 +6,7 @@ import com.iflytop.gd.common.enums.cmd.CmdDevice;
 import com.iflytop.gd.common.enums.cmd.CmdDirection;
 import com.iflytop.gd.hardware.command.CommandHandler;
 import com.iflytop.gd.hardware.drivers.StepMotorDriver.HBotDriver;
+import com.iflytop.gd.hardware.type.IO.OutputIOMId;
 import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId;
 import com.iflytop.gd.hardware.type.MId;
 import lombok.RequiredArgsConstructor;
@@ -22,6 +23,10 @@ import java.util.stream.Collectors;
 public class HBotHandler extends CommandHandler {
     private final HBotDriver driver_;
 
+    private final Map<CmdDevice, OutputIOMId>  supportCmdDeviceIOOutputMap = Map.ofEntries(
+            Map.entry(CmdDevice.gantry_z, OutputIOMId.DO_HBOTZ_MOTOR_CLAMP)
+    );
+
     private final Map<CmdDevice, StepMotorMId> stepMotorMIdMap_ = Map.ofEntries(
             Map.entry(CmdDevice.gantry_x, StepMotorMId.HBOT_X_MOTOR_MID),
             Map.entry(CmdDevice.gantry_y, StepMotorMId.HBOT_Y_MOTOR_MID),
@@ -46,6 +51,9 @@ public class HBotHandler extends CommandHandler {
     public void handleCommand(DeviceCommand command) throws Exception {
         // 发送命令
         StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+
+        OutputIOMId clampMId = supportCmdDeviceIOOutputMap.get(command.getDevice());
+
         if (command.getAction() == CmdAction.origin) {
             driver_.moveToHome(stepMotorMId);
         }
@@ -53,13 +61,17 @@ public class HBotHandler extends CommandHandler {
             driver_.stop(stepMotorMId);
         }
         else if(command.getAction() == CmdAction.move) {
+            driver_.openClamp(clampMId);
             double position = command.getParam().getPosition();
             driver_.moveTo(stepMotorMId, position);
+            driver_.closeClamp(clampMId);
         }
         else if(command.getAction() == CmdAction.move_by) {
+            driver_.openClamp(clampMId);
             double distance = command.getParam().getPosition();
 
             driver_.moveBy(stepMotorMId, distance);
+            driver_.closeClamp(clampMId);
         }
         else if(command.getAction() == CmdAction.set) {
             Double speed = command.getParam().getSpeed();

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

@@ -4,7 +4,6 @@ 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.common.enums.cmd.CmdDirection;
 import com.iflytop.gd.hardware.command.CommandHandler;
 import com.iflytop.gd.hardware.drivers.StepMotorDriver.ShakeMotorDriver;
 import com.iflytop.gd.hardware.type.StepMotor.StepMotorDirect;
@@ -49,20 +48,8 @@ public class ShakeMotorHandler extends CommandHandler {
         // 发送命令
         StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
 
-        if (command.getAction() == CmdAction.origin) {
-            driver_.moveToHome(stepMotorMId);
-        }
-        else if(command.getAction() == CmdAction.stop) {
-            driver_.stop(stepMotorMId);
-        }
-        else if(command.getAction() == CmdAction.move) {
-            double position = command.getParam().getPosition();
-            driver_.moveTo(stepMotorMId, position);
-        }
-        else if(command.getAction() == CmdAction.move_by) {
-            double distance = command.getParam().getPosition();
-
-            driver_.moveBy(stepMotorMId, distance);
+        if(command.getAction() == CmdAction.stop) {
+            driver_.stopShake(stepMotorMId);
         }
         else if(command.getAction() == CmdAction.set) {
             Double speed = command.getParam().getSpeed();
@@ -72,7 +59,7 @@ public class ShakeMotorHandler extends CommandHandler {
         }
         else if (command.getAction() == CmdAction.start) {
             StepMotorDirect direct = StepMotorDirect.FORWARD;
-            driver_.shake(stepMotorMId, direct);
+            driver_.startShake(stepMotorMId, direct);
         }
     }
 }

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

@@ -5,6 +5,8 @@ import com.iflytop.gd.common.enums.cmd.CmdAction;
 import com.iflytop.gd.common.enums.cmd.CmdDevice;
 import com.iflytop.gd.common.enums.cmd.CmdDirection;
 import com.iflytop.gd.hardware.command.CommandHandler;
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver;
+import com.iflytop.gd.hardware.type.IO.OutputIOMId;
 import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId;
 import com.iflytop.gd.hardware.drivers.StepMotorDriver.TrayMotorDriver;
 import com.iflytop.gd.hardware.type.MId;
@@ -22,6 +24,11 @@ import java.util.stream.Collectors;
 public class TrayMotorHandler extends CommandHandler {
     private final TrayMotorDriver driver_;
 
+
+    private final Map<CmdDevice, OutputIOMId>  supportCmdDeviceIOOutputMap = Map.ofEntries(
+            Map.entry(CmdDevice.tray_motor, OutputIOMId.DO_TRAY_MOTOR_CLAMP)
+    );
+
     private final Map<CmdDevice, StepMotorMId> stepMotorMIdMap_ = Map.ofEntries(
             Map.entry(CmdDevice.tray_motor, StepMotorMId.SHAKE_MOTOR_MID)
     );
@@ -45,6 +52,9 @@ public class TrayMotorHandler extends CommandHandler {
     public void handleCommand(DeviceCommand command) throws Exception {
         // 发送命令
         StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice());
+
+        OutputIOMId clampMId = supportCmdDeviceIOOutputMap.get(command.getDevice());
+
         if (command.getAction() == CmdAction.origin) {
             driver_.moveToHome(stepMotorMId);
         }
@@ -52,13 +62,16 @@ public class TrayMotorHandler extends CommandHandler {
             driver_.stop(stepMotorMId);
         }
         else if(command.getAction() == CmdAction.move) {
+            driver_.openClamp(clampMId);
             double position = command.getParam().getPosition();
             driver_.moveTo(stepMotorMId, position);
+            driver_.closeClamp(clampMId);
         }
         else if(command.getAction() == CmdAction.move_by) {
+            driver_.openClamp(clampMId);
             double distance = command.getParam().getPosition();
-
             driver_.moveBy(stepMotorMId, distance);
+            driver_.closeClamp(clampMId);
         }
         else if(command.getAction() == CmdAction.set) {
             Double speed = command.getParam().getSpeed();

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

@@ -17,4 +17,14 @@ public class OutputIOCtrlDriver {
 	public void setIOState(OutputIOMId IOOutput, Boolean state) throws HardwareException {
 		canBus.callcmd(IOOutput.mid, CmdId.extboard_write_outio, IOOutput.ioIndex, state ? 1 : 0);
 	}
+
+	public void open(OutputIOMId IOOutput) throws HardwareException
+	{
+		this.setIOState(IOOutput, true);
+	}
+
+	public void close(OutputIOMId IOOutput) throws HardwareException
+	{
+		this.setIOState(IOOutput, false);
+	}
 }

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

@@ -1,7 +1,9 @@
 package com.iflytop.gd.hardware.drivers.StepMotorDriver;
 
 import com.iflytop.gd.hardware.config.StepMotorConfig;
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver;
 import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.IO.OutputIOMId;
 import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId;
 import com.iflytop.gd.hardware.type.StepMotor.StepMotorRegIndex;
 import lombok.RequiredArgsConstructor;
@@ -15,6 +17,7 @@ import java.security.InvalidParameterException;
 @RequiredArgsConstructor
 public class HBotDriver {
     private final StepMotorConfig config_;
+    private final OutputIOCtrlDriver doDriver_;
     private final StepMotorCtrlDriver stepMotorCtrlDriver_;
 
     // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
@@ -63,4 +66,21 @@ public class HBotDriver {
     public void stop(StepMotorMId stepMotorMId) throws Exception {
         stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
     }
+
+    // ==== ==== ==== ==== ==== ==== IO Ctrl ==== ==== ==== ==== ====
+    public void openClamp(OutputIOMId iomId) throws Exception
+    {
+        if(iomId == null) {
+            return;
+        }
+        doDriver_.open(iomId);
+    }
+
+    public void closeClamp(OutputIOMId iomId) throws Exception
+    {
+        if(iomId == null) {
+            return;
+        }
+        doDriver_.close(iomId);
+    }
 }

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

@@ -66,8 +66,14 @@ public class ShakeMotorDriver {
     }
 
     // ==== ==== ==== ==== ==== ==== Ctrl ==== ==== ==== ==== ==== ====
-    public void shake(StepMotorMId stepMotorMId, StepMotorDirect direct) throws Exception
+    public void startShake(StepMotorMId stepMotorMId, StepMotorDirect direct) throws Exception
     {
         stepMotorCtrlDriver_.stepMotorEasyRotate(stepMotorMId, direct.getValue());
     }
+
+    public void stopShake(StepMotorMId stepMotorMId) throws Exception
+    {
+        this.stop(stepMotorMId);
+        this.moveToHome(stepMotorMId);
+    }
 }

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

@@ -1,7 +1,9 @@
 package com.iflytop.gd.hardware.drivers.StepMotorDriver;
 
 import com.iflytop.gd.hardware.config.StepMotorConfig;
+import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver;
 import com.iflytop.gd.hardware.exception.HardwareException;
+import com.iflytop.gd.hardware.type.IO.OutputIOMId;
 import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId;
 import com.iflytop.gd.hardware.type.StepMotor.StepMotorRegIndex;
 import lombok.RequiredArgsConstructor;
@@ -15,6 +17,7 @@ import java.security.InvalidParameterException;
 @RequiredArgsConstructor
 public class TrayMotorDriver {
     private final StepMotorConfig config_;
+    private final OutputIOCtrlDriver doDriver_;
     private final StepMotorCtrlDriver stepMotorCtrlDriver_;
 
     // ==== ==== ==== ==== ==== ==== Get ==== ==== ==== ==== ==== ====
@@ -63,4 +66,21 @@ public class TrayMotorDriver {
     public void stop(StepMotorMId stepMotorMId) throws Exception {
         stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
     }
+
+    // ==== ==== ==== ==== ==== ==== IO Ctrl ==== ==== ==== ==== ====
+    public void openClamp(OutputIOMId iomId) throws Exception
+    {
+        if(iomId == null) {
+            return;
+        }
+        doDriver_.open(iomId);
+    }
+
+    public void closeClamp(OutputIOMId iomId) throws Exception
+    {
+        if(iomId == null) {
+            return;
+        }
+        doDriver_.close(iomId);
+    }
 }