From 26a04819c15f799548a5d248bdea02005fe3bfcb Mon Sep 17 00:00:00 2001 From: HSZ_HeSongZhen <210202959@qq.com> Date: Mon, 12 May 2025 01:39:58 +0800 Subject: [PATCH] =?UTF-8?q?fixed:=20=E4=BF=AE=E6=94=B9=20HBOt?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../gd/hardware/command/handlers/HBotHandler.java | 67 ++++++++++++++++++++-- .../gd/hardware/type/Servo/LeisaiServoMId.java | 4 +- src/main/resources/application-dev.yml | 2 +- 3 files changed, 64 insertions(+), 9 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 c068587..58cb603 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 @@ -3,12 +3,15 @@ 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.LeisaiServoDriver; import com.iflytop.gd.hardware.drivers.StepMotorDriver.HBotDriver; import com.iflytop.gd.hardware.type.IO.OutputIOMId; +import com.iflytop.gd.hardware.type.Servo.LeisaiServoMId; +import com.iflytop.gd.hardware.type.Servo.LeisaiServoSpeedLevel; import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId; import com.iflytop.gd.hardware.type.MId; +import com.iflytop.gd.hardware.utils.Math.StepMotorConverter; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -22,6 +25,7 @@ import java.util.stream.Collectors; @RequiredArgsConstructor public class HBotHandler extends CommandHandler { private final HBotDriver driver_; + private final LeisaiServoDriver servoDriver_; private final Map supportCmdDeviceIOOutputMap = Map.ofEntries( Map.entry(CmdDevice.gantry_z, OutputIOMId.DO_HBOTZ_MOTOR_CLAMP) @@ -32,6 +36,13 @@ public class HBotHandler extends CommandHandler { Map.entry(CmdDevice.gantry_y, StepMotorMId.HBOT_Y_MOTOR_MID), Map.entry(CmdDevice.gantry_z, StepMotorMId.HBOT_Z_MOTOR_MID) ); + + + + private final Map leisaiServoMIdMap = Map.ofEntries( + Map.entry(CmdDevice.gantry_y, LeisaiServoMId.MainXSV), + Map.entry(CmdDevice.gantry_z, LeisaiServoMId.MainYSV) + ); private final Map supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream(). collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid)); private final Set supportActions = Set.of(CmdAction.set, CmdAction.origin, CmdAction.move, CmdAction.move_by, CmdAction.stop); @@ -51,26 +62,70 @@ public class HBotHandler extends CommandHandler { public void handleCommand(DeviceCommand command) throws Exception { // 发送命令 StepMotorMId stepMotorMId = stepMotorMIdMap_.get(command.getDevice()); + LeisaiServoMId servoMId = leisaiServoMIdMap.get(command.getDevice()); + + + boolean isServo = command.getDevice() != CmdDevice.gantry_z; OutputIOMId clampMId = supportCmdDeviceIOOutputMap.get(command.getDevice()); if (command.getAction() == CmdAction.origin) { - driver_.moveToHome(stepMotorMId); + driver_.openClamp(clampMId); + if(isServo) + { + log.info("HBotHandler origin servo"); + servoDriver_.moveToZero(servoMId); + } + else + { + driver_.moveToHome(stepMotorMId); + + } + driver_.closeClamp(clampMId); + } else if(command.getAction() == CmdAction.stop) { - driver_.stop(stepMotorMId); + driver_.openClamp(clampMId); + if(isServo) + { + log.info("HBotHandler stop servo"); + servoDriver_.moduleStop(servoMId); + } + else + { + driver_.stop(stepMotorMId); + } + driver_.closeClamp(clampMId); } else if(command.getAction() == CmdAction.move) { driver_.openClamp(clampMId); - double position = command.getParam().getPosition(); - driver_.moveTo(stepMotorMId, position); + if(isServo) + { + double userPos = command.getParam().getPosition(); + int motor_Pos = StepMotorConverter.toMotorPosition(userPos); + log.info("HBotHandler move to userPos: {}, motor_Pos: {}", userPos, motor_Pos); + servoDriver_.moveTo(servoMId, LeisaiServoSpeedLevel.DEFAULT, motor_Pos); + } + else + { + driver_.moveTo(stepMotorMId, command.getParam().getPosition()); + } driver_.closeClamp(clampMId); } else if(command.getAction() == CmdAction.move_by) { driver_.openClamp(clampMId); double distance = command.getParam().getPosition(); - driver_.moveBy(stepMotorMId, distance); + if(isServo) + { + int motor_Pos = StepMotorConverter.toMotorPosition(distance); + log.info("HBotHandler move by distance: {}, motor_Pos: {}", distance, motor_Pos); + servoDriver_.moveBy(servoMId, LeisaiServoSpeedLevel.DEFAULT, motor_Pos); + } + else + { + driver_.moveBy(stepMotorMId, distance); + } driver_.closeClamp(clampMId); } else if(command.getAction() == CmdAction.set) { diff --git a/src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiServoMId.java b/src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiServoMId.java index 0d7cc95..03a6d06 100644 --- a/src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiServoMId.java +++ b/src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiServoMId.java @@ -4,8 +4,8 @@ import com.iflytop.gd.hardware.type.MId; import org.springframework.util.Assert; public enum LeisaiServoMId { - HBotXLeisaiSV(MId.MainXSV), - HBotYLeisaiSV(MId.MainYSV), + MainXSV(MId.MainXSV), + MainYSV(MId.MainYSV), ; final public MId mid; diff --git a/src/main/resources/application-dev.yml b/src/main/resources/application-dev.yml index 902eb3d..748e630 100644 --- a/src/main/resources/application-dev.yml +++ b/src/main/resources/application-dev.yml @@ -36,7 +36,7 @@ command_bus: device.enableCanBus: true iflytophald: - ip: 192.168.1.140 + ip: 192.168.8.168 cmdch.port: 19004 datach.port: 19005