同步代码

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<Void> 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);
         });

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<Container> containerList = containerService.getList();
         List<SolutionContainerState> solutionBucket = deviceState.getSolutionModule().getSolutionContainer();
         for (Container container : containerList) {

src/main/java/com/iflytop/gd/hardware/command/handlers/HBotHandler.java

@@ -74,7 +74,8 @@ public class HBotHandler extends CommandHandler {
             if(isServo)
             {
                 log.info("HBotHandler origin servo");
-                servoDriver_.moveToZero(servoMId);
+                servoDriver_.enable(servoMId, 1);
+                servoDriver_.moveToZeroBlock(servoMId);
             }
             else
             {
@@ -104,7 +105,8 @@ 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);
             }
             else
             {
@@ -120,7 +122,8 @@ 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);
             }
             else
             {

src/main/java/com/iflytop/gd/hardware/command/handlers/ShakeMotorHandler.java

@@ -29,7 +29,7 @@ public class ShakeMotorHandler extends CommandHandler {
     private final Map<CmdDevice, MId> supportCmdDeviceMIdMap = stepMotorMIdMap_.entrySet().stream().
             collect(Collectors.toMap(Map.Entry::getKey, entry -> entry.getValue().mid));
 
-    private final Set<CmdAction> supportActions = Set.of(CmdAction.start, CmdAction.stop);
+    private final Set<CmdAction> supportActions = Set.of(CmdAction.start, CmdAction.stop, CmdAction.set);
 
 
     @Override

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<StepMotorMId, Double> 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),

src/main/java/com/iflytop/gd/hardware/constants/MiniServoConstant.java

@@ -33,7 +33,6 @@ public class MiniServoConstant {
 //				zeroPos = 644;//底部 644/3000 = 220/1024
 //				refPos  = 644;//底部
 //			}}
-
 	);
 
 	static public Integer getZeroPos(MiniServoMId mid) {

src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/ClawDriver.java

@@ -98,14 +98,13 @@ 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);
-        miniServoDriver.miniServoEnable(servoMid, 0);
+        log.info("[ {} ] moveTo {}, servo position {} end", servoMid, position, servoPosition);
     }
 
     public void stop(MiniServoMId servoMid) throws HardwareException {
         log.info("[ {} ] stop", servoMid.mid.getDescription());
         miniServoDriver.miniServoStop(servoMid);
-        miniServoDriver.miniServoEnable(servoMid, 0);
     }
 }

src/main/java/com/iflytop/gd/hardware/drivers/MiniServoDriver/DualRobotDriver.java

@@ -123,13 +123,11 @@ public class DualRobotDriver {
     {
         miniServoDriver.miniServoEnable(servoMId, enable ? 1 : 0);
     }
-    
+
     public void moveToHome(MiniServoMId servoMid) throws HardwareException
     {
         log.info("Servo {} Move To Home", servoMid.mid.getDescription());
         this.servoEnable(servoMid, true);
-        miniServoDriver.miniServoMoveToZero(servoMid);
-        this.servoEnable(servoMid, false);
     }
 
     public void moveJointTo(MiniServoMId servoMid, double position) throws HardwareException
@@ -139,7 +137,6 @@ public class DualRobotDriver {
         log.info("Servo {} Move To position {}, Servo Position {}", servoMid.mid.getDescription(), position, servoPosition);
         this.servoEnable(servoMid, true);
         miniServoDriver.miniServoMoveToBlock(servoMid, servoPosition);
-        this.servoEnable(servoMid, false);
     }
 
     public void stop(MiniServoMId servoMid) throws HardwareException {

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/DoorDriver.java

@@ -43,7 +43,6 @@ public class DoorDriver{
         log.info("Motor {} Move To Home", stepMotorMId.mid.getDescription());
         motorEnable(stepMotorMId, true);
         stepMotorCtrlDriver_.stepMotorEasyMoveToZeroBlock(stepMotorMId);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveTo(StepMotorMId stepMotorMId, double position) throws Exception {
@@ -57,7 +56,6 @@ public class DoorDriver{
         int motorPosition = StepMotorConverter.toMotorPosition(position);
         log.info("Motor {} Move To position {}, Motor Position {}", stepMotorMId.mid.getDescription(), position, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveToBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveBy(StepMotorMId stepMotorMId, double distance) throws Exception {
@@ -71,7 +69,6 @@ public class DoorDriver{
         int motorPosition = StepMotorConverter.toMotorPosition(distance);
         log.info("Motor {} Move By distance {}, Motor Position {}", stepMotorMId.mid.getDescription(), distance, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveByBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void stop(StepMotorMId stepMotorMId) throws Exception {

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HBotDriver.java

@@ -46,7 +46,6 @@ public class HBotDriver {
         log.info("Motor {} Move To Home", stepMotorMId.mid.getDescription());
         motorEnable(stepMotorMId, true);
         stepMotorCtrlDriver_.stepMotorEasyMoveToZeroBlock(stepMotorMId);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveTo(StepMotorMId stepMotorMId, double position) throws Exception {
@@ -60,7 +59,6 @@ public class HBotDriver {
         int motorPosition = StepMotorConverter.toMotorPosition(position);
         log.info("Motor {} Move To position {}, Motor Position {}", stepMotorMId.mid.getDescription(), position, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveToBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveBy(StepMotorMId stepMotorMId, double distance) throws Exception {
@@ -74,7 +72,6 @@ public class HBotDriver {
         int motorPosition = StepMotorConverter.toMotorPosition(distance);
         log.info("Motor {} Move By distance {}, Motor Position {}", stepMotorMId.mid.getDescription(), distance, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveByBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void stop(StepMotorMId stepMotorMId) throws Exception {

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/HeaterMotorDriver.java

@@ -43,7 +43,6 @@ public class HeaterMotorDriver {
         log.info("Motor {} Move To Home", stepMotorMId.mid.getDescription());
         motorEnable(stepMotorMId, true);
         stepMotorCtrlDriver_.stepMotorEasyMoveToZeroBlock(stepMotorMId);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveTo(StepMotorMId stepMotorMId, double position) throws Exception {
@@ -57,7 +56,6 @@ public class HeaterMotorDriver {
         int motorPosition = StepMotorConverter.toMotorPosition(position);
         log.info("Motor {} Move To position {}, Motor Position {}", stepMotorMId.mid.getDescription(), position, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveToBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveBy(StepMotorMId stepMotorMId, double distance) throws Exception {
@@ -71,7 +69,6 @@ public class HeaterMotorDriver {
         int motorPosition = StepMotorConverter.toMotorPosition(distance);
         log.info("Motor {} Move By distance {}, Motor Position {}", stepMotorMId.mid.getDescription(), distance, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveByBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void stop(StepMotorMId stepMotorMId) throws Exception {

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/ShakeMotorDriver.java

@@ -44,7 +44,6 @@ public class ShakeMotorDriver {
         log.info("Motor {} Move To Home", stepMotorMId.mid.getDescription());
         motorEnable(stepMotorMId, true);
         stepMotorCtrlDriver_.stepMotorEasyMoveToZeroBlock(stepMotorMId);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveTo(StepMotorMId stepMotorMId, double position) throws Exception {
@@ -58,7 +57,6 @@ public class ShakeMotorDriver {
         int motorPosition = StepMotorConverter.toMotorPosition(position);
         log.info("Motor {} Move To position {}, Motor Position {}", stepMotorMId.mid.getDescription(), position, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveToBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveBy(StepMotorMId stepMotorMId, double distance) throws Exception {
@@ -72,7 +70,6 @@ public class ShakeMotorDriver {
         int motorPosition = StepMotorConverter.toMotorPosition(distance);
         log.info("Motor {} Move By distance {}, Motor Position {}", stepMotorMId.mid.getDescription(), distance, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveByBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void stop(StepMotorMId stepMotorMId) throws Exception {

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/StepMotorCtrlDriver.java

@@ -92,7 +92,6 @@ public class StepMotorCtrlDriver {
 		stepMotorEasyMoveToZeroBlock(id);
 		Integer nowpos     = stepMotorReadPos(id);
 		Integer measurepos = -canBus.moduleGetReg(id.mid, RegIndex.kreg_step_motor_dpos) + nowpos;
-		stepMotorEnable(id, 0);
 		return measurepos;
 	}
 

src/main/java/com/iflytop/gd/hardware/drivers/StepMotorDriver/TrayMotorDriver.java

@@ -46,7 +46,6 @@ public class TrayMotorDriver {
         log.info("Motor {} Move To Home", stepMotorMId.mid.getDescription());
         motorEnable(stepMotorMId, true);
         stepMotorCtrlDriver_.stepMotorEasyMoveToZeroBlock(stepMotorMId);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveTo(StepMotorMId stepMotorMId, double position) throws Exception {
@@ -60,7 +59,6 @@ public class TrayMotorDriver {
         int motorPosition = StepMotorConverter.toMotorPosition(position);
         log.info("Motor {} Move To position {}, Motor Position {}", stepMotorMId.mid.getDescription(), position, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveToBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void moveBy(StepMotorMId stepMotorMId, double distance) throws Exception {
@@ -74,13 +72,11 @@ public class TrayMotorDriver {
         int motorPosition = StepMotorConverter.toMotorPosition(distance);
         log.info("Motor {} Move By distance {}, Motor Position {}", stepMotorMId.mid.getDescription(), distance, motorPosition);
         stepMotorCtrlDriver_.stepMotorEasyMoveByBlock(stepMotorMId, motorPosition);
-        motorEnable(stepMotorMId, false);
     }
 
     public void stop(StepMotorMId stepMotorMId) throws Exception {
         log.info("Motor {} Stop", stepMotorMId.mid.getDescription());
         stepMotorCtrlDriver_.stepMotorStop(stepMotorMId);
-        motorEnable(stepMotorMId, true);
     }
 
     // ==== ==== ==== ==== ==== ==== IO Ctrl ==== ==== ==== ==== ====

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