From c599391495aed4b50ea2dfd8c6d7cf638683afba Mon Sep 17 00:00:00 2001 From: HSZ_HeSongZhen <210202959@qq.com> Date: Sun, 11 May 2025 22:32:55 +0800 Subject: [PATCH] =?UTF-8?q?add:=E5=A2=9E=E5=8A=A0=E7=8A=B6=E6=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../drivers/DIDriver/InputDetectDriver.java | 82 +++++++++----------- .../gd/hardware/drivers/LeisaiServoDriver.java | 16 +++- .../gd/hardware/service/GDDeviceStatusService.java | 90 ++++++++++++++++++++++ .../iflytop/gd/hardware/type/IO/InputIOMId.java | 39 ++++++++++ .../java/com/iflytop/gd/hardware/type/MId.java | 10 +-- .../com/iflytop/gd/hardware/type/RegIndex.java | 11 ++- .../gd/hardware/type/Servo/LeisaiRegIndex.java | 7 +- .../gd/hardware/type/Servo/LeisaiServoMId.java | 4 +- .../gd/hardware/type/Servo/MiniServoMId.java | 6 +- src/main/resources/application-dev.yml | 2 +- 10 files changed, 207 insertions(+), 60 deletions(-) create mode 100644 src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java create mode 100644 src/main/java/com/iflytop/gd/hardware/type/IO/InputIOMId.java diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/DIDriver/InputDetectDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/DIDriver/InputDetectDriver.java index f6b4dd5..b7137b9 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/DIDriver/InputDetectDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/DIDriver/InputDetectDriver.java @@ -1,44 +1,38 @@ -//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))); -// } -// } -//} +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.CmdId; +import com.iflytop.gd.hardware.type.IO.InputIOMId; +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 canBus; + + public Boolean getIOState(InputIOMId 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(InputIOMId ioid) throws HardwareException { + return canBus.callcmd(ioid.mid, CmdId.write_out_io, ioid.ioIndex).getContentI32(0) != 0; + } +} diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/LeisaiServoDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/LeisaiServoDriver.java index 2554fa1..b4023c0 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/LeisaiServoDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/LeisaiServoDriver.java @@ -4,6 +4,7 @@ import com.iflytop.gd.hardware.comm.can.A8kCanBusService; import com.iflytop.gd.hardware.exception.HardwareException; import com.iflytop.gd.hardware.type.A8kPacket; import com.iflytop.gd.hardware.type.CmdId; +import com.iflytop.gd.hardware.type.Servo.LeisaiRegIndex; import com.iflytop.gd.hardware.type.Servo.LeisaiServoMId; import com.iflytop.gd.hardware.type.Servo.LeisaiServoSpeedLevel; import lombok.RequiredArgsConstructor; @@ -33,10 +34,10 @@ public class LeisaiServoDriver { return packet.getContentI32(0); } - public Integer readIoState(LeisaiServoMId id, Integer io_index) throws HardwareException { + public Boolean readIoState(LeisaiServoMId id, Integer io_index) throws HardwareException { log.info("readIoState called with id: {}, io_index: {}", id, io_index); A8kPacket packet = canBus.callcmd(id.mid, CmdId.leisai_servo_read_io_state, io_index); - return packet.getContentI32(0); + return packet.getContentI32(0) != 0; } public void moveTo(LeisaiServoMId id, LeisaiServoSpeedLevel speedLevel, Integer pos) throws HardwareException { @@ -58,4 +59,15 @@ public class LeisaiServoDriver { log.info("moveToZero called with id: {}", id); canBus.callcmd(id.mid, CmdId.leisai_servo_move_to_zero); } + + public void setReg(LeisaiServoMId id, LeisaiRegIndex regindex, int val) throws HardwareException { + log.info("setReg called with id: {}, regindex: {}, val: {}", id, regindex, val); + canBus.moduleSetReg(id.mid, regindex.regIndex, val); + } + + + public Integer getReg(LeisaiServoMId id, LeisaiRegIndex regindex) throws HardwareException { + log.info("getReg called with id: {}, regindex: {}", id, regindex); + return canBus.moduleGetReg(id.mid, regindex.regIndex); + } } diff --git a/src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java b/src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java new file mode 100644 index 0000000..cee71e2 --- /dev/null +++ b/src/main/java/com/iflytop/gd/hardware/service/GDDeviceStatusService.java @@ -0,0 +1,90 @@ +package com.iflytop.gd.hardware.service; + +import com.iflytop.gd.hardware.drivers.DIDriver.InputDetectDriver; +import com.iflytop.gd.hardware.drivers.HeaterRodDriver; +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.exception.HardwareException; +import com.iflytop.gd.hardware.type.IO.InputIOMId; +import com.iflytop.gd.hardware.type.Servo.LeisaiServoMId; +import com.iflytop.gd.hardware.type.StepMotor.StepMotorMId; +import com.iflytop.gd.hardware.type.driver.HeaterRodSlavedId; +import com.iflytop.gd.hardware.utils.Math.StepMotorConverter; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Component; + +@Slf4j +@Component +@RequiredArgsConstructor +public class GDDeviceStatusService { + private final InputDetectDriver inputDetectDriver; + private final HeaterRodDriver heaterRodDriver; + private final StepMotorCtrlDriver stepMotorCtrlDriver; + private final MiniServoDriver miniServoDriver; + private final LeisaiServoDriver leisaiServoDriver; + + /** + * 获取步进电机的原点状态 + * @param stepMotorMId + * @return true 在原点 false 不在原点 + */ + public Boolean stepMotorIsOrigin(StepMotorMId stepMotorMId) throws HardwareException { + return stepMotorCtrlDriver.stepMotorReadIoState(stepMotorMId, 0); + } + + /** + * 获取XY 伺服的原点状态 + * @param mid + * @return + * @throws HardwareException + */ + public Boolean getXYServoIsOrigin(LeisaiServoMId mid) throws HardwareException + { + return leisaiServoDriver.readIoState(mid, 0); + } + + /** + * 获取 拍子 试管架拍子 急停 到位信号 + * @param inputIOMId + * @return + * @throws HardwareException + */ + public Boolean getInputState(InputIOMId inputIOMId) throws HardwareException { + return inputDetectDriver.getIOState(inputIOMId); + } + + /** + * 获取加热棒温度 + * @param heaterRodSlavedId + * @return + * @throws Exception + */ + public Double getHeaterRodTemperature(HeaterRodSlavedId heaterRodSlavedId) throws Exception { + return heaterRodDriver.getTemperature(heaterRodSlavedId); + } + + /** + * 获取电机位置 + * @param stepMotorMId + * @return + * @throws HardwareException + */ + public Double getStepMotorPostion(StepMotorMId stepMotorMId) throws HardwareException { + Integer motorPosition = stepMotorCtrlDriver.stepMotorReadPos(stepMotorMId); + Double realPosition = StepMotorConverter.toUserPosition(motorPosition); + return realPosition; + } + + /** + * 获取伺服类电机位置 + * @param mid + * @return + * @throws HardwareException + */ + public Double getXYServoPosition(LeisaiServoMId mid) throws HardwareException { + int servoPosition = leisaiServoDriver.readPosition(mid); + return (double)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 new file mode 100644 index 0000000..65176b6 --- /dev/null +++ b/src/main/java/com/iflytop/gd/hardware/type/IO/InputIOMId.java @@ -0,0 +1,39 @@ +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), + E_STOP("E_STOP [ 急停 ]", MId.IO1_IO, 18,false), + ; + + final public String description; + final public MId mid; + final public int ioIndex; + final public boolean mirror; + + InputIOMId(String description, MId mid, int ioIndex, boolean mirror) { + this.description = description; + this.mid = mid; + this.ioIndex = ioIndex; + this.mirror = mirror; + + } +} diff --git a/src/main/java/com/iflytop/gd/hardware/type/MId.java b/src/main/java/com/iflytop/gd/hardware/type/MId.java index 2309285..1346612 100644 --- a/src/main/java/com/iflytop/gd/hardware/type/MId.java +++ b/src/main/java/com/iflytop/gd/hardware/type/MId.java @@ -8,14 +8,14 @@ public enum MId { IO1Board(5, "台面 IO 板模块"), PWMLight(6, "PWM 灯"), TriColorLight(7, "三色灯"), - HBotClawS(8, "夹爪舵机"),// - DualRobotAxis1S(9, "双轴机械臂1舵机"), - DualRobotAxis2S(10, "双轴机械臂2舵机"), + HBotClawSV(8, "夹爪舵机"),// + DualRobotAxis1SV(9, "双轴机械臂1舵机"), + DualRobotAxis2SV(10, "双轴机械臂2舵机"), IO1_IO(11, "台面 IO 板模块"), IO1_ADC(12, "台面 ADC 板模块"), LiquidDistributionArm(13, "加液臂"), - HBotXLeisaiSV(14, "X轴Leisai舵机"),// - HBotYLeisaiSV(15, "Y轴Leisai舵机"),// + MainXSV(14, "X轴Leisai舵机"),// + MainYSV(15, "Y轴Leisai舵机"),// // IO2Board(25, "台下 IO 板模块"), diff --git a/src/main/java/com/iflytop/gd/hardware/type/RegIndex.java b/src/main/java/com/iflytop/gd/hardware/type/RegIndex.java index 8c8b36c..fecd216 100644 --- a/src/main/java/com/iflytop/gd/hardware/type/RegIndex.java +++ b/src/main/java/com/iflytop/gd/hardware/type/RegIndex.java @@ -145,8 +145,14 @@ public enum RegIndex { kreg_liquid_distribution_arm_pos15_d0(10640), // 位置15 舵机0位置 kreg_liquid_distribution_arm_pos15_d1(10641), // 位置15 舵机1位置 kreg_liquid_distribution_arm_pos16_d0(10642), // 位置16 舵机0位置 - kreg_liquid_distribution_arm_pos16_d1(10643); // 位置16 舵机1位置 - + kreg_liquid_distribution_arm_pos16_d1(10643), // 位置16 舵机1位置 + + kreg_leisai_servo_is_enable(10702), // 是否使能 + kreg_leisai_servo_has_move_zero(10703), // 是否回零 + kreg_leisai_servo_default_velocity(10704), // 默认速度 + kreg_leisai_servo_low_velocity(10705), // 低速 + kreg_leisai_servo_mid_velocity(10706), // 中速 + kreg_leisai_servo_high_velocity(10707); // 高速 ; public final int index; @@ -172,3 +178,4 @@ public enum RegIndex { return null; } } + diff --git a/src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiRegIndex.java b/src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiRegIndex.java index 9562b8b..3cfc3a5 100644 --- a/src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiRegIndex.java +++ b/src/main/java/com/iflytop/gd/hardware/type/Servo/LeisaiRegIndex.java @@ -3,7 +3,12 @@ package com.iflytop.gd.hardware.type.Servo; import com.iflytop.gd.hardware.type.RegIndex; public enum LeisaiRegIndex { - kreg_mini_servo_firmware_main_version(RegIndex.kreg_mini_servo_firmware_main_version), + kreg_leisai_servo_is_enable(RegIndex.kreg_leisai_servo_is_enable), + kreg_leisai_servo_has_move_zero(RegIndex.kreg_leisai_servo_has_move_zero), + kreg_leisai_servo_default_velocity(RegIndex.kreg_leisai_servo_default_velocity), + kreg_leisai_servo_low_velocity(RegIndex.kreg_leisai_servo_low_velocity), + kreg_leisai_servo_mid_velocity(RegIndex.kreg_leisai_servo_mid_velocity), + kreg_leisai_servo_high_velocity(RegIndex.kreg_leisai_servo_high_velocity), ; public final RegIndex regIndex; 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 868a183..0d7cc95 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.HBotXLeisaiSV), - HBotYLeisaiSV(MId.HBotYLeisaiSV), + HBotXLeisaiSV(MId.MainXSV), + HBotYLeisaiSV(MId.MainYSV), ; final public MId mid; diff --git a/src/main/java/com/iflytop/gd/hardware/type/Servo/MiniServoMId.java b/src/main/java/com/iflytop/gd/hardware/type/Servo/MiniServoMId.java index 3983535..3c3c639 100644 --- a/src/main/java/com/iflytop/gd/hardware/type/Servo/MiniServoMId.java +++ b/src/main/java/com/iflytop/gd/hardware/type/Servo/MiniServoMId.java @@ -4,9 +4,9 @@ import com.iflytop.gd.hardware.type.MId; public enum MiniServoMId { NotSet(MId.NotSet),// - DUAL_ROBOT_AXIS1_MID(MId.DualRobotAxis1S),// - DUAL_ROBOT_AXIS2_MID(MId.DualRobotAxis2S), - CLAW_MID(MId.HBotClawS) + DUAL_ROBOT_AXIS1_MID(MId.DualRobotAxis1SV),// + DUAL_ROBOT_AXIS2_MID(MId.DualRobotAxis2SV), + CLAW_MID(MId.HBotClawSV) ; final public MId mid; diff --git a/src/main/resources/application-dev.yml b/src/main/resources/application-dev.yml index 1445f9e..902eb3d 100644 --- a/src/main/resources/application-dev.yml +++ b/src/main/resources/application-dev.yml @@ -48,4 +48,4 @@ iflytophald: #工控机 config modbus: port: ttyS1 - baudrate: 9600 \ No newline at end of file + baudrate: 9600