diff --git a/src/main/java/com/qyft/ms/app/controller/TestController.java b/src/main/java/com/qyft/ms/app/controller/TestController.java index eada1fd..349bf30 100644 --- a/src/main/java/com/qyft/ms/app/controller/TestController.java +++ b/src/main/java/com/qyft/ms/app/controller/TestController.java @@ -4,6 +4,7 @@ import cn.hutool.json.JSONObject; import com.qyft.ms.app.device.status.DeviceStatus; import com.qyft.ms.app.model.bo.Point3D; import com.qyft.ms.app.model.vo.SelfTestVO; +import com.qyft.ms.app.service.TestService; import com.qyft.ms.app.service.VirtualDeviceService; import com.qyft.ms.system.common.device.command.CommandFuture; import com.qyft.ms.system.common.device.command.DeviceCommandGenerator; @@ -34,6 +35,7 @@ public class TestController { private final DeviceStatus deviceStatus; private final DeviceCommandService deviceCommandService; private final VirtualDeviceService virtualDeviceService; + private final TestService testService; @Operation(summary = "启动虚拟模式") @PostMapping("/virtual") @@ -72,6 +74,11 @@ public class TestController { return Result.success(); } + @Operation(summary = "测试轴速度准确度") + @PostMapping("/testSpeed") + public Result testSpeed(String xyz, Double position, Double speed) throws Exception { + return Result.success(testService.testSpeed(xyz, position, speed)); + } protected void commandWait(CommandFuture... futures) throws Exception { CompletableFuture[] responseFutures = Arrays.stream(futures) diff --git a/src/main/java/com/qyft/ms/app/device/spray/SprayTaskExecutor.java b/src/main/java/com/qyft/ms/app/device/spray/SprayTaskExecutor.java index c503156..f53da1f 100644 --- a/src/main/java/com/qyft/ms/app/device/spray/SprayTaskExecutor.java +++ b/src/main/java/com/qyft/ms/app/device/spray/SprayTaskExecutor.java @@ -166,7 +166,6 @@ public class SprayTaskExecutor { commandWait(commandFutureArray); currentStep++;//当前喷涂步数。因为暂停可能本次路线还没走完，所以这里只有在走完一次路线后才会自增 sprayTask.setCurrentStep(currentStep); - subFinishTime(); } //一次喷涂完毕后停止推注射泵 DeviceCommand highVoltageCloseCommand = DeviceCommandGenerator.highVoltageClose(); //关闭高压 @@ -179,6 +178,7 @@ public class SprayTaskExecutor { if ("grid".equals(sprayTimes.getMatrixPathType()) && sprayTimes.getGridDelay() != null) { delay(2, sprayTimes.getGridDelay() * 1000); } + subFinishTime(); } delay(1, sprayTimes.getDelay() * 1000); sprayNum++; @@ -325,123 +325,158 @@ public class SprayTaskExecutor { } /** - * 返回每片的预计完成时间列表（包含 index 与 finishTime） + * 估算每张玻片完成喷涂的时间点 */ public List> estimateFinishTimePerSlide() { + LocalDateTime startTime = LocalDateTime.now(); SprayTask sprayTask = SprayTask.getInstance(); // ===== 常量 ===== - final long CMD_LAT_MS = 15L; // 单条命令往返耗时(ms) - final long PER_OVERHEAD_MS = 0L; // 每次喷涂后附加延时(ms) - final long JOB_OVERHEAD_MS = 0L; // 任务结束后全局延时(ms) - final double X_SPEED_COMP = 0; // x轴速度补偿 - final double Y_SPEED_COMP = 0; // y轴速度补偿 - // ================ - // ===== 缓存 ===== - int cacheSprayNum = sprayTask.getSprayNum();//当前玻片已经完成喷涂的次数 - // ================ - + final long CMD_LAT_MS = 350L; // 单条命令往返耗时(ms) + final long PER_OVERHEAD_MS = 0L; // 每次喷涂路线步骤附加延时(ms) + final long JOB_OVERHEAD_MS = 0L; // 任务结束后全局附加延时(ms) + + // ===== 加速/减速（方法内部定义） ===== + final double X_ACCEL = Double.MAX_VALUE; // x 轴加速度(mm/s²) + final double X_DECEL = Double.MAX_VALUE; // x 轴减速度(mm/s²) + final double Y_ACCEL = Double.MAX_VALUE; + final double Y_DECEL = Double.MAX_VALUE; + final double Z_ACCEL = Double.MAX_VALUE; + final double Z_DECEL = Double.MAX_VALUE; + // ========================================== + + int cacheSprayNum = sprayTask.getSprayNum(); + int cacheCurrentStep = sprayTask.getCurrentStep(); List> finishTimes = new ArrayList<>(); long cumulativeMs = 0L; - List sprayTaskParams = sprayTask.getSprayTaskParams(); - double curX = 0, curY = 0, curZ = 0;//定义当前位置 - int cacheCurrentStep = sprayTask.getCurrentStep(); - for (SprayTaskParams sprayTaskParam : sprayTaskParams) {//循环玻片 - if (sprayTask.getCurrentIndex() != null && sprayTaskParam.getIndex() <= sprayTask.getCurrentIndex()) {//喷涂过的玻片跳过 + List paramsList = sprayTask.getSprayTaskParams(); + double curX = 0, curY = 0, curZ = 0; + + for (SprayTaskParams params : paramsList) { + if (sprayTask.getCurrentIndex() != null + && params.getIndex() <= sprayTask.getCurrentIndex()) { continue; } - long sliceMs = 0L; //当前玻片耗时 - int sprayNum = 1; //当前玻片是第几次喷涂 - for (SprayTimes sprayTimes : sprayTaskParam.getTimes()) {//每个拨片有多次喷涂，循环每次喷涂 - if (sprayTask.getCurrentIndex() != null && sprayTaskParam.getIndex() > sprayTask.getCurrentIndex() && sprayNum < cacheSprayNum) { + + long sliceMs = 0L; + int sprayNum = 1; + + for (SprayTimes times : params.getTimes()) { + if (sprayTask.getCurrentIndex() != null && params.getIndex() > sprayTask.getCurrentIndex() && sprayNum < cacheSprayNum) { sprayNum++; continue; } - Double[] slide = slideArr[sprayTaskParam.getIndex()];//获取玻片原点的坐标 - List sprayTaskStepList = getSprayPath(sprayTimes);//计算本次喷涂的路线 - //只有在玻片首次喷涂前才需要移动，另外还未喷涂的玻片也需要计算时间 + if (cacheCurrentStep == 0) { - //先移动到玻片左上角位置 - SysSettings slideHeightSysSettings = sysSettingsService.getOne(new LambdaQueryWrapper().eq(SysSettings::getCode, "slide_height")); - Double slideHeight = Double.parseDouble(slideHeightSysSettings.getValue()); - double height = slideHeight - sprayTimes.getMotorZHeight();//下降z轴高度 + SysSettings ss = sysSettingsService.getOne(new LambdaQueryWrapper().eq(SysSettings::getCode, "slide_height")); + double slideHeight = Double.parseDouble(ss.getValue()); + double targetZ = slideHeight - times.getMotorZHeight(); + sliceMs += concurrentMoveMs( - curX, slideArr[sprayTaskParam.getIndex()][0], 20, - curY, slideArr[sprayTaskParam.getIndex()][1], 20, - curZ, height, 20, + curX, slideArr[params.getIndex()][0], times.getMovingSpeed(), X_ACCEL, X_DECEL, + curY, slideArr[params.getIndex()][1], times.getMovingSpeed(), Y_ACCEL, Y_DECEL, + curZ, targetZ, times.getMovingSpeed(), Z_ACCEL, Z_DECEL, CMD_LAT_MS ); - curX = slideArr[sprayTaskParam.getIndex()][0]; - curY = slideArr[sprayTaskParam.getIndex()][1]; - curZ = height; + + curX = slideArr[params.getIndex()][0]; + curY = slideArr[params.getIndex()][1]; + curZ = targetZ; } - double lastX = 0; - double lastY = 0; - int currentStep = 0; //记录当前线程喷涂步骤序号 - double xLength = 0; - double yLength = 0; - for (SprayTaskStep sprayTaskStep : sprayTaskStepList) {//因为田字格喷涂其实是两次 - for (int i = 0; i < sprayTaskStep.getSprayPathPointList().size(); i++) {//循环路线 + + sliceMs += 500; + + List stepList = getSprayPath(times); + int currentStep = 0; + double lastX = 0, lastY = 0; + + for (SprayTaskStep step : stepList) { + if (currentStep >= cacheCurrentStep) { + sliceMs += 10_000; + } + for (Point2D pt : step.getSprayPathPointList()) { if (currentStep < cacheCurrentStep) { currentStep++; continue; } - Point2D currentPoint = sprayTaskStep.getSprayPathPointList().get(i); - if (lastX != currentPoint.x) { - xLength += currentPoint.x; - lastX = currentPoint.x; - curX = slide[0] + currentPoint.x; - } - if (lastY != currentPoint.y) { - yLength += currentPoint.y; - lastY = currentPoint.y; - curY = slide[1] + currentPoint.y; - } - currentStep++;//当前喷涂步数。因为暂停可能本次路线还没走完，所以这里只有在走完一次路线后才会自增 + double dx = Math.abs(lastX - pt.x); + long tX = calculateAxisMoveTime(dx, times.getMovingSpeed(), X_ACCEL, X_DECEL); + sliceMs += tX; + lastX = pt.x; + curX = slideArr[params.getIndex()][0] + lastX; + + double dy = Math.abs(lastY - pt.y); + long tY = calculateAxisMoveTime(dy, times.getMovingSpeed(), Y_ACCEL, Y_DECEL); + sliceMs += tY; + lastY = pt.y; + curY = slideArr[params.getIndex()][1] + lastY; + + sliceMs += CMD_LAT_MS; + currentStep++; } - if ("grid".equals(sprayTimes.getMatrixPathType()) && sprayTimes.getGridDelay() != null) { - if (sprayTask.getRemainingGridDelay() != null) { - sliceMs += sprayTask.getRemainingGridDelay(); - } else { - sliceMs += sprayTimes.getGridDelay() * 1000; - } + if ("grid".equals(times.getMatrixPathType()) && times.getGridDelay() != null) { + sliceMs += (sprayTask.getRemainingGridDelay() != null + ? sprayTask.getRemainingGridDelay() + : times.getGridDelay() * 1_000); } + sliceMs += PER_OVERHEAD_MS; } + cacheCurrentStep = 0; - sliceMs += (long) (xLength / sprayTimes.getMovingSpeed()) * 1000; - sliceMs += (long) (yLength / sprayTimes.getMovingSpeed()) * 1000; - if (sprayTask.getRemainingDelay() != null) { - sliceMs += sprayTask.getRemainingDelay(); - } else { - sliceMs += sprayTimes.getDelay() * 1000; - } + sliceMs += (sprayTask.getRemainingDelay() != null + ? sprayTask.getRemainingDelay() + : times.getDelay() * 1_000); + sprayNum++; + sliceMs += JOB_OVERHEAD_MS; + cumulativeMs += sliceMs; + + Map rec = new HashMap<>(); + rec.put("index", params.getIndex()); + rec.put("finishTime", startTime.plus(Duration.ofMillis(cumulativeMs))); + finishTimes.add(rec); } - cumulativeMs += sliceMs; - LocalDateTime startTime = LocalDateTime.now(); - Map rec = new HashMap<>(); - rec.put("index", sprayTaskParam.getIndex()); - rec.put("finishTime", startTime.plus(Duration.ofMillis(cumulativeMs))); - finishTimes.add(rec); } + return finishTimes; } - private long moveMs(double from, double to, double speed, long cmdDelayMs) { - return (long) (Math.abs(to - from) / speed * 1000) + cmdDelayMs; + /** + * 计算单轴移动耗时（支持不同加速和减速） + */ + private long calculateAxisMoveTime(double distance, double maxSpeed, double accel, double decel) { + double dAcc = maxSpeed * maxSpeed / (2 * accel); + double dDec = maxSpeed * maxSpeed / (2 * decel); + if (distance <= dAcc + dDec) { + // 三角速度曲线 + double vp = Math.sqrt(distance * 2 * accel * decel / (accel + decel)); + double tAcc = vp / accel; + double tDec = vp / decel; + return (long) ((tAcc + tDec) * 1_000); + } else { + // 梯形速度曲线 + double tAcc = maxSpeed / accel; + double tDec = maxSpeed / decel; + double tConst = (distance - dAcc - dDec) / maxSpeed; + return (long) ((tAcc + tConst + tDec) * 1_000); + } } + /** + * 三轴并行运动耗时，取最大并加命令延时 + */ private long concurrentMoveMs( - double fx, double tx, double sx, - double fy, double ty, double sy, - double fz, double tz, double sz, - long cmd) { - long x = moveMs(fx, tx, sx, cmd); - long y = moveMs(fy, ty, sy, cmd); - long z = moveMs(fz, tz, sz, cmd); - return Math.max(x, Math.max(y, z)); + double curX, double tgtX, double spdX, double accX, double decX, + double curY, double tgtY, double spdY, double accY, double decY, + double curZ, double tgtZ, double spdZ, double accZ, double decZ, + long cmdLatMs + ) { + long tX = calculateAxisMoveTime(Math.abs(tgtX - curX), spdX, accX, decX); + long tY = calculateAxisMoveTime(Math.abs(tgtY - curY), spdY, accY, decY); + long tZ = calculateAxisMoveTime(Math.abs(tgtZ - curZ), spdZ, accZ, decZ); + return Math.max(tX, Math.max(tY, tZ)) + cmdLatMs; } diff --git a/src/main/java/com/qyft/ms/app/service/TestService.java b/src/main/java/com/qyft/ms/app/service/TestService.java index c75df82..d5183d6 100644 --- a/src/main/java/com/qyft/ms/app/service/TestService.java +++ b/src/main/java/com/qyft/ms/app/service/TestService.java @@ -1,13 +1,82 @@ package com.qyft.ms.app.service; +import cn.hutool.json.JSONObject; +import com.qyft.ms.system.common.device.command.CommandFuture; +import com.qyft.ms.system.common.device.command.DeviceCommandGenerator; +import com.qyft.ms.system.model.bo.DeviceCommand; +import com.qyft.ms.system.service.device.DeviceCommandService; import lombok.RequiredArgsConstructor; import org.springframework.stereotype.Service; +import java.util.Arrays; +import java.util.concurrent.CompletableFuture; +import java.util.concurrent.TimeUnit; + /** * 测试用 */ @Service @RequiredArgsConstructor public class TestService { + private final DeviceCommandService deviceCommandService; + + /** + * 测试指定轴从当前点移动到目标点所耗时，并计算预计时间（毫秒级）。 + * + * @param axis 指定轴：x、y 或 z（不区分大小写） + * @param target 目标位置 + * @param speed 速度（单位与设备协议保持一致，单位：位置/秒） + * @return 格式化的结果字符串，例如 "实际用时：123ms，预计用时：1200ms" + * @throws Exception 设备通信异常或非法轴参数 + */ + public String testSpeed(String axis, Double target, Double speed) throws Exception { + // 1. 获取当前三轴位置 + DeviceCommand getPosCmd = DeviceCommandGenerator.motorXyzPositionGet(); + CommandFuture getPosFuture = deviceCommandService.sendCommandNoFront(getPosCmd); + commandWait(getPosFuture); + JSONObject data = getPosFuture.getResponseResult().getJSONObject("data"); + + // 2. 根据 axis 获取当前轴位置 + double current = switch (axis.toLowerCase()) { + case "x" -> data.getDouble("xAxisPosition"); + case "y" -> data.getDouble("yAxisPosition"); + case "z" -> data.getDouble("zAxisPosition"); + default -> throw new IllegalArgumentException("Unknown axis: " + axis); + }; + + // 3. 构造对应轴的移动指令 + DeviceCommand moveCmd = switch (axis.toLowerCase()) { + case "x" -> DeviceCommandGenerator.motorXPositionSet(target, speed); + case "y" -> DeviceCommandGenerator.motorYPositionSet(target, speed); + case "z" -> DeviceCommandGenerator.motorZPositionSet(target, speed); + default -> + // 该分支在上面已处理，理论上不会到这里 + throw new IllegalArgumentException("Unknown axis: " + axis); + }; + + // 4. 发送移动指令并计时（毫秒级） + long startMs = System.currentTimeMillis(); + CommandFuture moveFuture = deviceCommandService.sendCommandNoFront(moveCmd); + long cmdMs = System.currentTimeMillis(); + commandWait(moveFuture); + long endMs = System.currentTimeMillis(); + + // 5. 计算实际用时与预计用时（毫秒） + long actualMs = endMs - startMs; + long cmdActualMs = cmdMs - startMs; + long estimatedMs = (long) (Math.abs(current - target) / speed * 1000); + long backMs = endMs - cmdMs - estimatedMs; + + // 6. 返回结果 + return String.format("实际：%dms，预计：%dms，通信：%dms，包装：%dms", actualMs, estimatedMs, cmdActualMs, backMs); + } + + private void commandWait(CommandFuture... futures) throws Exception { + CompletableFuture[] responseFutures = Arrays.stream(futures) + .map(CommandFuture::getResponseFuture) + .toArray(CompletableFuture[]::new); + CompletableFuture.allOf(responseFutures) + .get(120, TimeUnit.SECONDS); + } } diff --git a/src/main/resources/application.yml b/src/main/resources/application.yml index a82a970..ea3c91d 100644 --- a/src/main/resources/application.yml +++ b/src/main/resources/application.yml @@ -20,7 +20,7 @@ spring: mybatis-plus: configuration: # 开启 SQL 日志输出（可选） - # log-impl: org.apache.ibatis.logging.stdout.StdOutImpl + log-impl: org.apache.ibatis.logging.stdout.StdOutImpl # 如果需要加载 XML 文件（自定义 SQL），可配置 mapper-locations： mapper-locations: classpath*:mapper/*.xml @@ -40,10 +40,11 @@ jwt: tcp: enable: true # 是否开启 TCP 连接 server-enable: true # 是否开启 TCP 连接 -# host: 127.0.0.1 + host: 127.0.0.1 # host: 192.168.1.106 # host: 192.168.1.140 - host: 192.168.100.168 +# host: 192.168.100.168 +# host: 192.168.100.207 port: 9080 reconnect: 5000 # 断线重连间隔（单位：毫秒） timeout: 10000 # 连接超时时间（单位：毫秒）