From 46668e6fb90bac8029ea6a5e7364dccd03b34e0c Mon Sep 17 00:00:00 2001 From: HSZ_HeSongZhen <210202959@qq.com> Date: Wed, 7 May 2025 19:48:08 +0800 Subject: [PATCH] =?UTF-8?q?add:=E6=89=98=E7=9B=98=E7=94=B5=E6=9C=BA=20Hbot?= =?UTF-8?q?=20Z=20=E8=BD=B4=E7=94=B5=E6=9C=BA=E5=A2=9E=E5=8A=A0=E6=8A=B1?= =?UTF-8?q?=E8=BD=B4=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../gd/hardware/command/handlers/HBotHandler.java | 12 ++++++++++++ .../hardware/command/handlers/ShakeMotorHandler.java | 19 +++---------------- .../hardware/command/handlers/TrayMotorHandler.java | 15 ++++++++++++++- .../drivers/DODriver/OutputIOCtrlDriver.java | 10 ++++++++++ .../hardware/drivers/StepMotorDriver/HBotDriver.java | 20 ++++++++++++++++++++ .../drivers/StepMotorDriver/ShakeMotorDriver.java | 8 +++++++- .../drivers/StepMotorDriver/TrayMotorDriver.java | 20 ++++++++++++++++++++ 7 files changed, 86 insertions(+), 18 deletions(-) diff --git a/src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java b/src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java index 221bedd..c068587 100644 --- a/src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java +++ b/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 supportCmdDeviceIOOutputMap = Map.ofEntries( + Map.entry(CmdDevice.gantry_z, OutputIOMId.DO_HBOTZ_MOTOR_CLAMP) + ); + private final Map 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(); diff --git a/src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java b/src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java index 8dfdebf..bf15d17 100644 --- a/src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java +++ b/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); } } } diff --git a/src/main/java/com/iflytop/gd/hardware/command/handlers/TrayMotorHandler.java b/src/main/java/com/iflytop/gd/hardware/command/handlers/TrayMotorHandler.java index 91226a5..ce14c99 100644 --- a/src/main/java/com/iflytop/gd/hardware/command/handlers/TrayMotorHandler.java +++ b/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 supportCmdDeviceIOOutputMap = Map.ofEntries( + Map.entry(CmdDevice.tray_motor, OutputIOMId.DO_TRAY_MOTOR_CLAMP) + ); + private final Map 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(); diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/DODriver/OutputIOCtrlDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/DODriver/OutputIOCtrlDriver.java index 99dbc7e..73812f2 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/DODriver/OutputIOCtrlDriver.java +++ b/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); + } } diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java index 4208e4c..fd09382 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java +++ b/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); + } } diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java index 727c2bd..f9ded73 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java +++ b/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); + } } diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java index c026aba..fa46526 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java +++ b/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); + } }