From f4a3363932c99c8dad2cf14dea7cd1a56207aed2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=99=BD=E5=87=A4=E5=90=89?= Date: Mon, 12 May 2025 05:36:31 +0800 Subject: [PATCH] =?UTF-8?q?=E5=90=8C=E6=AD=A5=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../gd/app/cmd/debug/DoorCloseCommandHandler.java | 4 +- .../iflytop/gd/app/service/DeviceInitService.java | 47 +++++++++++++--------- .../gd/hardware/command/handlers/HBotHandler.java | 12 ++++-- .../command/handlers/ShakeMotorHandler.java | 2 +- .../gd/hardware/config/StepMotorConfig.java | 1 + .../drivers/MiniServoDriver/ClawDriver.java | 3 +- .../init/zapp_sub_module_reg_initial_value.csv | 20 ++++----- 7 files changed, 52 insertions(+), 37 deletions(-) diff --git a/src/main/java/com/iflytop/gd/app/cmd/debug/DoorCloseCommandHandler.java b/src/main/java/com/iflytop/gd/app/cmd/debug/DoorCloseCommandHandler.java index 789762d..ff62266 100644 --- a/src/main/java/com/iflytop/gd/app/cmd/debug/DoorCloseCommandHandler.java +++ b/src/main/java/com/iflytop/gd/app/cmd/debug/DoorCloseCommandHandler.java @@ -25,13 +25,11 @@ import java.util.concurrent.CompletableFuture; @CommandMapping("debug_door_close") public class DoorCloseCommandHandler extends BaseCommandHandler { private final DeviceCommandService deviceCommandService; - private final DevicePositionService devicePositionService; @Override public CompletableFuture handle(CmdDTO cmdDTO) { return runAsync(() -> { - DevicePosition devicePosition = devicePositionService.getPosition(DevicePositionCode.doorClose); - DeviceCommandBundle deviceCommand = DeviceCommandGenerator.doorMove(devicePosition.getDistance()); + DeviceCommandBundle deviceCommand = DeviceCommandGenerator.doorOrigin(); CommandFuture deviceCommandFuture = deviceCommandService.sendCommand(cmdDTO.getCommandId(), cmdDTO.getCommand(), deviceCommand); commandWait(deviceCommandFuture); }); 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 d69c0e8..ec8b452 100644 --- a/src/main/java/com/iflytop/gd/app/service/DeviceInitService.java +++ b/src/main/java/com/iflytop/gd/app/service/DeviceInitService.java @@ -16,10 +16,11 @@ import com.opencsv.CSVReader; import jakarta.annotation.PostConstruct; import lombok.RequiredArgsConstructor; import lombok.extern.slf4j.Slf4j; +import org.springframework.core.io.ClassPathResource; import org.springframework.stereotype.Service; -import java.io.FileReader; -import java.nio.file.Paths; +import java.io.IOException; +import java.io.InputStreamReader; import java.util.List; @Slf4j @@ -36,41 +37,48 @@ public class DeviceInitService { try { initDeviceState(); initDeviceSetData(); - deviceStateService.setInitComplete(true); } catch (Exception ignored) { } }).start(); } - public void initDeviceSetData() { + public void initDeviceSetData() throws Exception { if (deviceStateService.getDeviceState().isVirtual() || deviceStateService.getDeviceState().isInitComplete()) { return; } try { + // 等待设备状态初始化完成 Thread.sleep(2000); - String filePath = Paths.get("src", "main", "resources", "init", "zapp_sub_module_reg_initial_value.csv").toString(); - CSVReader reader = new CSVReader(new FileReader(filePath)); - reader.readNext(); - String[] line; - while ((line = reader.readNext()) != null) { - DeviceInitializationData data = new DeviceInitializationData(); - data.setId(Integer.parseInt(line[0])); - data.setMid(line[1]); - data.setRegIndex(line[2]); - data.setRegInitVal(Integer.parseInt(line[3])); - sendToDevice(data); + + // 使用 ClassPathResource 来从类路径加载 CSV 文件并使用 InputStream 读取 + ClassPathResource resource = new ClassPathResource("init/zapp_sub_module_reg_initial_value.csv"); + try (CSVReader reader = new CSVReader(new InputStreamReader(resource.getInputStream()))) { + reader.readNext(); // 跳过表头 + + String[] line; + while ((line = reader.readNext()) != null) { + DeviceInitializationData data = new DeviceInitializationData(); + data.setId(Integer.parseInt(line[0])); + data.setMid(line[1]); + data.setRegIndex(line[2]); + data.setRegInitVal(Integer.parseInt(line[3])); + sendToDevice(data); + } } - reader.close(); } catch (Exception e) { log.error("设备初始化失败", e); + // 如果发生错误，可以递归调用重试（如果需要） initDeviceSetData(); } - } - public void sendToDevice(DeviceInitializationData data) throws HardwareException { - canBusService.moduleSetReg(MId.valueOf(data.getMid()), RegIndex.valueOf(data.getRegIndex()), data.getRegInitVal()); + 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 initDeviceState() { @@ -79,6 +87,7 @@ public class DeviceInitService { for (HeatModuleCode code : HeatModuleCode.values()) { heatArea.add(new HeatModuleState(code)); } + List containerList = containerService.getList(); List solutionBucket = deviceState.getSolutionModule().getSolutionContainer(); for (Container container : containerList) { diff --git a/src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java b/src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java index 714ae52..d6c8d2f 100644 --- a/src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java +++ b/src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java @@ -74,7 +74,9 @@ public class HBotHandler extends CommandHandler { if(isServo) { log.info("HBotHandler origin servo"); - servoDriver_.moveToZero(servoMId); + servoDriver_.enable(servoMId, 1); + servoDriver_.moveToZeroBlock(servoMId); + servoDriver_.enable(servoMId, 0); } else { @@ -104,7 +106,9 @@ public class HBotHandler extends CommandHandler { double userPos = command.getParam().getPosition(); int motor_Pos = StepMotorConverter.toMotorPosition(userPos); log.info("HBotHandler move to userPos: {}, motor_Pos: {}", userPos, motor_Pos); - servoDriver_.moveTo(servoMId, LeisaiServoSpeedLevel.DEFAULT, motor_Pos); + servoDriver_.enable(servoMId, 1); + servoDriver_.moveToBlock(servoMId, LeisaiServoSpeedLevel.DEFAULT, motor_Pos); + servoDriver_.enable(servoMId, 0); } else { @@ -120,7 +124,9 @@ public class HBotHandler extends CommandHandler { { int motor_Pos = StepMotorConverter.toMotorPosition(distance); log.info("HBotHandler move by distance: {}, motor_Pos: {}", distance, motor_Pos); - servoDriver_.moveBy(servoMId, LeisaiServoSpeedLevel.DEFAULT, motor_Pos); + servoDriver_.enable(servoMId, 1); + servoDriver_.moveByBlock(servoMId, LeisaiServoSpeedLevel.DEFAULT, motor_Pos); + servoDriver_.enable(servoMId, 0); } else { diff --git a/src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java b/src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java index bf15d17..6f4c49e 100644 --- a/src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java +++ b/src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java @@ -29,7 +29,7 @@ public class ShakeMotorHandler extends CommandHandler { private final Map supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream(). collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid)); - private final Set supportActions = Set.of(CmdAction.start, CmdAction.stop); + private final Set supportActions = Set.of(CmdAction.start, CmdAction.stop, CmdAction.set); @Override diff --git a/src/main/java/com/iflytop/gd/hardware/config/StepMotorConfig.java b/src/main/java/com/iflytop/gd/hardware/config/StepMotorConfig.java index 041520e..806c087 100644 --- a/src/main/java/com/iflytop/gd/hardware/config/StepMotorConfig.java +++ b/src/main/java/com/iflytop/gd/hardware/config/StepMotorConfig.java @@ -83,6 +83,7 @@ public class StepMotorConfig { static final double HEATER_MOTOR_DEFAULT_MAX_LIMIT = 100.0; static final double ACID_PUMP_MOTOR_DEFAULT_MAX_LIMIT = 1000000; final static Map stepMotorMaxLimitMap = Map.ofEntries( + Map.entry(StepMotorMId.DOOR_MOTOR_MID, 400.0), Map.entry(StepMotorMId.SHAKE_MOTOR_MID, 20000.0), Map.entry(StepMotorMId.TRAY_MOTOR_MID, 200.0), Map.entry(StepMotorMId.HBOT_X_MOTOR_MID, 1400.0), diff --git a/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/ClawDriver.java b/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/ClawDriver.java index 0d93d66..59edc7f 100644 --- a/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/ClawDriver.java +++ b/src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/ClawDriver.java @@ -98,8 +98,9 @@ public class ClawDriver { } miniServoDriver.miniServoEnable(servoMid, 1); int servoPosition = toServoPosition(position); - log.info("[ {} ] moveTo {}, servo position {}", servoMid, position, servoPosition); + log.info("[ {} ] moveTo {}, servo position {} ...", servoMid, position, servoPosition); miniServoDriver.miniServoMoveToBlock(servoMid, servoPosition); + log.info("[ {} ] moveTo {}, servo position {} end", servoMid, position, servoPosition); miniServoDriver.miniServoEnable(servoMid, 0); } diff --git a/src/main/resources/init/zapp_sub_module_reg_initial_value.csv b/src/main/resources/init/zapp_sub_module_reg_initial_value.csv index e8f44cf..cf11676 100644 --- a/src/main/resources/init/zapp_sub_module_reg_initial_value.csv +++ b/src/main/resources/init/zapp_sub_module_reg_initial_value.csv @@ -47,9 +47,9 @@ id,mid,regIndex,regInitVal 46,DualRobotAxis2SV,kreg_mini_servo_servo_protect_time,200 47,DualRobotAxis2SV,kreg_mini_servo_servo_overload_torque,80 48,DualRobotAxis2SV,kreg_mini_servo_servo_acc,0 -49,LiquidDistributionArm,kreg_liquid_distribution_arm_enable,1 -50,LiquidDistributionArm,kreg_liquid_distribution_arm_pos0_d0,571 -51,LiquidDistributionArm,kreg_liquid_distribution_arm_pos0_d1,1149 +49,LiquidDistributionArm,kreg_liquid_distribution_arm_enable,0 +50,LiquidDistributionArm,kreg_liquid_distribution_arm_pos0_d0,466 +51,LiquidDistributionArm,kreg_liquid_distribution_arm_pos0_d1,992 52,LiquidDistributionArm,kreg_liquid_distribution_arm_pos1_d0,616 53,LiquidDistributionArm,kreg_liquid_distribution_arm_pos1_d1,613 54,LiquidDistributionArm,kreg_liquid_distribution_arm_pos2_d0,841 @@ -82,20 +82,20 @@ id,mid,regIndex,regInitVal 81,LiquidDistributionArm,kreg_liquid_distribution_arm_pos15_d1,1389 82,LiquidDistributionArm,kreg_liquid_distribution_arm_pos16_d0,1748 83,LiquidDistributionArm,kreg_liquid_distribution_arm_pos16_d1,1450 -84,DoorM,kreg_step_motor_shaft,1 +84,DoorM,kreg_step_motor_shaft,0 85,DoorM,kreg_step_motor_one_circle_pulse,500 86,DoorM,kreg_step_motor_one_circle_pulse_denominator,1 -87,DoorM,kreg_step_motor_default_velocity,500 -88,DoorM,kreg_step_motor_low_velocity,0 -89,DoorM,kreg_step_motor_mid_velocity,0 -90,DoorM,kreg_step_motor_high_velocity,0 +87,DoorM,kreg_step_motor_default_velocity,30 +88,DoorM,kreg_step_motor_low_velocity,30 +89,DoorM,kreg_step_motor_mid_velocity,30 +90,DoorM,kreg_step_motor_high_velocity,30 91,DoorM,kreg_step_motor_ihold,5 92,DoorM,kreg_step_motor_irun,15 93,DoorM,kreg_step_motor_iholddelay,10 94,DoorM,kreg_step_motor_iglobalscaler,0 95,DoorM,kreg_step_motor_mres,0 -96,DoorM,kreg_step_motor_run_to_zero_speed,100 -97,DoorM,kreg_step_motor_look_zero_edge_speed,100 +96,DoorM,kreg_step_motor_run_to_zero_speed,30 +97,DoorM,kreg_step_motor_look_zero_edge_speed,30 98,DoorM,kreg_step_motor_max_d,0 99,DoorM,kreg_step_motor_min_d,0 100,DoorM,kreg_step_motor_in_debug_mode,0