From e156048b1ea844434ca7675af45e37a2dfad6e8c Mon Sep 17 00:00:00 2001
From: pjb <pjb123456>
Date: 星期一, 16 六月 2025 14:18:55 +0800
Subject: [PATCH] rgv调度优化
---
src/main/java/com/zy/asrs/utils/Utils.java | 156 +++++++++++++++++++++++++++++++++++++++++++++++++++-
1 files changed, 153 insertions(+), 3 deletions(-)
diff --git a/src/main/java/com/zy/asrs/utils/Utils.java b/src/main/java/com/zy/asrs/utils/Utils.java
index fd24456..abdce8f 100644
--- a/src/main/java/com/zy/asrs/utils/Utils.java
+++ b/src/main/java/com/zy/asrs/utils/Utils.java
@@ -2,11 +2,8 @@
import com.core.common.Arith;
import com.core.common.Cools;
-import com.zy.core.properties.SlaveProperties;
import java.text.DecimalFormat;
-import java.util.ArrayList;
-import java.util.List;
/**
* Created by vincent on 2020/8/27
@@ -130,6 +127,159 @@
return new double[]{x, y};
}
+ public static double[] getRgvPosNew(Integer devNo,double a, double b) {
+ double[] rgvPosNew = getRgvPosNew(a, b);
+ switch (devNo){
+ case 101:
+ case 102:
+ case 103:
+ case 104:
+ case 105:
+ case 106:
+ case 107:
+ case 108:
+ case 109:
+ case 110:
+ case 111:
+ rgvPosNew[0] = rgvPosNew[0] - 30;
+ rgvPosNew[1] = rgvPosNew[1];
+ break;
+ case 112:
+ case 113:
+ case 114:
+ case 115:
+ rgvPosNew[0] = rgvPosNew[0] + 30;
+ rgvPosNew[1] = rgvPosNew[1];
+ break;
+ case 116:
+ case 117:
+ case 118:
+ case 119:
+ case 120:
+ case 121:
+ case 122:
+ case 123:
+ case 124:
+ case 125:
+ case 126:
+ case 127:
+ case 128:
+ case 129:
+ case 130:
+ case 131:
+ case 132:
+ case 133:
+ rgvPosNew[0] = rgvPosNew[0];
+ rgvPosNew[1] = rgvPosNew[1] + 30;
+ break;
+ case 134:
+ rgvPosNew[0] = rgvPosNew[0];
+ rgvPosNew[1] = rgvPosNew[1] - 30;
+ break;
+ }
+ return rgvPosNew;
+ }
+ public static double[] getRgvPosNew(double a, double b) {
+ // 瀹氫箟鍖洪棿鍙婂搴旂殑鍑犱綍鍙傛暟锛堟柊澧炲渾寮у弬鏁帮級
+ // 缁撴瀯锛歿start, end, 绫诲瀷, 鍙傛暟...}
+ // 绫诲瀷璇存槑锛�0-鐩寸嚎锛�1-鍦嗗姬锛堥渶瑕佸渾蹇冨潗鏍囷級
+ Object[][] intervals = {
+ // 鐩寸嚎鍖洪棿锛�0-134400锛�
+ {0.0, 296680.0, 0, 400.0, 750.0, 1000.0, 750.0},
+
+ // 寮х嚎鍖洪棿锛堟嫄鐐�-杞集-133锛変繚鎸佽礉濉炲皵鏇茬嚎
+ {296680.0, 450865.0, 2, 1000.0, 750.0, 1000.0, 700.0, 950.0, 725.0},
+
+ // 鐩寸嚎鍖洪棿
+ {450865.0, 950000.0,0, 1000.0, 700.0, 100.0, 700.0},
+
+ // 鐩寸嚎鍖洪棿
+ {950000.0, 1473370.0,0, 100.0, 700.0, 100.0, -40.0},
+ // 寮х嚎鍖洪棿锛堟嫄鐐�112-椤剁偣锛夛紝鎺у埗鐐瑰亣璁句负(1215, 80)
+ {1473370.0, 1550588.0,2, 100.0, -40.0, 50.0, -40.0,75.0,-15.0},
+ // 寮х嚎鍖洪棿锛堟嫄鐐�-椤剁偣-111锛夛紝鎺у埗鐐瑰亣璁句负(1164, 125)
+// {1016193.0, 1063563.0,0, 1164.0, 80.0, 1115.0, 125.0},
+ // 鐩寸嚎鍖洪棿
+ {1550588.0, 2100000.0,0, 50.0, -40.0, 50.0, 750.0},
+ // 寮х嚎鍖洪棿锛堟嫄鐐�101-杞集锛夛紝鎺у埗鐐瑰亣璁句负(1115, 750)
+// {2068254.0, 2110201.0,2, 1115.0, 720.0, 1100.0, 750.0,0.0,0.0},
+ // 鐩寸嚎鍖洪棿
+ {2100000.0, 2318250.0,0, 50.0, 750.0, 400.0, 750.0},
+ };
+
+ for (Object[] interval : intervals) {
+ double start = (Double) interval[0];
+ double end = (Double) interval[1];
+ int type = (Integer) interval[2];
+
+ if (b >= start && b <= end) {
+ double t = (b - start) / (end - start);
+
+ // 鏍规嵁涓嶅悓绫诲瀷璁＄畻鍧愭爣
+ switch (type) {
+ case 0: // 鐩寸嚎
+ return linearInterpolation(interval, t);
+ case 1: // 璐濆灏旀洸绾�
+ return bezierInterpolation(interval, t);
+ case 2: // 鍦嗗姬
+ return circularInterpolation(interval, t);
+ }
+ }
+ }
+ return new double[]{0, 0};
+ }
+
+
+
+ // 鐩寸嚎鎻掑��
+ private static double[] linearInterpolation(Object[] interval, double t) {
+ double x1 = (Double) interval[3];
+ double y1 = (Double) interval[4];
+ double x2 = (Double) interval[5];
+ double y2 = (Double) interval[6];
+ return new double[]{
+ x1 + t * (x2 - x1),
+ y1 + t * (y2 - y1)
+ };
+ }
+
+ // 璐濆灏旀洸绾挎彃鍊�
+ private static double[] bezierInterpolation(Object[] interval, double t) {
+ double x0 = (Double) interval[3];
+ double y0 = (Double) interval[4];
+ double x2 = (Double) interval[5];
+ double y2 = (Double) interval[6];
+ double cx = (Double) interval[7];
+ double cy = (Double) interval[8];
+ return new double[]{
+ Math.pow(1-t, 2)*x0 + 2*(1-t)*t*cx + t*t*x2,
+ Math.pow(1-t, 2)*y0 + 2*(1-t)*t*cy + t*t*y2
+ };
+ }
+
+ // 鍦嗗姬鎻掑�硷紙鏂板锛�
+ private static double[] circularInterpolation(Object[] interval, double t) {
+ // 鍙傛暟瑙ｆ瀽
+ double startX = (Double) interval[3];
+ double startY = (Double) interval[4];
+ double endX = (Double) interval[5];
+ double endY = (Double) interval[6];
+ double centerX = (Double) interval[7];
+ double centerY = (Double) interval[8];
+
+ // 璁＄畻璧峰瑙掑害鍜岀粓姝㈣搴�
+ double startAngle = Math.atan2(startY - centerY, startX - centerX);
+ double endAngle = Math.atan2(endY - centerY, endX - centerX);
+
+ // 瑙掑害鎻掑��
+ double currentAngle = startAngle + t * (endAngle - startAngle);
+ double radius = Math.hypot(startX - centerX, startY - centerY);
+
+ return new double[]{
+ centerX + radius * Math.cos(currentAngle),
+ centerY + radius * Math.sin(currentAngle)
+ };
+ }
}
--
Gitblit v1.9.1