From 7acd5377fa070f9b8c8bfc76e2a371fb762cb957 Mon Sep 17 00:00:00 2001 From: HSZ_HeSongZhen <210202959@qq.com> Date: Sun, 11 May 2025 18:04:20 +0800 Subject: [PATCH] =?UTF-8?q?add:=E5=A2=9E=E5=8A=A0=E9=9B=B7=E8=B5=9B?= =?UTF-8?q?=E5=92=8C=E5=8A=A0=E6=B6=B2=E8=87=82=E7=9A=84=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=E7=95=8C=E9=9D=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../app/config/A8kSubModuleInitRegConfig.java | 37 ++++++ .../app/hardware/driver/LeisaiServoDriver.java | 59 ++++++++++ .../driver/LiquidDistributionArmDriver.java | 69 +++++++++++ src/main/java/iflytop/app/hardware/type/CmdId.java | 14 +++ .../iflytop/app/hardware/type/LeisaiRegIndex.java | 12 ++ .../iflytop/app/hardware/type/LeisaiServoMId.java | 16 +++ .../app/hardware/type/LeisaiServoSpeedLevel.java | 9 ++ .../iflytop/app/hardware/type/LiquidArmMId.java | 15 +++ .../app/hardware/type/LiquidArmRegIndex.java | 45 ++++++++ src/main/java/iflytop/app/hardware/type/MId.java | 6 +- .../java/iflytop/app/hardware/type/ModuleType.java | 3 + .../java/iflytop/app/hardware/type/RegIndex.java | 42 ++++++- .../iflytop/app/menu/ExtApiPageGroupCfgMgr.java | 9 +- .../java/iflytop/app/page/LeisaiServoPage.java | 127 +++++++++++++++++++++ src/main/java/iflytop/app/page/LiquidArmPage.java | 99 ++++++++++++++++ .../java/iflytop/app/page/MiniServoCtrlPage.java | 4 - src/main/resources/application.yml | 2 +- 17 files changed, 554 insertions(+), 14 deletions(-) create mode 100644 src/main/java/iflytop/app/hardware/driver/LeisaiServoDriver.java create mode 100644 src/main/java/iflytop/app/hardware/driver/LiquidDistributionArmDriver.java create mode 100644 src/main/java/iflytop/app/hardware/type/LeisaiRegIndex.java create mode 100644 src/main/java/iflytop/app/hardware/type/LeisaiServoMId.java create mode 100644 src/main/java/iflytop/app/hardware/type/LeisaiServoSpeedLevel.java create mode 100644 src/main/java/iflytop/app/hardware/type/LiquidArmMId.java create mode 100644 src/main/java/iflytop/app/hardware/type/LiquidArmRegIndex.java create mode 100644 src/main/java/iflytop/app/page/LeisaiServoPage.java create mode 100644 src/main/java/iflytop/app/page/LiquidArmPage.java diff --git a/src/main/java/iflytop/app/config/A8kSubModuleInitRegConfig.java b/src/main/java/iflytop/app/config/A8kSubModuleInitRegConfig.java index 1b8b359..7352c68 100644 --- a/src/main/java/iflytop/app/config/A8kSubModuleInitRegConfig.java +++ b/src/main/java/iflytop/app/config/A8kSubModuleInitRegConfig.java @@ -103,6 +103,42 @@ public class A8kSubModuleInitRegConfig { addRegMarker(ModuleType.MiniServo, RegIndex.kreg_mini_servo_servo_overload_torque); // 过载扭矩 addRegMarker(ModuleType.MiniServo, RegIndex.kreg_mini_servo_servo_acc); // 加速度 + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_enable); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos0_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos0_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos1_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos1_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos2_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos2_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos3_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos3_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos4_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos4_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos5_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos5_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos6_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos6_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos7_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos7_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos8_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos8_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos9_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos9_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos10_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos10_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos11_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos11_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos12_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos12_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos13_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos13_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos14_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos14_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos15_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos15_d1); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos16_d0); + addRegMarker(ModuleType.LiquidDistributionArm, RegIndex.kreg_liquid_distribution_arm_pos16_d1); + } private void addRegMarker(ModuleType moduleTYpe, RegIndex regIndex) { @@ -110,3 +146,4 @@ public class A8kSubModuleInitRegConfig { } } + diff --git a/src/main/java/iflytop/app/hardware/driver/LeisaiServoDriver.java b/src/main/java/iflytop/app/hardware/driver/LeisaiServoDriver.java new file mode 100644 index 0000000..31d6156 --- /dev/null +++ b/src/main/java/iflytop/app/hardware/driver/LeisaiServoDriver.java @@ -0,0 +1,59 @@ +package iflytop.app.hardware.driver; + +import iflytop.app.hardware.channel.A8kCanBusConnection; +import iflytop.app.hardware.channel.A8kCanBusService; +import iflytop.app.hardware.type.*; +import iflytop.app.type.exception.HardwareException; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Component; + +@Component +@Slf4j +@RequiredArgsConstructor +public class LeisaiServoDriver { + final private A8kCanBusService canBus; + static public final Integer actionOvertime = 10000; + + public void moduleStop(LeisaiServoMId id) throws HardwareException { + log.info("moduleStop called with id: {}", id); + canBus.moduleStop(id.mid); + } + + public void enable(LeisaiServoMId id, Integer enable) throws HardwareException { + log.info("enable called with id: {}, enable: {}", id, enable); + canBus.callcmd(id.mid, CmdId.leisai_servo_enable, enable); + } + + public Integer readPosition(LeisaiServoMId id) throws HardwareException { + log.info("readPosition called with id: {}", id); + A8kPacket packet = canBus.callcmd(id.mid, CmdId.leisai_servo_read_pos); + return packet.getContentI32(0); + } + + public Integer 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); + } + + public void moveTo(LeisaiServoMId id, LeisaiServoSpeedLevel speedLevel, Integer pos) throws HardwareException { + log.info("moveTo called with id: {}, speedLevel: {}, pos: {}", id, speedLevel, pos); + canBus.callcmd(id.mid, CmdId.leisai_servo_move_to, speedLevel.ordinal(), pos); + } + + public void moveBy(LeisaiServoMId id, LeisaiServoSpeedLevel speedLevel, Integer dpos) throws HardwareException { + log.info("moveBy called with id: {}, speedLevel: {}, dpos: {}", id, speedLevel, dpos); + canBus.callcmd(id.mid, CmdId.leisai_servo_move_by, speedLevel.ordinal(), dpos); + } + + public void rotate(LeisaiServoMId id, LeisaiServoSpeedLevel speedLevel, Integer direction) throws HardwareException { + log.info("rotate called with id: {}, speedLevel: {}, direction: {}", id, speedLevel, direction); + canBus.callcmd(id.mid, CmdId.leisai_servo_rotate, speedLevel.ordinal(), direction); + } + + public void moveToZero(LeisaiServoMId id) throws HardwareException { + log.info("moveToZero called with id: {}", id); + canBus.callcmd(id.mid, CmdId.leisai_servo_move_to_zero); + } +} diff --git a/src/main/java/iflytop/app/hardware/driver/LiquidDistributionArmDriver.java b/src/main/java/iflytop/app/hardware/driver/LiquidDistributionArmDriver.java new file mode 100644 index 0000000..3fffbce --- /dev/null +++ b/src/main/java/iflytop/app/hardware/driver/LiquidDistributionArmDriver.java @@ -0,0 +1,69 @@ +package iflytop.app.hardware.driver; + +import com.fasterxml.jackson.databind.node.ObjectNode; +import iflytop.app.hardware.channel.A8kCanBusService; +import iflytop.app.hardware.type.*; +import iflytop.app.type.exception.HardwareException; +import iflytop.app.utils.ZJsonHelper; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Component; + +@Component +@Slf4j +@RequiredArgsConstructor +public class LiquidDistributionArmDriver { + final private A8kCanBusService canBus; + + public void liquidDistributionArmEnable(LiquidArmMId id, int enable) throws HardwareException { + log.info("liquidDistributionArmEnable called with id: {}, enable: {}", id, enable); + canBus.callcmd(id.mid, CmdId.liquid_distribution_arm_enable, enable); + } + + public void liquidDistributionArmMoveTo(LiquidArmMId id, int index) throws HardwareException { + log.info("liquidDistributionArmMoveTo called with id: {}, index: {}", id, index); + canBus.callcmd(id.mid, CmdId.liquid_distribution_arm_move_to, index); + } + + public int liquidDistributionReadPos(LiquidArmMId id) throws HardwareException { + log.info("liquidDistributionReadPos called with id: {}", id); + var packet = canBus.callcmd(id.mid, CmdId.liquid_distribution_arm_read_pos); + return packet.getContentI32(0); + } + + public void liquidDistributionSetCurPosAsPresetPos(LiquidArmMId id, int index) throws HardwareException { + log.info("liquidDistributionSetCurPosAsPresetPos called with id: {}, index: {}", id, index); + canBus.callcmd(id.mid, CmdId.liquid_distribution_arm_set_cur_pos_as_preset_pos, index); + } + + public void liquidDistributionSetCurPosAsMidPos(LiquidArmMId id) throws HardwareException { + log.info("liquidDistributionSetCurPosAsMidPos called with id: {}", id); + canBus.callcmd(id.mid, CmdId.liquid_distribution_arm_set_cur_pos_as_mid_pos); + } + + public void moduleStop(LiquidArmMId id) throws HardwareException { + log.info("moduleStop called with id: {}", id); + canBus.moduleStop(id.mid); + } + + public void setReg(LiquidArmMId id, LiquidArmRegIndex 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(LiquidArmMId id, LiquidArmRegIndex regindex) throws HardwareException { + log.info("getReg called with id: {}, regindex: {}", id, regindex); + return canBus.moduleGetReg(id.mid, regindex.regIndex); + } + + public Object getAllReg(LiquidArmMId id) throws HardwareException { + log.info("getAllReg called with id: {}", id); + ObjectNode node = ZJsonHelper.createObjectNode(); + for (LiquidArmRegIndex regIndex : LiquidArmRegIndex.values()) { + Integer regVal = getReg(id, regIndex); + log.info("read reg {} -> {}", regIndex, regVal); + node.put(regIndex.name(), getReg(id, regIndex)); + } + return node; + } +} diff --git a/src/main/java/iflytop/app/hardware/type/CmdId.java b/src/main/java/iflytop/app/hardware/type/CmdId.java index 655f0b6..f291d67 100644 --- a/src/main/java/iflytop/app/hardware/type/CmdId.java +++ b/src/main/java/iflytop/app/hardware/type/CmdId.java @@ -65,6 +65,20 @@ public enum CmdId { mini_servo_rotate_with_torque(0x360b, "MINI_SERVO_ROTATE_WITH_TORQUE"), mini_servo_set_cur_pos(0x360c, "MINI_SERVO_SET_CUR_POS"), + leisai_servo_enable(0x3701, "LEISAI_SERVO_ENABLE"),// + leisai_servo_read_pos(0x3702, "LEISAI_SERVO_READ_POS"),// + leisai_servo_read_io_state(0x3703, "LEISAI_SERVO_READ_IO_STATE"),// + leisai_servo_move_by(0x3704, "LEISAI_SERVO_MOVE_BY"), // (dpos,speedlevel)->null speedlevel=0,1,2,3(default,low,mid,high) + leisai_servo_move_to(0x3705, "LEISAI_SERVO_MOVE_TO"), // (pos,speedlevel)->null speedlevel=0,1,2,3(default,low,mid,high) + leisai_servo_rotate(0x3706, "LEISAI_SERVO_ROTATE"), // (direction,speedlevel)-> null + leisai_servo_move_to_zero(0x3707, "LEISAI_SERVO_MOVE_TO_ZERO"), // (null)-> null + + liquid_distribution_arm_enable(0x3801, "LIQUID_DISTRIBUTION_ARM_ENABLE"),// + liquid_distribution_arm_move_to(0x3802, "LIQUID_DISTRIBUTION_ARM_MOVE_TO"), // (pos)->null pos=(-1,0,1...16) -1代表未知未知，0代表待机位置,1...16分别对应16加酸孔位 + liquid_distribution_arm_read_pos(0x3803, "LIQUID_DISTRIBUTION_ARM_READ_POS"), // (null)->pos pos=(-1,0,1...16) -1代表未知未知，0代表待机位置,1...16分别对应16加酸孔位 + liquid_distribution_arm_set_cur_pos_as_mid_pos(0x3804, "LIQUID_DISTRIBUTION_ARM_SET_CUR_POS_AS_MID_POS"), // (null)->null 校准舵机 + liquid_distribution_arm_set_cur_pos_as_preset_pos(0x3805, "LIQUID_DISTRIBUTION_ARM_SET_CUR_POS_AS_PRESET_POS"), // 设置当前位置作为预设位置(posindex)->null pos=(0,1...16) 0代表未知未知，1...16分别对应16加酸孔位 + read_in_io(0x4001, "READ_IN_IO"), write_out_io(0x4002, "WRITE_OUT_IO"), read_muti_in_io(0x4003, "READ_MUTI_IN_IO"), diff --git a/src/main/java/iflytop/app/hardware/type/LeisaiRegIndex.java b/src/main/java/iflytop/app/hardware/type/LeisaiRegIndex.java new file mode 100644 index 0000000..836895c --- /dev/null +++ b/src/main/java/iflytop/app/hardware/type/LeisaiRegIndex.java @@ -0,0 +1,12 @@ +package iflytop.app.hardware.type; + +public enum LeisaiRegIndex { + kreg_mini_servo_firmware_main_version(RegIndex.kreg_mini_servo_firmware_main_version), + ; + + public final RegIndex regIndex; + + LeisaiRegIndex(RegIndex regIndex) { + this.regIndex = regIndex; + } +} diff --git a/src/main/java/iflytop/app/hardware/type/LeisaiServoMId.java b/src/main/java/iflytop/app/hardware/type/LeisaiServoMId.java new file mode 100644 index 0000000..3ada35c --- /dev/null +++ b/src/main/java/iflytop/app/hardware/type/LeisaiServoMId.java @@ -0,0 +1,16 @@ +package iflytop.app.hardware.type; + +import org.springframework.util.Assert; + +public enum LeisaiServoMId { + HBotXLeisaiSV(MId.HBotXLeisaiSV), + HBotYLeisaiSV(MId.HBotYLeisaiSV), + ; + + final public MId mid; + + LeisaiServoMId(MId mid) { + Assert.isTrue(this.name().equals(mid.name()), "LeisaiServoMId Init fail"); + this.mid = mid; + } +} diff --git a/src/main/java/iflytop/app/hardware/type/LeisaiServoSpeedLevel.java b/src/main/java/iflytop/app/hardware/type/LeisaiServoSpeedLevel.java new file mode 100644 index 0000000..43c5a78 --- /dev/null +++ b/src/main/java/iflytop/app/hardware/type/LeisaiServoSpeedLevel.java @@ -0,0 +1,9 @@ +package iflytop.app.hardware.type; + +public enum LeisaiServoSpeedLevel { + DEFAULT, + LOW, + MID, + HIGH, + ; +} diff --git a/src/main/java/iflytop/app/hardware/type/LiquidArmMId.java b/src/main/java/iflytop/app/hardware/type/LiquidArmMId.java new file mode 100644 index 0000000..1e07bff --- /dev/null +++ b/src/main/java/iflytop/app/hardware/type/LiquidArmMId.java @@ -0,0 +1,15 @@ +package iflytop.app.hardware.type; + +import org.springframework.util.Assert; + +public enum LiquidArmMId { + LiquidDistributionArm(MId.LiquidDistributionArm),// + ; + + final public MId mid; + + LiquidArmMId(MId mid) { + Assert.isTrue(this.name().equals(mid.name()), "LiquidDistributionArm Init fail"); + this.mid = mid; + } +} diff --git a/src/main/java/iflytop/app/hardware/type/LiquidArmRegIndex.java b/src/main/java/iflytop/app/hardware/type/LiquidArmRegIndex.java new file mode 100644 index 0000000..b29a3dd --- /dev/null +++ b/src/main/java/iflytop/app/hardware/type/LiquidArmRegIndex.java @@ -0,0 +1,45 @@ +package iflytop.app.hardware.type; + +public enum LiquidArmRegIndex { + kreg_liquid_distribution_arm_enable(RegIndex.kreg_liquid_distribution_arm_enable), // 电机使能 + kreg_liquid_distribution_arm_pos0_d0(RegIndex.kreg_liquid_distribution_arm_pos0_d0), // 位置0 舵机0位置 + kreg_liquid_distribution_arm_pos0_d1(RegIndex.kreg_liquid_distribution_arm_pos0_d1), // 位置0 舵机1位置 + kreg_liquid_distribution_arm_pos1_d0(RegIndex.kreg_liquid_distribution_arm_pos1_d0), // 位置1 舵机0位置 + kreg_liquid_distribution_arm_pos1_d1(RegIndex.kreg_liquid_distribution_arm_pos1_d1), // 位置1 舵机1位置 + kreg_liquid_distribution_arm_pos2_d0(RegIndex.kreg_liquid_distribution_arm_pos2_d0), // 位置2 舵机0位置 + kreg_liquid_distribution_arm_pos2_d1(RegIndex.kreg_liquid_distribution_arm_pos2_d1), // 位置2 舵机1位置 + kreg_liquid_distribution_arm_pos3_d0(RegIndex.kreg_liquid_distribution_arm_pos3_d0), // 位置3 舵机0位置 + kreg_liquid_distribution_arm_pos3_d1(RegIndex.kreg_liquid_distribution_arm_pos3_d1), // 位置3 舵机1位置 + kreg_liquid_distribution_arm_pos4_d0(RegIndex.kreg_liquid_distribution_arm_pos4_d0), // 位置4 舵机0位置 + kreg_liquid_distribution_arm_pos4_d1(RegIndex.kreg_liquid_distribution_arm_pos4_d1), // 位置4 舵机1位置 + kreg_liquid_distribution_arm_pos5_d0(RegIndex.kreg_liquid_distribution_arm_pos5_d0), // 位置5 舵机0位置 + kreg_liquid_distribution_arm_pos5_d1(RegIndex.kreg_liquid_distribution_arm_pos5_d1), // 位置5 舵机1位置 + kreg_liquid_distribution_arm_pos6_d0(RegIndex.kreg_liquid_distribution_arm_pos6_d0), // 位置6 舵机0位置 + kreg_liquid_distribution_arm_pos6_d1(RegIndex.kreg_liquid_distribution_arm_pos6_d1), // 位置6 舵机1位置 + kreg_liquid_distribution_arm_pos7_d0(RegIndex.kreg_liquid_distribution_arm_pos7_d0), // 位置7 舵机0位置 + kreg_liquid_distribution_arm_pos7_d1(RegIndex.kreg_liquid_distribution_arm_pos7_d1), // 位置7 舵机1位置 + kreg_liquid_distribution_arm_pos8_d0(RegIndex.kreg_liquid_distribution_arm_pos8_d0), // 位置8 舵机0位置 + kreg_liquid_distribution_arm_pos8_d1(RegIndex.kreg_liquid_distribution_arm_pos8_d1), // 位置8 舵机1位置 + kreg_liquid_distribution_arm_pos9_d0(RegIndex.kreg_liquid_distribution_arm_pos9_d0), // 位置9 舵机0位置 + kreg_liquid_distribution_arm_pos9_d1(RegIndex.kreg_liquid_distribution_arm_pos9_d1), // 位置9 舵机1位置 + kreg_liquid_distribution_arm_pos10_d0(RegIndex.kreg_liquid_distribution_arm_pos10_d0), // 位置10 舵机0位置 + kreg_liquid_distribution_arm_pos10_d1(RegIndex.kreg_liquid_distribution_arm_pos10_d1), // 位置10 舵机1位置 + kreg_liquid_distribution_arm_pos11_d0(RegIndex.kreg_liquid_distribution_arm_pos11_d0), // 位置11 舵机0位置 + kreg_liquid_distribution_arm_pos11_d1(RegIndex.kreg_liquid_distribution_arm_pos11_d1), // 位置11 舵机1位置 + kreg_liquid_distribution_arm_pos12_d0(RegIndex.kreg_liquid_distribution_arm_pos12_d0), // 位置12 舵机0位置 + kreg_liquid_distribution_arm_pos12_d1(RegIndex.kreg_liquid_distribution_arm_pos12_d1), // 位置12 舵机1位置 + kreg_liquid_distribution_arm_pos13_d0(RegIndex.kreg_liquid_distribution_arm_pos13_d0), // 位置13 舵机0位置 + kreg_liquid_distribution_arm_pos13_d1(RegIndex.kreg_liquid_distribution_arm_pos13_d1), // 位置13 舵机1位置 + kreg_liquid_distribution_arm_pos14_d0(RegIndex.kreg_liquid_distribution_arm_pos14_d0), // 位置14 舵机0位置 + kreg_liquid_distribution_arm_pos14_d1(RegIndex.kreg_liquid_distribution_arm_pos14_d1), // 位置14 舵机1位置 + kreg_liquid_distribution_arm_pos15_d0(RegIndex.kreg_liquid_distribution_arm_pos15_d0), // 位置15 舵机0位置 + kreg_liquid_distribution_arm_pos15_d1(RegIndex.kreg_liquid_distribution_arm_pos15_d1), // 位置15 舵机1位置 + kreg_liquid_distribution_arm_pos16_d0(RegIndex.kreg_liquid_distribution_arm_pos16_d0), // 位置16 舵机0位置 + kreg_liquid_distribution_arm_pos16_d1(RegIndex.kreg_liquid_distribution_arm_pos16_d1); // 位置16 舵机1位置 + + public final RegIndex regIndex; + + LiquidArmRegIndex(RegIndex regIndex) { + this.regIndex = regIndex; + } +} diff --git a/src/main/java/iflytop/app/hardware/type/MId.java b/src/main/java/iflytop/app/hardware/type/MId.java index 91c19dd..2b109ea 100644 --- a/src/main/java/iflytop/app/hardware/type/MId.java +++ b/src/main/java/iflytop/app/hardware/type/MId.java @@ -11,8 +11,11 @@ public enum MId { DualRobotAxis2SV(10, "双轴机械臂2舵机"), IO1_IO(11, "台面 IO 板模块"), IO1_ADC(12, "台面 ADC 板模块"), + LiquidDistributionArm(13, "加液臂"), + HBotXLeisaiSV(14, "X轴Leisai舵机"),// + HBotYLeisaiSV(15, "Y轴Leisai舵机"),// - IO2Board(25, "台下 IO 板模块"), +// IO2Board(25, "台下 IO 板模块"), DoorBoard(40, "门电机板模块"),// DoorM(41, "门电机"), @@ -60,7 +63,6 @@ public enum MId { AcidPump8Board(145, "加酸泵8板模块"),// AcidPump8M(146, "加酸泵8电机"), ; - ; final public String name; final public int index; diff --git a/src/main/java/iflytop/app/hardware/type/ModuleType.java b/src/main/java/iflytop/app/hardware/type/ModuleType.java index 3d3abc8..bc5a356 100644 --- a/src/main/java/iflytop/app/hardware/type/ModuleType.java +++ b/src/main/java/iflytop/app/hardware/type/ModuleType.java @@ -12,6 +12,8 @@ public enum ModuleType { TriColorLightCtrl(101), // PwmLightCtrl(102), // ADC(103), // ADC扩展 + LeiSaiServo(104), // 雷赛伺服 + LiquidDistributionArm(105), // 液体分配机械臂控制 UNKNOWN(999); // 未知类型 public final int code; @@ -31,3 +33,4 @@ public enum ModuleType { } + diff --git a/src/main/java/iflytop/app/hardware/type/RegIndex.java b/src/main/java/iflytop/app/hardware/type/RegIndex.java index 3dbef4b..c8d9c30 100644 --- a/src/main/java/iflytop/app/hardware/type/RegIndex.java +++ b/src/main/java/iflytop/app/hardware/type/RegIndex.java @@ -63,7 +63,7 @@ public enum RegIndex { kreg_mini_servo_is_move(10254), // 是否在运动 kreg_mini_servo_status(10255), // 舵机状态 kreg_mini_servo_voltage(10256), // 电压 反馈当前舵机工作电压,反馈精度为0.1V,即120*0.1=12V - kreg_mini_servo_current(10257), // 电流 反馈当前工作电流值,单位为ma + kreg_mini_servo_current(10257), // 电流 反馈当前工作电流���,单位为ma kreg_mini_servo_temperature(10258), // 温度 反馈当前舵机内部工作温度,反馈精度为1摄氏度 kreg_mini_servo_loadvalue(10259), // 负载值 输出驱动电机的当前占空比电压,单位为0.1%,取值0-1000 kreg_mini_servo_target_pos_tolerance(10260), // 目标位置容忍度 @@ -109,7 +109,45 @@ public enum RegIndex { kreg_mini_servo_servo_move_flag(10566), // 移动标志 kreg_mini_servo_servo_current_current(10569), // 当前电流 反馈当前工作电流值,单位为6.26.5mA,最大可反馈电流为500*6.5mA=3250mA - ; + /*********************************************************************************************************************** + * liquid_distribution_arm * + ***********************************************************************************************************************/ + + kreg_liquid_distribution_arm_enable(10600), // 电机使能 + kreg_liquid_distribution_arm_pos0_d0(10610), // 位置0 舵机0位置 + kreg_liquid_distribution_arm_pos0_d1(10611), // 位置0 舵机1位置 + kreg_liquid_distribution_arm_pos1_d0(10612), // 位置1 舵机0位置 + kreg_liquid_distribution_arm_pos1_d1(10613), // 位置1 舵机1位置 + kreg_liquid_distribution_arm_pos2_d0(10614), // 位置2 舵机0位置 + kreg_liquid_distribution_arm_pos2_d1(10615), // 位置2 舵机1位置 + kreg_liquid_distribution_arm_pos3_d0(10616), // 位置3 舵机0位置 + kreg_liquid_distribution_arm_pos3_d1(10617), // 位置3 舵机1位置 + kreg_liquid_distribution_arm_pos4_d0(10618), // 位置4 舵机0位置 + kreg_liquid_distribution_arm_pos4_d1(10619), // 位置4 舵机1位置 + kreg_liquid_distribution_arm_pos5_d0(10620), // 位置5 舵机0位置 + kreg_liquid_distribution_arm_pos5_d1(10621), // 位置5 舵机1位置 + kreg_liquid_distribution_arm_pos6_d0(10622), // 位置6 舵机0位置 + kreg_liquid_distribution_arm_pos6_d1(10623), // 位置6 舵机1位置 + kreg_liquid_distribution_arm_pos7_d0(10624), // 位置7 舵机0位置 + kreg_liquid_distribution_arm_pos7_d1(10625), // 位置7 舵机1位置 + kreg_liquid_distribution_arm_pos8_d0(10626), // 位置8 舵机0位置 + kreg_liquid_distribution_arm_pos8_d1(10627), // 位置8 舵机1位置 + kreg_liquid_distribution_arm_pos9_d0(10628), // 位置9 舵机0位置 + kreg_liquid_distribution_arm_pos9_d1(10629), // 位置9 舵机1位置 + kreg_liquid_distribution_arm_pos10_d0(10630), // 位置10 舵机0位置 + kreg_liquid_distribution_arm_pos10_d1(10631), // 位置10 舵机1位置 + kreg_liquid_distribution_arm_pos11_d0(10632), // 位置11 舵机0位置 + kreg_liquid_distribution_arm_pos11_d1(10633), // 位置11 舵机1位置 + kreg_liquid_distribution_arm_pos12_d0(10634), // 位置12 舵机0位置 + kreg_liquid_distribution_arm_pos12_d1(10635), // 位置12 舵机1位置 + kreg_liquid_distribution_arm_pos13_d0(10636), // 位置13 舵机0位置 + kreg_liquid_distribution_arm_pos13_d1(10637), // 位置13 舵机1位置 + kreg_liquid_distribution_arm_pos14_d0(10638), // 位置14 舵机0位置 + kreg_liquid_distribution_arm_pos14_d1(10639), // 位置14 舵机1位置 + 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位置 public final int index; public final Boolean trace; diff --git a/src/main/java/iflytop/app/menu/ExtApiPageGroupCfgMgr.java b/src/main/java/iflytop/app/menu/ExtApiPageGroupCfgMgr.java index 38ebb20..ab359dd 100644 --- a/src/main/java/iflytop/app/menu/ExtApiPageGroupCfgMgr.java +++ b/src/main/java/iflytop/app/menu/ExtApiPageGroupCfgMgr.java @@ -1,9 +1,6 @@ package iflytop.app.menu; -import iflytop.app.page.A8kSubModuleParameterInitDebugPage; -import iflytop.app.page.DeviceInitPage; -import iflytop.app.page.MiniServoCtrlPage; -import iflytop.app.page.MotorCtrlPage; +import iflytop.app.page.*; import iflytop.app.utils.ZList; import jakarta.annotation.PostConstruct; import org.springframework.stereotype.Component; @@ -70,7 +67,9 @@ public class ExtApiPageGroupCfgMgr { ))); pushMenu(new Menu("驱动", ZList.of( new Menu(MiniServoCtrlPage.class, "舵机驱动"), - new Menu(MotorCtrlPage.class, "电机驱动") + new Menu(MotorCtrlPage.class, "电机驱动"), + new Menu(LiquidArmPage.class, "加液臂驱动"), + new Menu(LeisaiServoPage.class, "雷赛伺服电机驱动") ))); diff --git a/src/main/java/iflytop/app/page/LeisaiServoPage.java b/src/main/java/iflytop/app/page/LeisaiServoPage.java new file mode 100644 index 0000000..884af15 --- /dev/null +++ b/src/main/java/iflytop/app/page/LeisaiServoPage.java @@ -0,0 +1,127 @@ +package iflytop.app.page; + +import iflytop.app.hardware.driver.LeisaiServoDriver; +import iflytop.app.hardware.type.LeisaiServoMId; +import iflytop.app.hardware.type.LeisaiServoSpeedLevel; +import iflytop.app.type.exception.HardwareException; +import iflytop.app.utils.ZJsonNode; +import iflytop.extui.mgr.ExtApiPageMgr; +import jakarta.annotation.PostConstruct; +import jakarta.annotation.Resource; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Component; + +@Component +@Slf4j +@RequiredArgsConstructor +public class LeisaiServoPage { + private final LeisaiServoDriver driver; + + @Resource + private ExtApiPageMgr extApiPageMgr; + + private LeisaiServoMId id = LeisaiServoMId.HBotXLeisaiSV; + private Integer dpos = 10; + private LeisaiServoSpeedLevel speedLevel = LeisaiServoSpeedLevel.DEFAULT; + + public void setLeisaiServoId(LeisaiServoMId id) { + log.info("setLeisaiServoId called with id: {}", id); + this.id = id; + } + + public void setLeisaiServoDpos(Integer dpos) { + log.info("setLeisaiServoDpos called with dpos: {}", dpos); + this.dpos = dpos; + } + + public void setLeisaiServoSpeedLevel(LeisaiServoSpeedLevel speedLevel) { + log.info("setLeisaiServoSpeedLevel called with speedLevel: {}", speedLevel); + this.speedLevel = speedLevel; + } + + public void enableServo() throws Exception { + log.info("enableServo called"); + driver.enable(id, 1); + } + + public void disableServo() throws Exception { + log.info("disableServo called"); + driver.enable(id, 0); + } + + public Integer readPosition() throws Exception { + log.info("readPosition called"); + return driver.readPosition(id); + } + + public void moveTo(Integer pos) throws Exception { + log.info("moveTo called with pos: {}", pos); + driver.moveTo(id, speedLevel, pos); + } + + public void moveByForward() throws Exception { + log.info("moveByForward called with dpos: {}", dpos); + driver.moveBy(id, speedLevel, dpos); + } + + public void moveByBackward() throws Exception { + log.info("moveByBackward called with dpos: {}", dpos); + driver.moveBy(id, speedLevel, -dpos); + } + + public void rotateForward() throws Exception { + log.info("rotateForward called"); + driver.rotate(id, speedLevel, 1); + } + + public void rotateBackward() throws Exception { + log.info("rotateBackward called"); + driver.rotate(id, speedLevel, -1); + } + + public void moveToZero() throws Exception { + log.info("moveToZero called"); + driver.moveToZero(id); + } + + public Object readIOState() throws HardwareException { + log.info("readIOState called"); + ZJsonNode node = new ZJsonNode(); + node.get("IO1").set(driver.readIoState(id, 0)); + node.get("IO2").set(driver.readIoState(id, 1)); + return node.get(); + } + + @PostConstruct + void init() { + var page = extApiPageMgr.newPage(this); + + page.newGroup("上下文"); + page.addFunction("设置伺服ID", this::setLeisaiServoId).setParamVal("id", () -> id); + page.addFunction("设置速度等级", this::setLeisaiServoSpeedLevel).setParamVal("speedLevel", () -> speedLevel); + + page.newGroup("基础操作"); + page.addFunction("使能伺服", this::enableServo); + page.addFunction("失能伺服", this::disableServo); + page.addFunction("归零", this::moveToZero); + + page.newGroup("相对移动"); + page.addFunction("设置相对移动距离", this::setLeisaiServoDpos).setParamVal("dpos", () -> dpos); + page.addFunction("相对正向移动", this::moveByForward); + page.addFunction("相对反向移动", this::moveByBackward); + + page.newGroup("绝对移动"); + page.addFunction("绝对移动", this::moveTo); + + page.newGroup("旋转"); + page.addFunction("顺时针旋转", this::rotateForward); + page.addFunction("逆时针旋转", this::rotateBackward); + + page.newGroup("状态"); + page.addFunction("读取位置", this::readPosition); + page.addFunction("读取IO", this::readIOState); + + extApiPageMgr.addPage(page); + } +} diff --git a/src/main/java/iflytop/app/page/LiquidArmPage.java b/src/main/java/iflytop/app/page/LiquidArmPage.java new file mode 100644 index 0000000..33527ee --- /dev/null +++ b/src/main/java/iflytop/app/page/LiquidArmPage.java @@ -0,0 +1,99 @@ +package iflytop.app.page; + +import com.fasterxml.jackson.databind.JsonNode; +import iflytop.app.hardware.driver.LiquidDistributionArmDriver; +import iflytop.app.hardware.type.LiquidArmMId; +import iflytop.app.hardware.type.LiquidArmRegIndex; +import iflytop.app.type.exception.HardwareException; +import iflytop.app.utils.ZJsonNode; +import iflytop.extui.mgr.ExtApiPageMgr; +import jakarta.annotation.PostConstruct; +import lombok.RequiredArgsConstructor; +import lombok.extern.slf4j.Slf4j; +import org.springframework.stereotype.Component; + +@Component +@Slf4j +@RequiredArgsConstructor +public class LiquidArmPage { + private final LiquidDistributionArmDriver liquidDistributionArmCtrlDriver; + private final ExtApiPageMgr extApiPageMgr; + + LiquidArmMId id = LiquidArmMId.LiquidDistributionArm; + + public void setLiquidArmId(LiquidArmMId id) { + log.info("setLiquidArmId called with id: {}", id); + this.id = id; + } + + public void enable(Boolean enable) throws HardwareException { + log.info("enable called with enable: {}", enable); + liquidDistributionArmCtrlDriver.liquidDistributionArmEnable(id, enable ? 1 : 0); + } + + public void stop() throws HardwareException { + log.info("stop called"); + liquidDistributionArmCtrlDriver.moduleStop(id); + } + + public void moveTo(Integer index) throws HardwareException { + log.info("moveTo called with index: {}", index); + liquidDistributionArmCtrlDriver.liquidDistributionArmMoveTo(id, index); + } + + public void setCurPosAsPresetPos(Integer index) throws HardwareException { + log.info("setCurPosAsPresetPos called with index: {}", index); + liquidDistributionArmCtrlDriver.liquidDistributionSetCurPosAsPresetPos(id, index); + } + + public void setCurPosAsMidPos() throws HardwareException { + log.info("setCurPosAsMidPos called"); + liquidDistributionArmCtrlDriver.liquidDistributionSetCurPosAsMidPos(id); + } + + public Integer readPosIndex() throws HardwareException { + log.info("readPosIndex called"); + return liquidDistributionArmCtrlDriver.liquidDistributionReadPos(id); + } + + public void setReg(LiquidArmRegIndex reg, Integer val) throws HardwareException { + log.info("setReg called with reg: {}, val: {}", reg, val); + liquidDistributionArmCtrlDriver.setReg(id, reg, val); + } + + public JsonNode getAllReg() { + log.info("getAllReg called"); + ZJsonNode jsonNode = new ZJsonNode(); + for (LiquidArmRegIndex reg : LiquidArmRegIndex.values()) { + try { + jsonNode.get(reg.name()).set(liquidDistributionArmCtrlDriver.getReg(id, reg)); + } catch (HardwareException e) { + log.error("Error reading register {}: {}", reg, e.getMessage()); + jsonNode.get(reg.name()).set("error"); + } + } + return jsonNode.get(); + } + + @PostConstruct + void init() + { + var page = extApiPageMgr.newPage(this); + page.newGroup("上下文"); + page.addFunction("设置加液臂ID", this::setLiquidArmId).setParamVal("id", () -> id); +// + page.newGroup("基础操作"); + page.addFunction("使能", this::enable).setParamVal("enable", () -> true); + page.addFunction("读取当前索引", this::readPosIndex); + page.addFunction("设置当前位置为中点", this::setCurPosAsMidPos); + page.addFunction("移动到索引N位置", this::moveTo).setParamVal("index", () -> 0); + page.addFunction("设置当前位置为索引N位置", this::setCurPosAsPresetPos).setParamVal("index", () -> 0); + page.addFunction("停止", this::stop); + + page.newGroup("寄存器"); + page.addFunction("设置寄存器", this::setReg); + page.addFunction("获取寄存器", this::getAllReg); + + extApiPageMgr.addPage(page); + } +} diff --git a/src/main/java/iflytop/app/page/MiniServoCtrlPage.java b/src/main/java/iflytop/app/page/MiniServoCtrlPage.java index 32574a4..3d20912 100644 --- a/src/main/java/iflytop/app/page/MiniServoCtrlPage.java +++ b/src/main/java/iflytop/app/page/MiniServoCtrlPage.java @@ -78,10 +78,6 @@ public class MiniServoCtrlPage { return miniServoDriver.miniServoReadPos(id); } - - - - @PostConstruct void init() { var page = extApiPageMgr.newPage(this); diff --git a/src/main/resources/application.yml b/src/main/resources/application.yml index 700e558..2e95725 100644 --- a/src/main/resources/application.yml +++ b/src/main/resources/application.yml @@ -4,7 +4,7 @@ server: device.enableCanBus: true iflytophald: - ip: 192.168.8.168 + ip: 192.168.1.140 cmdch.port: 19004 datach.port: 19005