diff --git a/src/main/java/com/iflytop/gd/app/cmd/CapLiftingOriginCommand.java b/src/main/java/com/iflytop/gd/app/cmd/CapLiftingOriginCommand.java index 1c0d7e1..bd77acf 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/CapLiftingOriginCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/CapLiftingOriginCommand.java @@ -3,8 +3,12 @@ package com.iflytop.gd.app.cmd; import com.iflytop.gd.app.core.BaseCommandHandler; import com.iflytop.gd.app.model.dto.CmdDTO; import com.iflytop.gd.app.service.DeviceCommandUtilService; +import com.iflytop.gd.app.service.DeviceStateService; import com.iflytop.gd.app.service.SelfTestService; import com.iflytop.gd.common.annotation.CommandMapping; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.service.IOService; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -21,14 +25,18 @@ import java.util.concurrent.CompletableFuture; public class CapLiftingOriginCommand extends BaseCommandHandler { private final DeviceCommandUtilService deviceCommandUtilService; private final SelfTestService selfTestService; + private final OutputIOCtrlDriver outputIOCtrlDriver; @Override public CompletableFuture handle(CmdDTO cmdDTO) { return runAsync(() -> { + outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP); //拍子升降回原点 deviceCommandUtilService.trayMotorOrigin(cmdDTO.getCommandId(), cmdDTO.getCommand()); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); //将拍子升降是否在原点状态设置为true selfTestService.getSelfTestState().setCapLiftingOrigin(true); + }); } } diff --git a/src/main/java/com/iflytop/gd/app/cmd/DoorOriginCommand.java b/src/main/java/com/iflytop/gd/app/cmd/DoorOriginCommand.java new file mode 100644 index 0000000..96372af --- /dev/null +++ b/src/main/java/com/iflytop/gd/app/cmd/DoorOriginCommand.java @@ -0,0 +1,37 @@ +package com.iflytop.gd.app.cmd; + +import com.iflytop.gd.app.core.BaseCommandHandler; +import com.iflytop.gd.app.model.dto.CmdDTO; +import com.iflytop.gd.app.service.DeviceCommandUtilService; +import com.iflytop.gd.app.service.DevicePositionService; +import com.iflytop.gd.app.service.DeviceStateService; +import com.iflytop.gd.common.annotation.CommandMapping; +import com.iflytop.gd.common.enums.data.DevicePositionCode; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Component; + +import java.util.concurrent.CompletableFuture; + +/** + * 门回原点 + */ +@Slf4j +@Component +@RequiredArgsConstructor +@CommandMapping("door_origin")//业务指令注解 +public class DoorOriginCommand extends BaseCommandHandler { + private final DeviceCommandUtilService deviceCommandUtilService; + private final DeviceStateService deviceStateService; + + @Override + public CompletableFuture handle(CmdDTO cmdDTO) { + return runAsync(() -> { + //门电机回原点 + deviceCommandUtilService.doorOrigin(cmdDTO.getCommandId(), cmdDTO.getCommand()); + //将门状态设置为false + deviceStateService.setDoorStatus(false); + }); + } +} + diff --git a/src/main/java/com/iflytop/gd/app/cmd/DualRobotJointOriginCommand.java b/src/main/java/com/iflytop/gd/app/cmd/DualRobotJointOriginCommand.java index 00bffc8..52c4014 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/DualRobotJointOriginCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/DualRobotJointOriginCommand.java @@ -2,12 +2,10 @@ package com.iflytop.gd.app.cmd; import com.iflytop.gd.app.core.BaseCommandHandler; import com.iflytop.gd.app.model.dto.CmdDTO; -import com.iflytop.gd.app.service.DeviceCommandUtilService; import com.iflytop.gd.app.service.DeviceStateService; import com.iflytop.gd.app.service.SelfTestService; import com.iflytop.gd.common.annotation.CommandMapping; -import com.iflytop.gd.common.enums.cmd.CmdAxis; -import com.iflytop.gd.hardware.type.LiquidDistributionArmDriver; +import com.iflytop.gd.hardware.drivers.LiquidDistributionArmDriver; import com.iflytop.gd.hardware.type.Servo.LiquidArmMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; @@ -31,8 +29,6 @@ public class DualRobotJointOriginCommand extends BaseCommandHandler { public CompletableFuture handle(CmdDTO cmdDTO) { return runAsync(() -> { if (!deviceStateService.getDeviceState().isVirtual()) {//虚拟模式不执行 - //加液臂电机使能 TODO将来干掉 - liquidDistributionArmDriver.liquidDistributionArmEnable(LiquidArmMId.LiquidDistributionArm, 1); //将加液臂移动至0位 liquidDistributionArmDriver.liquidDistributionArmMoveTo(LiquidArmMId.LiquidDistributionArm, 0); } diff --git a/src/main/java/com/iflytop/gd/app/cmd/GantryZOriginCommand.java b/src/main/java/com/iflytop/gd/app/cmd/GantryZOriginCommand.java index 1378435..9acc36e 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/GantryZOriginCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/GantryZOriginCommand.java @@ -5,6 +5,8 @@ import com.iflytop.gd.app.model.dto.CmdDTO; import com.iflytop.gd.app.service.DeviceCommandUtilService; import com.iflytop.gd.app.service.SelfTestService; import com.iflytop.gd.common.annotation.CommandMapping; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -21,11 +23,14 @@ import java.util.concurrent.CompletableFuture; public class GantryZOriginCommand extends BaseCommandHandler { private final DeviceCommandUtilService deviceCommandUtilService; private final SelfTestService selfTestService; + private final OutputIOCtrlDriver outputIOCtrlDriver; @Override public CompletableFuture handle(CmdDTO cmdDTO) { return runAsync(() -> { + outputIOCtrlDriver.open(OutputIOMId.DO_HBOTZ_MOTOR_CLAMP); deviceCommandUtilService.gantryZMoveOrigin(cmdDTO.getCommandId(), cmdDTO.getCommand()); + outputIOCtrlDriver.close(OutputIOMId.DO_HBOTZ_MOTOR_CLAMP); selfTestService.getSelfTestState().setGantryZOrigin(true);//设置是否在原点状态 }); } diff --git a/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java b/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java index ade79e5..810c8af 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/MoveToHeatAreaCommand.java @@ -13,8 +13,6 @@ import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.common.enums.data.DevicePositionCode; import com.iflytop.gd.common.exception.AppException; import com.iflytop.gd.common.result.ResultCode; -import com.iflytop.gd.hardware.service.GDDeviceStatusService; -import com.iflytop.gd.hardware.type.IO.InputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -45,14 +43,14 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId); //校验目标加热位是否有托盘 try{ - deviceStateService.setGantryArmStateIdle(true); + deviceStateService.setGantryArmStateIdle(false); isExecuting.set(true); - Boolean heatModuleTray = deviceCommandUtilService.heatModuleTray(heatModuleId); + Boolean heatModuleTray = deviceCommandUtilService.heatModuleSensorState(heatModuleId); if (!heatModuleTray) { throw new AppException(ResultCode.TARGET_HEAT_MODULE_NO_TRAY); } }finally { - deviceStateService.setGantryArmStateIdle(false); + deviceStateService.setGantryArmStateIdle(true); isExecuting.set(false); } return runAsync(() -> { @@ -100,7 +98,7 @@ public class MoveToHeatAreaCommand extends BaseCommandHandler { deviceCommandUtilService.heaterMotorMove(cmdDTO.getCommandId(), cmdDTO.getCommand(), heatModuleId, trayLower);//下降加热位托盘 deviceStateService.setHeatModuleStateTrayStatus(heatModuleId, 1); } finally { - deviceStateService.setGantryArmStateIdle(false); + deviceStateService.setGantryArmStateIdle(true); isExecuting.set(false); } }); diff --git a/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java b/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java index 211ac5d..677449a 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/MoveToSolutionAreaCommand.java @@ -13,7 +13,6 @@ import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.common.enums.data.DevicePositionCode; import com.iflytop.gd.common.exception.AppException; import com.iflytop.gd.common.result.ResultCode; -import com.iflytop.gd.hardware.exception.HardwareException; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -44,14 +43,14 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler { HeatModuleCode heatModuleId = HeatModuleCode.valueOf(heatId); //校验目标加热位是否有托盘 try{ - deviceStateService.setGantryArmStateIdle(true); + deviceStateService.setGantryArmStateIdle(false); isExecuting.set(true); - Boolean heatModuleTray = deviceCommandUtilService.heatModuleTray(heatModuleId); + Boolean heatModuleTray = deviceCommandUtilService.heatModuleSensorState(heatModuleId); if (heatModuleTray) { throw new AppException(ResultCode.TARGET_HEAT_MODULE_OCCUPIED); } }finally { - deviceStateService.setGantryArmStateIdle(false); + deviceStateService.setGantryArmStateIdle(true); isExecuting.set(false); } return runAsync(() -> { @@ -102,7 +101,7 @@ public class MoveToSolutionAreaCommand extends BaseCommandHandler { trayState.setInSolutionModule(true); deviceCommandUtilService.gantryZMoveBy(cmdDTO.getCommandId(), cmdDTO.getCommand(), -clawDescend);//抬升机械臂至托盘上方 }finally { - deviceStateService.setGantryArmStateIdle(false); + deviceStateService.setGantryArmStateIdle(true); isExecuting.set(false); } }); diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorLiftDownCommand.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorLiftDownCommand.java index 3fabbb8..b5fb95e 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorLiftDownCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorLiftDownCommand.java @@ -7,6 +7,8 @@ import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.cmd.CommandFuture; import com.iflytop.gd.common.cmd.DeviceCommandBundle; import com.iflytop.gd.common.cmd.DeviceCommandGenerator; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -22,6 +24,7 @@ import java.util.concurrent.CompletableFuture; @CommandMapping("debug_cover_elevator_lift_down") public class DebugCoverElevatorLiftDownCommand extends BaseCommandHandler { private final DeviceCommandService deviceCommandService; + private final OutputIOCtrlDriver outputIOCtrlDriver; @Override public CompletableFuture handle(CmdDTO cmdDTO) { @@ -34,9 +37,11 @@ public class DebugCoverElevatorLiftDownCommand extends BaseCommandHandler { CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); commandWait(deviceCommandFuture); } + outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP); DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMoveBy(-distance); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); commandWait(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); }); } } diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorLiftUpCommand.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorLiftUpCommand.java index 5812dbe..e50c516 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorLiftUpCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorLiftUpCommand.java @@ -7,6 +7,8 @@ import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.cmd.CommandFuture; import com.iflytop.gd.common.cmd.DeviceCommandBundle; import com.iflytop.gd.common.cmd.DeviceCommandGenerator; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -23,6 +25,7 @@ import java.util.concurrent.CompletableFuture; @CommandMapping("debug_cover_elevator_lift_up") public class DebugCoverElevatorLiftUpCommand extends BaseCommandHandler { private final DeviceCommandService deviceCommandService; + private final OutputIOCtrlDriver outputIOCtrlDriver; @Override public CompletableFuture handle(CmdDTO cmdDTO) { @@ -35,9 +38,11 @@ public class DebugCoverElevatorLiftUpCommand extends BaseCommandHandler { CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); commandWait(deviceCommandFuture); } + outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP); DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMoveBy(distance); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); commandWait(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); }); } } diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorResetCommand.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorResetCommand.java index a8d92f0..e45b771 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorResetCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorResetCommand.java @@ -7,6 +7,8 @@ import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.cmd.CommandFuture; import com.iflytop.gd.common.cmd.DeviceCommandBundle; import com.iflytop.gd.common.cmd.DeviceCommandGenerator; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -22,13 +24,16 @@ import java.util.concurrent.CompletableFuture; @CommandMapping("debug_cover_elevator_reset") public class DebugCoverElevatorResetCommand extends BaseCommandHandler { private final DeviceCommandService deviceCommandService; + private final OutputIOCtrlDriver outputIOCtrlDriver; @Override public CompletableFuture handle(CmdDTO cmdDTO) { return runAsync(() -> { + outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP); DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorOrigin(); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); commandWait(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); }); } } diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorStopCommand.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorStopCommand.java index 310a5bf..1ee5892 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorStopCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugCoverElevatorStopCommand.java @@ -7,6 +7,8 @@ import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.cmd.CommandFuture; import com.iflytop.gd.common.cmd.DeviceCommandBundle; import com.iflytop.gd.common.cmd.DeviceCommandGenerator; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -22,6 +24,7 @@ import java.util.concurrent.CompletableFuture; @CommandMapping("debug_cover_elevator_stop") public class DebugCoverElevatorStopCommand extends BaseCommandHandler { private final DeviceCommandService deviceCommandService; + private final OutputIOCtrlDriver outputIOCtrlDriver; @Override public CompletableFuture handle(CmdDTO cmdDTO) { @@ -29,6 +32,7 @@ public class DebugCoverElevatorStopCommand extends BaseCommandHandler { DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorStop(); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); commandWait(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); }); } } diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugLiquidArmResetCommand.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugLiquidArmResetCommand.java index 942846d..6655f16 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugLiquidArmResetCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugLiquidArmResetCommand.java @@ -3,7 +3,7 @@ package com.iflytop.gd.app.cmd.debug; import com.iflytop.gd.app.core.BaseCommandHandler; import com.iflytop.gd.app.model.dto.CmdDTO; import com.iflytop.gd.common.annotation.CommandMapping; -import com.iflytop.gd.hardware.type.LiquidDistributionArmDriver; +import com.iflytop.gd.hardware.drivers.LiquidDistributionArmDriver; import com.iflytop.gd.hardware.type.Servo.LiquidArmMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmMoveCommand.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmMoveCommand.java index 00c42c8..26b8811 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmMoveCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmMoveCommand.java @@ -7,6 +7,8 @@ import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.cmd.CommandFuture; import com.iflytop.gd.common.cmd.DeviceCommandBundle; import com.iflytop.gd.common.cmd.DeviceCommandGenerator; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -24,6 +26,7 @@ import java.util.concurrent.CompletableFuture; @CommandMapping("debug_transportation_arm_move") public class DebugTransportationArmMoveCommand extends BaseCommandHandler { private final DeviceCommandService deviceCommandService; + private final OutputIOCtrlDriver outputIOCtrlDriver; private boolean stop = false; @Override @@ -51,9 +54,11 @@ public class DebugTransportationArmMoveCommand extends BaseCommandHandler { futuresList.add(deviceCommandFuture); } if (zDimVelocity != null) { + outputIOCtrlDriver.open(OutputIOMId.DO_HBOTZ_MOTOR_CLAMP); DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZSet(zDimVelocity); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); futuresList.add(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_HBOTZ_MOTOR_CLAMP); } commandWait(futuresList.toArray(new CommandFuture[0])); diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmResetCommand.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmResetCommand.java index f86cb5c..af30375 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmResetCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmResetCommand.java @@ -7,6 +7,8 @@ import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.cmd.CommandFuture; import com.iflytop.gd.common.cmd.DeviceCommandBundle; import com.iflytop.gd.common.cmd.DeviceCommandGenerator; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -24,6 +26,7 @@ import java.util.concurrent.CompletableFuture; @CommandMapping("debug_transportation_arm_reset") public class DebugTransportationArmResetCommand extends BaseCommandHandler { private final DeviceCommandService deviceCommandService; + private final OutputIOCtrlDriver outputIOCtrlDriver; @Override public CompletableFuture handle(CmdDTO cmdDTO) { @@ -42,9 +45,11 @@ public class DebugTransportationArmResetCommand extends BaseCommandHandler { } if (dim != null && dim.contains("z")) { + outputIOCtrlDriver.open(OutputIOMId.DO_HBOTZ_MOTOR_CLAMP); DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZOrigin(); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); futuresList.add(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_HBOTZ_MOTOR_CLAMP); } commandWait(futuresList.toArray(new CommandFuture[0])); diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmStopCommand.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmStopCommand.java index 659edfa..5c70542 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmStopCommand.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DebugTransportationArmStopCommand.java @@ -7,6 +7,8 @@ import com.iflytop.gd.common.annotation.CommandMapping; import com.iflytop.gd.common.cmd.CommandFuture; import com.iflytop.gd.common.cmd.DeviceCommandBundle; import com.iflytop.gd.common.cmd.DeviceCommandGenerator; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.stereotype.Component; @@ -25,6 +27,7 @@ import java.util.concurrent.CompletableFuture; public class DebugTransportationArmStopCommand extends BaseCommandHandler { private final DeviceCommandService deviceCommandService; private final DebugTransportationArmMoveCommand debugTransportationArmMoveCommand; + private final OutputIOCtrlDriver outputIOCtrlDriver; @Override public CompletableFuture handle(CmdDTO cmdDTO) { @@ -47,6 +50,7 @@ public class DebugTransportationArmStopCommand extends BaseCommandHandler { DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZStop(); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); futuresList.add(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_HBOTZ_MOTOR_CLAMP); } commandWait(futuresList.toArray(new CommandFuture[0])); diff --git a/src/main/java/com/iflytop/gd/app/controller/StepCommandController.java b/src/main/java/com/iflytop/gd/app/controller/StepCommandController.java new file mode 100644 index 0000000..c4d2fa2 --- /dev/null +++ b/src/main/java/com/iflytop/gd/app/controller/StepCommandController.java @@ -0,0 +1,54 @@ +package com.iflytop.gd.app.controller; + +import com.iflytop.gd.app.model.bo.Point3D; +import com.iflytop.gd.app.service.StepCommandService; +import com.iflytop.gd.common.enums.HeatModuleCode; +import com.iflytop.gd.common.result.Result; +import io.swagger.v3.oas.annotations.Operation; +import io.swagger.v3.oas.annotations.tags.Tag; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.web.bind.annotation.GetMapping; +import org.springframework.web.bind.annotation.PostMapping; +import org.springframework.web.bind.annotation.RequestMapping; +import org.springframework.web.bind.annotation.RestController; + +@Tag(name = "单步调试") +@RestController +@RequestMapping("/api/cmd/step") +@RequiredArgsConstructor +@Slf4j +public class StepCommandController { + private final StepCommandService stepCommandService; + + @Operation(summary = "获取当前龙门架机械臂坐标") + @GetMapping("/gantry-point") + public Result getGantryPoint() throws Exception { + Point3D point3D = stepCommandService.getGantryPoint(); + return Result.success(point3D.getX() + "," + point3D.getY() + "," + point3D.getZ()); + } + + @Operation(summary = "失能所有电机") + @PostMapping("/disability-all") + public Result disabilityAll() throws Exception { + stepCommandService.disabilityAll(); + return Result.success(); + } + + @Operation(summary = "使能所有电机") + @PostMapping("/enable-all") + public Result enableAll() throws Exception { + stepCommandService.enableAll(); + return Result.success(); + } + + + @Operation(summary = "龙门架移至托盘夹取处") + @PostMapping("/gantry-tray") + public Result gantryToTray(HeatModuleCode moduleCode) throws Exception { + stepCommandService.gantryToTray(moduleCode); + return Result.success(); + } + + +} diff --git a/src/main/java/com/iflytop/gd/app/controller/SystemController.java b/src/main/java/com/iflytop/gd/app/controller/SystemController.java index 81b88e9..acd2283 100644 --- a/src/main/java/com/iflytop/gd/app/controller/SystemController.java +++ b/src/main/java/com/iflytop/gd/app/controller/SystemController.java @@ -1,6 +1,7 @@ package com.iflytop.gd.app.controller; import com.iflytop.gd.app.core.device.DeviceState; +import com.iflytop.gd.app.model.bo.Point3D; import com.iflytop.gd.app.model.dto.SetSystemDatetimeDTO; import com.iflytop.gd.app.service.DeviceStateService; import com.iflytop.gd.app.service.SystemConfigService; @@ -12,7 +13,7 @@ import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; import org.springframework.web.bind.annotation.*; -@Tag(name = "系统设置") +@Tag(name = "系统") @RestController @RequestMapping("/api/sys") @RequiredArgsConstructor @@ -29,7 +30,7 @@ public class SystemController { } @Operation(summary = "模拟向硬件写入完毕") - @GetMapping("/initComplete") + @PostMapping("/initComplete") public Result setInitComplete(Boolean initComplete) { deviceStateService.setInitComplete(initComplete); return Result.success(); @@ -48,5 +49,4 @@ public class SystemController { return Result.success(); } - } diff --git a/src/main/java/com/iflytop/gd/app/core/device/SelfTestState.java b/src/main/java/com/iflytop/gd/app/core/device/SelfTestState.java index 714d668..d7e9c5a 100644 --- a/src/main/java/com/iflytop/gd/app/core/device/SelfTestState.java +++ b/src/main/java/com/iflytop/gd/app/core/device/SelfTestState.java @@ -6,6 +6,9 @@ import lombok.Data; @Schema(description = "自检状态") @Data public class SelfTestState { + @Schema(description = "门是否在原点") + private boolean doorOrigin = false; + @Schema(description = "龙门架机械臂x轴是否在原点") private boolean gantryXOrigin = false; diff --git a/src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java b/src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java index b483dde..ad5a7bf 100644 --- a/src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java +++ b/src/main/java/com/iflytop/gd/app/service/DeviceCommandUtilService.java @@ -10,11 +10,13 @@ import com.iflytop.gd.common.enums.AcidPumpDeviceCode; import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.common.enums.cmd.CmdAxis; import com.iflytop.gd.common.enums.data.DevicePositionCode; +import com.iflytop.gd.hardware.drivers.DODriver.OutputIOCtrlDriver; import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorCtrlDriver; import com.iflytop.gd.hardware.exception.HardwareException; import com.iflytop.gd.hardware.service.GDDeviceStatusService; import com.iflytop.gd.hardware.type.IO.InputIOMId; -import com.iflytop.gd.hardware.type.LiquidDistributionArmDriver; +import com.iflytop.gd.hardware.type.IO.OutputIOMId; +import com.iflytop.gd.hardware.drivers.LiquidDistributionArmDriver; import com.iflytop.gd.hardware.type.Servo.LiquidArmMId; import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId; import lombok.RequiredArgsConstructor; @@ -32,6 +34,7 @@ public class DeviceCommandUtilService { private final LiquidDistributionArmDriver liquidDistributionArmDriver; private final GDDeviceStatusService gdDeviceStatusService; private final StepMotorCtrlDriver stepMotorCtrlDriver; + private final OutputIOCtrlDriver outputIOCtrlDriver; /** * 门电机回原点 */ @@ -75,6 +78,7 @@ public class DeviceCommandUtilService { * 龙门架机械臂移动到指定点 */ public void gantryMove(String cmdId, String cmdCode, Point3D point) throws Exception { + outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP); DeviceCommandBundle gantryXMoveDeviceCommand = DeviceCommandGenerator.gantryXMove(point.getX()); CommandFuture gantryXMoveDeviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, gantryXMoveDeviceCommand); @@ -84,6 +88,7 @@ public class DeviceCommandUtilService { DeviceCommandBundle gantryZMoveDeviceCommand = DeviceCommandGenerator.gantryZMove(point.getZ()); CommandFuture gantryZMoveDeviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, gantryZMoveDeviceCommand); commandWait(gantryXMoveDeviceCommandFuture, gantryYMoveDeviceCommandFuture, gantryZMoveDeviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); } /** @@ -183,9 +188,11 @@ public class DeviceCommandUtilService { * 龙门架机械臂Z轴回原点 */ public void gantryZMoveOrigin(String cmdId, String cmdCode) throws Exception { + outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP); DeviceCommandBundle deviceCommand = DeviceCommandGenerator.gantryZOrigin(); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand); commandWait(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); } @@ -442,9 +449,11 @@ public class DeviceCommandUtilService { } double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance(); double capLiftingHeightSum = (6 - capNum) * capLiftingHeight; + outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP); DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(capLiftingHeightSum); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand); commandWait(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); } /** @@ -453,10 +462,12 @@ public class DeviceCommandUtilService { * @param num 1为上升1格，-1为下降1格 */ public void capMotorMove(String cmdId, String cmdCode, int num) throws Exception { + outputIOCtrlDriver.open(OutputIOMId.DO_TRAY_MOTOR_CLAMP); double capLiftingHeight = devicePositionService.getPosition(DevicePositionCode.capLiftingHeight).getDistance(); DeviceCommandBundle deviceCommand = DeviceCommandGenerator.trayMotorMove(num * capLiftingHeight); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdId, cmdCode, deviceCommand); commandWait(deviceCommandFuture); + outputIOCtrlDriver.close(OutputIOMId.DO_TRAY_MOTOR_CLAMP); } @@ -760,16 +771,16 @@ public class DeviceCommandUtilService { } /** - * 获取指定加热模块是否有托盘 + * 获取指定加热模块托盘检测传感器数值 */ - public Boolean heatModuleTray(HeatModuleCode heatModuleCode) throws HardwareException { + public Boolean heatModuleSensorState(HeatModuleCode heatModuleCode) throws HardwareException { return switch (heatModuleCode) { - case heat_module_01 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE1_EXSIT); - case heat_module_02 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE2_EXSIT); - case heat_module_03 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE3_EXSIT); - case heat_module_04 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE4_EXSIT); - case heat_module_05 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE5_EXSIT); - case heat_module_06 -> gdDeviceStatusService.getInputState(InputIOMId.Heater_TUBE6_EXSIT); + case heat_module_01 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE01_EXIST); + case heat_module_02 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE02_EXIST); + case heat_module_03 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE03_EXIST); + case heat_module_04 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE04_EXIST); + case heat_module_05 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE05_EXIST); + case heat_module_06 -> gdDeviceStatusService.getInputState(InputIOMId.HEAT_MODULE06_EXIST); }; } diff --git a/src/main/java/com/iflytop/gd/app/service/DeviceInitService.java b/src/main/java/com/iflytop/gd/app/service/DeviceInitService.java index ec8b452..3bc390f 100644 --- a/src/main/java/com/iflytop/gd/app/service/DeviceInitService.java +++ b/src/main/java/com/iflytop/gd/app/service/DeviceInitService.java @@ -9,9 +9,19 @@ import com.iflytop.gd.common.enums.ContainerCode; import com.iflytop.gd.common.enums.ContainerType; import com.iflytop.gd.common.enums.HeatModuleCode; import com.iflytop.gd.hardware.comm.can.A8kCanBusService; -import com.iflytop.gd.hardware.exception.HardwareException; +import com.iflytop.gd.hardware.constants.ActionOvertimeConstant; +import com.iflytop.gd.hardware.drivers.LeisaiServoDriver; +import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoDriver; +import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorCtrlDriver; +import com.iflytop.gd.hardware.drivers.TricolorLightDriver; +import com.iflytop.gd.hardware.type.CmdId; +import com.iflytop.gd.hardware.drivers.LiquidDistributionArmDriver; import com.iflytop.gd.hardware.type.MId; import com.iflytop.gd.hardware.type.RegIndex; +import com.iflytop.gd.hardware.type.Servo.LeisaiServoMId; +import com.iflytop.gd.hardware.type.Servo.LiquidArmMId; +import com.iflytop.gd.hardware.type.Servo.MiniServoMId; +import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId; import com.opencsv.CSVReader; import jakarta.annotation.PostConstruct; import lombok.RequiredArgsConstructor; @@ -19,7 +29,6 @@ import lombok.extern.slf4j.Slf4j; import org.springframework.core.io.ClassPathResource; import org.springframework.stereotype.Service; -import java.io.IOException; import java.io.InputStreamReader; import java.util.List; @@ -30,19 +39,73 @@ public class DeviceInitService { private final ContainerService containerService; private final DeviceStateService deviceStateService; private final A8kCanBusService canBusService; + private final StepMotorCtrlDriver stepMotorCtrlDriver; + private final MiniServoDriver miniServoDriver; + private final LiquidDistributionArmDriver liquidDistributionArmDriver; + private final TricolorLightDriver tricolorLightDriver; + private final LeisaiServoDriver leisaiServoDriver; + private final ActionOvertimeConstant actionOvertimeConstant; @PostConstruct public void init() { new Thread(() -> { try { + tricolorLightDriver.open(MId.TriColorLight, TricolorLightDriver.Color.BLUE); initDeviceState(); initDeviceSetData(); + initOvertime(); + initEnable(); + tricolorLightDriver.open(MId.TriColorLight, TricolorLightDriver.Color.GREEN); deviceStateService.setInitComplete(true); - } catch (Exception ignored) { + } catch (Exception e) { + try { + tricolorLightDriver.open(MId.TriColorLight, TricolorLightDriver.Color.RED); + } catch (Exception ignored) { + } + log.error("设备初始化失败", e); } }).start(); } + /** + * 设置各动作超时时间 + */ + public void initOvertime(){ + actionOvertimeConstant.pushNewConfig(StepMotorMId.DOOR_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 60 * 1000); + actionOvertimeConstant.pushNewConfig(LeisaiServoMId.MainXSV, CmdId.step_motor_easy_move_to_zero, 30 * 1000); + actionOvertimeConstant.pushNewConfig(LeisaiServoMId.MainYSV, CmdId.step_motor_easy_move_to_zero, 30 * 1000); + actionOvertimeConstant.pushNewConfig(StepMotorMId.HBOT_Z_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 30 * 1000); + actionOvertimeConstant.pushNewConfig(StepMotorMId.HEATER_1_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 15 * 1000); + actionOvertimeConstant.pushNewConfig(StepMotorMId.HEATER_2_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 15 * 1000); + actionOvertimeConstant.pushNewConfig(StepMotorMId.HEATER_3_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 15 * 1000); + actionOvertimeConstant.pushNewConfig(StepMotorMId.HEATER_4_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 15 * 1000); + actionOvertimeConstant.pushNewConfig(StepMotorMId.HEATER_5_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 15 * 1000); + actionOvertimeConstant.pushNewConfig(StepMotorMId.HEATER_6_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 15 * 1000); + actionOvertimeConstant.pushNewConfig(StepMotorMId.TRAY_MOTOR_MID, CmdId.step_motor_easy_move_to_zero, 120 * 1000); + actionOvertimeConstant.pushNewConfig(LiquidArmMId.LiquidDistributionArm, CmdId.liquid_distribution_arm_move_to, 3 * 1000); + } + + /** + * 初始化所有设备使能 + */ + public void initEnable() throws Exception { + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.DOOR_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.SHAKE_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.TRAY_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HBOT_Z_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_1_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_2_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_3_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_4_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_5_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_6_MOTOR_MID, 1); + + liquidDistributionArmDriver.liquidDistributionArmEnable(LiquidArmMId.LiquidDistributionArm, 1); + miniServoDriver.miniServoForceEnable(MiniServoMId.CLAW_MID, 1); + leisaiServoDriver.enable(LeisaiServoMId.MainXSV, 1); + leisaiServoDriver.enable(LeisaiServoMId.MainYSV, 1); + } + public void initDeviceSetData() throws Exception { if (deviceStateService.getDeviceState().isVirtual() || deviceStateService.getDeviceState().isInitComplete()) { return; @@ -67,18 +130,14 @@ public class DeviceInitService { } } } catch (Exception e) { - log.error("设备初始化失败", e); + log.error("设备初始化写入参数失败，2秒后重试", e); // 如果发生错误，可以递归调用重试（如果需要） initDeviceSetData(); } } - public void sendToDevice(DeviceInitializationData data){ - try{ - canBusService.moduleSetReg(MId.valueOf(data.getMid()), RegIndex.valueOf(data.getRegIndex()), data.getRegInitVal()); - }catch (Exception e) { - log.error("设备初始化写入失败", e); - } + public void sendToDevice(DeviceInitializationData data) throws Exception { + canBusService.moduleSetReg(MId.valueOf(data.getMid()), RegIndex.valueOf(data.getRegIndex()), data.getRegInitVal()); } public void initDeviceState() { diff --git a/src/main/java/com/iflytop/gd/app/service/DeviceStateService.java b/src/main/java/com/iflytop/gd/app/service/DeviceStateService.java index d5a3f27..6fde15f 100644 --- a/src/main/java/com/iflytop/gd/app/service/DeviceStateService.java +++ b/src/main/java/com/iflytop/gd/app/service/DeviceStateService.java @@ -20,6 +20,8 @@ public class DeviceStateService { private final DeviceState deviceState = new DeviceState(); + + public synchronized DeviceState getDeviceState() { return deviceState; } @@ -28,6 +30,7 @@ public class DeviceStateService { support.addPropertyChangeListener(deviceStateListener); } + public synchronized HeatModuleState getHeatModuleState(HeatModuleCode moduleCode) { HeatModuleState heatModuleState = null; for (HeatModuleState t : deviceState.getHeatModule()) { diff --git a/src/main/java/com/iflytop/gd/app/service/StepCommandService.java b/src/main/java/com/iflytop/gd/app/service/StepCommandService.java new file mode 100644 index 0000000..184d3e5 --- /dev/null +++ b/src/main/java/com/iflytop/gd/app/service/StepCommandService.java @@ -0,0 +1,81 @@ +package com.iflytop.gd.app.service; + +import com.iflytop.gd.app.model.bo.Point3D; +import com.iflytop.gd.common.enums.HeatModuleCode; +import com.iflytop.gd.hardware.drivers.LeisaiServoDriver; +import com.iflytop.gd.hardware.drivers.MiniServoDriver.MiniServoDriver; +import com.iflytop.gd.hardware.drivers.StepMotorDriver.StepMotorCtrlDriver; +import com.iflytop.gd.hardware.service.GDDeviceStatusService; +import com.iflytop.gd.hardware.drivers.LiquidDistributionArmDriver; +import com.iflytop.gd.hardware.type.Servo.LeisaiServoMId; +import com.iflytop.gd.hardware.type.Servo.LiquidArmMId; +import com.iflytop.gd.hardware.type.Servo.MiniServoMId; +import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId; +import lombok.RequiredArgsConstructor; +import org.springframework.stereotype.Service; + +/** + * 单步运动调试服务 + */ +@Service +@RequiredArgsConstructor +public class StepCommandService { + private final DeviceCommandUtilService deviceCommandUtilService; + private final StepMotorCtrlDriver stepMotorCtrlDriver; + private final MiniServoDriver miniServoDriver; + private final LiquidDistributionArmDriver liquidDistributionArmDriver; + private final LeisaiServoDriver leisaiServoDriver; + private final GDDeviceStatusService gdDeviceStatusService; + + public void gantryToTray(HeatModuleCode moduleCode) throws Exception { + Point3D heatAreaTrayClawPoint3D = deviceCommandUtilService.getHeatAreaTrayClawPoint3D(moduleCode);//获取指定托盘上方点位 + deviceCommandUtilService.gantryMove(heatAreaTrayClawPoint3D); + } + + /** + * 获取当前龙门架机械臂所在点位 + */ + public Point3D getGantryPoint() throws Exception { + double x = gdDeviceStatusService.getXYServoPosition(LeisaiServoMId.MainXSV); + double y = gdDeviceStatusService.getXYServoPosition(LeisaiServoMId.MainYSV); + double z = gdDeviceStatusService.getStepMotorPostion(StepMotorMId.HBOT_X_MOTOR_MID); + return new Point3D(x, y, z); + } + + public void enableAll() throws Exception { + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.DOOR_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.SHAKE_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.TRAY_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HBOT_Z_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_1_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_2_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_3_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_4_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_5_MOTOR_MID, 1); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_6_MOTOR_MID, 1); + + liquidDistributionArmDriver.liquidDistributionArmEnable(LiquidArmMId.LiquidDistributionArm, 1); + miniServoDriver.miniServoForceEnable(MiniServoMId.CLAW_MID, 1); + leisaiServoDriver.enable(LeisaiServoMId.MainXSV, 1); + leisaiServoDriver.enable(LeisaiServoMId.MainYSV, 1); + } + + + public void disabilityAll() throws Exception { + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.DOOR_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.SHAKE_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.TRAY_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HBOT_Z_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_1_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_2_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_3_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_4_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_5_MOTOR_MID, 0); + stepMotorCtrlDriver.stepMotorEnable(StepMotorMId.HEATER_6_MOTOR_MID, 0); + + liquidDistributionArmDriver.liquidDistributionArmEnable(LiquidArmMId.LiquidDistributionArm, 1); + miniServoDriver.miniServoForceEnable(MiniServoMId.CLAW_MID, 1); + leisaiServoDriver.enable(LeisaiServoMId.MainXSV, 1); + leisaiServoDriver.enable(LeisaiServoMId.MainYSV, 1); + } +} diff --git a/src/main/java/com/iflytop/gd/hardware/constants/ActionOvertimeConstant.java b/src/main/java/com/iflytop/gd/hardware/constants/ActionOvertimeConstant.java index 6b2a005..fac5cc7 100644 --- a/src/main/java/com/iflytop/gd/hardware/constants/ActionOvertimeConstant.java +++ b/src/main/java/com/iflytop/gd/hardware/constants/ActionOvertimeConstant.java @@ -3,6 +3,8 @@ 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.type.Servo.LeisaiServoMId; +import com.iflytop.gd.hardware.type.Servo.LiquidArmMId; import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId; import jakarta.annotation.PostConstruct; import org.springframework.stereotype.Component; @@ -12,7 +14,7 @@ import java.util.List; @Component public class ActionOvertimeConstant { - final Integer defaultOvertime = 60 * 1000; + final Integer defaultOvertime = 80 * 1000; static class OvertimeConfigItem { public MId mid; @@ -22,11 +24,6 @@ public class ActionOvertimeConstant { List overtimeConfigItems = new ArrayList<>(); - @PostConstruct - void init() { -// pushNewConfig(StepMotorMId.IncubatorRotateCtrlM, CmdId.step_motor_easy_move_to_zero, 20 * 1000); - } - public Integer get(MId mid, CmdId cmdId) { return priGetOvertime(mid, cmdId); } @@ -35,8 +32,13 @@ public class ActionOvertimeConstant { return priGetOvertime(mid.mid, cmdId); } - - private void pushNewConfig(StepMotorMId mid, CmdId cmdId, Integer overtime) { + public void pushNewConfig(LeisaiServoMId mid, CmdId cmdId, Integer overtime) { + pushNewConfig(mid.mid, cmdId, overtime); + } + public void pushNewConfig(LiquidArmMId mid, CmdId cmdId, Integer overtime) { + pushNewConfig(mid.mid, cmdId, overtime); + } + public void pushNewConfig(StepMotorMId mid, CmdId cmdId, Integer overtime) { pushNewConfig(mid.mid, cmdId, overtime); } diff --git a/src/main/java/com/iflytop/gd/hardware/type/LiquidDistributionArmDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/LiquidDistributionArmDriver.java similarity index 88% rename from src/main/java/com/iflytop/gd/hardware/type/LiquidDistributionArmDriver.java rename to src/main/java/com/iflytop/gd/hardware/drivers/LiquidDistributionArmDriver.java index 3fee77e..5d5dd8a 100644 --- a/src/main/java/com/iflytop/gd/hardware/type/LiquidDistributionArmDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/LiquidDistributionArmDriver.java @@ -1,8 +1,9 @@ -package com.iflytop.gd.hardware.type; +package com.iflytop.gd.hardware.drivers; -import com.fasterxml.jackson.databind.node.ObjectNode; 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.CmdId; import com.iflytop.gd.hardware.type.Servo.LiquidArmMId; import com.iflytop.gd.hardware.type.Servo.LiquidArmRegIndex; import lombok.RequiredArgsConstructor; @@ -16,8 +17,8 @@ import org.springframework.stereotype.Component; @Slf4j @RequiredArgsConstructor public class LiquidDistributionArmDriver { + private final ActionOvertimeConstant actionOvertimeConstant; final private A8kCanBusService canBus; - /** * 使能 * @param id @@ -42,9 +43,11 @@ public class LiquidDistributionArmDriver { public void liquidDistributionArmMoveToBlock(LiquidArmMId id, int index) throws HardwareException { liquidDistributionArmMoveTo(id,index); - canBus.waitForMod(id.mid,3000); + waitForMod(id, actionOvertimeConstant.get(id.mid, CmdId.liquid_distribution_arm_move_to)); + } + public void waitForMod(LiquidArmMId id, Integer overtime) throws HardwareException { + canBus.waitForMod(id.mid, overtime); } - public int liquidDistributionReadPos(LiquidArmMId id) throws HardwareException { log.info("liquidDistributionReadPos called with id: {}", id); diff --git a/src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java b/src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java index cee71e2..5e97958 100644 --- a/src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java +++ b/src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java @@ -1,5 +1,6 @@ package com.iflytop.gd.hardware.service; +import com.iflytop.gd.app.model.bo.Point3D; import com.iflytop.gd.hardware.drivers.DIDriver.InputDetectDriver; import com.iflytop.gd.hardware.drivers.HeaterRodDriver; import com.iflytop.gd.hardware.drivers.LeisaiServoDriver; @@ -85,6 +86,7 @@ public class GDDeviceStatusService { */ public Double getXYServoPosition(LeisaiServoMId mid) throws HardwareException { int servoPosition = leisaiServoDriver.readPosition(mid); - return (double)servoPosition; + return StepMotorConverter.toUserPosition(servoPosition); } + } diff --git a/src/main/java/com/iflytop/gd/hardware/type/IO/InputIOMId.java b/src/main/java/com/iflytop/gd/hardware/type/IO/InputIOMId.java index 217173a..1da9e4b 100644 --- a/src/main/java/com/iflytop/gd/hardware/type/IO/InputIOMId.java +++ b/src/main/java/com/iflytop/gd/hardware/type/IO/InputIOMId.java @@ -3,24 +3,19 @@ package com.iflytop.gd.hardware.type.IO; import com.iflytop.gd.hardware.type.MId; public enum InputIOMId { - Heater_CAP1_EXSIT("Heater_CAP1_EXSIT [ 加热模块拍子1到位 ]", MId.IO1_IO, 0,false), - Heater_CAP2_EXSIT("Heater_CAP2_EXSIT [ 加热模块拍子2到位 ]", MId.IO1_IO, 1,false), - Heater_CAP3_EXSIT("Heater_CAP3_EXSIT [ 加热模块拍子3到位 ]", MId.IO1_IO, 2,false), - Heater_CAP4_EXSIT("Heater_CAP4_EXSIT [ 加热模块拍子4到位 ]", MId.IO1_IO, 3,false), - Heater_CAP5_EXSIT("Heater_CAP5_EXSIT [ 加热模块拍子5到位 ]", MId.IO1_IO, 4,false), - Heater_CAP6_EXSIT("Heater_CAP6_EXSIT [ 加热模块拍子6到位 ]", MId.IO1_IO, 5,false), - Heater_TUBE1_EXSIT("Heater_TUBE1_EXSIT [ 加热模块托盘1到位 ]", MId.IO1_IO, 6,false), - Heater_TUBE2_EXSIT("Heater_TUBE2_EXSIT [ 加热模块托盘2到位 ]", MId.IO1_IO, 7,false), - Heater_TUBE3_EXSIT("Heater_TUBE3_EXSIT [ 加热模块托盘3到位 ]", MId.IO1_IO, 8,false), - Heater_TUBE4_EXSIT("Heater_TUBE4_EXSIT [ 加热模块托盘4到位 ]", MId.IO1_IO, 9,false), - Heater_TUBE5_EXSIT("Heater_TUBE5_EXSIT [ 加热模块托盘5到位 ]", MId.IO1_IO, 10,false), - Heater_TUBE6_EXSIT("Heater_TUBE6_EXSIT [ 加热模块托盘6到位 ]", MId.IO1_IO, 11,false), - TRAY_CAP1_EXSIT("TRAY_CAP1_EXSIT [ 拍子存放位拍子1到位 ]", MId.IO1_IO, 12,false), - TRAY_CAP2_EXSIT("TRAY_CAP2_EXSIT [ 拍子存放位拍子2到位 ]", MId.IO1_IO, 13,false), - TRAY_CAP3_EXSIT("TRAY_CAP3_EXSIT [ 拍子存放位拍子3到位 ]", MId.IO1_IO, 14,false), - TRAY_CAP4_EXSIT("TRAY_CAP4_EXSIT [ 拍子存放位拍子4到位 ]", MId.IO1_IO, 15,false), - TRAY_CAP5_EXSIT("TRAY_CAP5_EXSIT [ 拍子存放位拍子5到位 ]", MId.IO1_IO, 16,false), - TRAY_CAP6_EXSIT("TRAY_CAP6_EXSIT [ 拍子存放位拍子6到位 ]", MId.IO1_IO, 17,false), + HEAT_MODULE01_EXIST("HEAT_MODULE01_EXIST [ 加热模1传感器存在 ]", MId.IO1_IO, 0,false), + HEAT_MODULE02_EXIST("HEAT_MODULE02_EXIST [ 加热模2传感器存在 ]", MId.IO1_IO, 1,false), + HEAT_MODULE03_EXIST("HEAT_MODULE03_EXIST [ 加热模3传感器存在 ]", MId.IO1_IO, 2,false), + HEAT_MODULE04_EXIST("HEAT_MODULE04_EXIST [ 加热模4传感器存在 ]", MId.IO1_IO, 3,false), + HEAT_MODULE05_EXIST("HEAT_MODULE05_EXIST [ 加热模5传感器存在 ]", MId.IO1_IO, 4,false), + HEAT_MODULE06_EXIST("HEAT_MODULE06_EXIST [ 加热模6传感器存在 ]", MId.IO1_IO, 5,false), + CAP01_EXIST("CAP01_EXIST [ 拍子存放位拍子1到位 ]", MId.IO1_IO, 6,false), + CAP02_EXIST("CAP02_EXIST [ 拍子存放位拍子2到位 ]", MId.IO1_IO, 7,false), + CAP03_EXIST("CAP03_EXIST [ 拍子存放位拍子3到位 ]", MId.IO1_IO, 8,false), + CAP04_EXIST("CAP04_EXIST [ 拍子存放位拍子4到位 ]", MId.IO1_IO, 9,false), + CAP05_EXIST("CAP05_EXIST [ 拍子存放位拍子5到位 ]", MId.IO1_IO, 10,false), + CAP06_EXIST("CAP06_EXIST [ 拍子存放位拍子6到位 ]", MId.IO1_IO, 11,false), + E_STOP("E_STOP [ 急停 ]", MId.IO1_IO, 18,false), ;