From f94b1436d7a28c8e28d010b2cb657ab7c064e353 Mon Sep 17 00:00:00 2001
From: 826220679@qq.com <826220679@qq.com>
Date: 星期日, 28 十二月 2025 20:36:38 +0800
Subject: [PATCH] 修改了导航预览
---
src/dikuai/daohangyulan.java | 130 +++++++++++++++++++++++++++++++------------
1 files changed, 94 insertions(+), 36 deletions(-)
diff --git a/src/dikuai/daohangyulan.java b/src/dikuai/daohangyulan.java
index edfc655..bf29464 100644
--- a/src/dikuai/daohangyulan.java
+++ b/src/dikuai/daohangyulan.java
@@ -10,6 +10,7 @@
import zhuye.Shouye;
import zhuye.MapRenderer;
import gecaoji.Gecaoji;
+import gecaoji.Device;
import gecaoji.lujingdraw;
import publicway.buttonset;
@@ -82,16 +83,30 @@
return;
}
- // 瑙ｆ瀽璺緞鍧愭爣
- pathPoints = lujingdraw.parsePlannedPath(plannedPath);
- if (pathPoints == null || pathPoints.size() < 2) {
+ // 1. 瑙ｆ瀽璺緞鍧愭爣锛堝師濮嬭矾寰勶級
+ List<Point2D.Double> rawPathPoints = lujingdraw.parsePlannedPath(plannedPath);
+ if (rawPathPoints == null || rawPathPoints.size() < 2) {
JOptionPane.showMessageDialog(null, "璺緞鍧愭爣瑙ｆ瀽澶辫触锛屾棤娉曡繘琛屽鑸瑙�", "閿欒", JOptionPane.ERROR_MESSAGE);
return;
}
+
+ // 3. 鏋勫缓鏈�缁堝鑸矾寰勭偣鍒楄〃
+ // 鐩存帴浣跨敤瑙ｆ瀽鍑烘潵鐨勮矾寰勭偣
+ pathPoints = rawPathPoints;
+
+ if (pathPoints == null || pathPoints.size() < 2) {
+ JOptionPane.showMessageDialog(null, "鏃犳硶鏋勫缓鏈夋晥鐨勫鑸矾寰�", "閿欒", JOptionPane.ERROR_MESSAGE);
+ return;
+ }
this.currentDikuai = dikuai;
this.currentPathIndex = 0;
this.currentSpeed = DEFAULT_SPEED;
+ // 灏嗗綋鍓嶉�熷害(绫�/绉�)杞崲涓簁m/h鍐欏叆璁惧yaw灞炴��
+ try {
+ double kmh = this.currentSpeed * 3.6;
+ Device.getActiveDevice().setYaw(String.valueOf(kmh));
+ } catch (Exception ignore) { }
this.navigationTrack.clear();
// 鑾峰彇棣栭〉鍜屽湴鍥炬覆鏌撳櫒
@@ -363,43 +378,67 @@
return;
}
- Point2D.Double targetPoint = pathPoints.get(currentPathIndex + 1);
-
- // 璁＄畻鍒扮洰鏍囩偣鐨勮窛绂�
- double dx = targetPoint.x - currentPos.x;
- double dy = targetPoint.y - currentPos.y;
- double distance = Math.hypot(dx, dy);
-
// 璁＄畻姣忓抚绉诲姩鐨勮窛绂伙紙绫筹級
double moveDistance = currentSpeed * (TIMER_INTERVAL_MS / 1000.0);
- if (distance <= moveDistance) {
- // 鍒拌揪鐩爣鐐癸紝绉诲姩鍒颁笅涓�涓偣
- mower.setPosition(targetPoint.x, targetPoint.y);
- navigationTrack.add(new Point2D.Double(targetPoint.x, targetPoint.y));
- mapRenderer.addNavigationPreviewTrackPoint(targetPoint);
+ // 鍒拌揪闃堝�硷細褰撹窛绂诲皬浜庤繖涓�兼椂锛岃涓哄凡鍒拌揪鐩爣鐐癸紙0.05绫筹級
+ double arrivalThreshold = 0.05;
+
+ // 寰幆澶勭悊锛岀洿鍒扮Щ鍔ㄨ窛绂荤敤瀹屾垨鍒拌揪璺緞缁堢偣
+ double remainingDistance = moveDistance;
+
+ while (remainingDistance > 0.001 && currentPathIndex < pathPoints.size() - 1) {
+ Point2D.Double targetPoint = pathPoints.get(currentPathIndex + 1);
- currentPathIndex++;
+ // 璁＄畻鍒扮洰鏍囩偣鐨勮窛绂�
+ double dx = targetPoint.x - currentPos.x;
+ double dy = targetPoint.y - currentPos.y;
+ double distance = Math.hypot(dx, dy);
- // 濡傛灉杩樻湁涓嬩竴涓偣锛岃绠楁柟鍚�
- if (currentPathIndex < pathPoints.size() - 1) {
- Point2D.Double nextPoint = pathPoints.get(currentPathIndex + 1);
- double heading = calculateHeading(targetPoint, nextPoint);
- setMowerHeading(heading);
+ // 濡傛灉璺濈灏忎簬鍒拌揪闃堝�硷紝璁や负宸插埌杈剧洰鏍囩偣锛岀Щ鍔ㄥ埌涓嬩竴涓偣
+ if (distance <= arrivalThreshold) {
+ // 鍒拌揪鐩爣鐐癸紝绉诲姩鍒扮簿纭綅缃�
+ mower.setPosition(targetPoint.x, targetPoint.y);
+ navigationTrack.add(new Point2D.Double(targetPoint.x, targetPoint.y));
+ mapRenderer.addNavigationPreviewTrackPoint(targetPoint);
+
+ currentPos = new Point2D.Double(targetPoint.x, targetPoint.y);
+ currentPathIndex++;
+
+ // 濡傛灉杩樻湁涓嬩竴涓偣锛屾洿鏂版柟鍚�
+ if (currentPathIndex < pathPoints.size() - 1) {
+ Point2D.Double nextPoint = pathPoints.get(currentPathIndex + 1);
+ double heading = calculateHeading(targetPoint, nextPoint);
+ setMowerHeading(heading);
+ }
+
+ // 缁х画澶勭悊鍓╀綑鐨勭Щ鍔ㄨ窛绂�
+ continue;
}
- } else {
+
// 鍚戠洰鏍囩偣绉诲姩
- // 鍏堟洿鏂版柟鍚戯紝纭繚杞﹀ご鏈濆悜鐩爣鐐�
+ // 璁＄畻鏈瀹為檯绉诲姩鐨勮窛绂伙紙涓嶈秴杩囧墿浣欒窛绂诲拰鍒扮洰鏍囩偣鐨勮窛绂伙級
+ double actualMoveDistance = Math.min(remainingDistance, distance);
+
+ // 鏇存柊鏂瑰悜锛岀‘淇濊溅澶存湞鍚戠洰鏍囩偣
double heading = calculateHeading(currentPos, targetPoint);
setMowerHeading(heading);
- double ratio = moveDistance / distance;
+ // 璁＄畻鏂颁綅缃�
+ double ratio = actualMoveDistance / distance;
double newX = currentPos.x + dx * ratio;
double newY = currentPos.y + dy * ratio;
mower.setPosition(newX, newY);
navigationTrack.add(new Point2D.Double(newX, newY));
mapRenderer.addNavigationPreviewTrackPoint(new Point2D.Double(newX, newY));
+
+ // 鏇存柊褰撳墠浣嶇疆鍜屽墿浣欒窛绂�
+ currentPos = new Point2D.Double(newX, newY);
+ remainingDistance -= actualMoveDistance;
+
+ // 濡傛灉宸茬粡鍒拌揪鐩爣鐐归檮杩戯紙璺濈灏忎簬闃堝�硷級锛屽湪涓嬫寰幆涓細澶勭悊
+ // 缁х画寰幆澶勭悊鍓╀綑鐨勭Щ鍔ㄨ窛绂�
}
// 鏇存柊閫熷害鏄剧ず鍒板湴鍥炬覆鏌撳櫒
@@ -416,9 +455,9 @@
/**
* 璁＄畻涓ょ偣涔嬮棿鐨勬柟鍚戣锛堝害锛�
- * 鍥炬爣榛樿鏈濅笂锛屽悜鍙虫棆杞�90搴﹁溅澶存湞鍙�
- * atan2杩斿洖鐨勮搴︼細鍚戝彸鏄�0搴︼紝鍚戜笂鏄�90搴�
- * 闇�瑕佽浆鎹负鍥炬爣鏃嬭浆瑙掑害锛氬悜鍙抽渶瑕�90搴︼紝鍚戜笂闇�瑕�0搴�
+ * 杞﹁締鍥炬爣鐨勮溅澶撮粯璁ゆ槸鍦ㄥ睆骞曟涓嬫柟锛�270搴︼級
+ * atan2杩斿洖鐨勮搴︼細鍚戝彸鏄�0搴︼紝鍚戜笂鏄�90搴︼紝鍚戝乏鏄�180搴︼紝鍚戜笅鏄�-90搴︼紙270搴︼級
+ * 闇�瑕佽浆鎹负鍥炬爣鏃嬭浆瑙掑害锛屼娇杞﹀ご鏈濆悜琛岄┒鏂瑰悜
*/
private double calculateHeading(Point2D.Double from, Point2D.Double to) {
double dx = to.x - from.x;
@@ -431,13 +470,13 @@
atan2Angle += 360;
}
- // 鍥炬爣榛樿鏈濅笂锛�0搴︼級锛屽悜鍙虫棆杞�90搴﹁溅澶存湞鍙�
- // 鎵�浠ワ細杩愬姩鏂瑰悜鍚戝彸锛�0搴︼級鈫� 闇�瑕佹棆杞�90搴�
- // 杩愬姩鏂瑰悜鍚戜笂锛�90搴︼級鈫� 闇�瑕佹棆杞�0搴�
- // 杩愬姩鏂瑰悜鍚戝乏锛�180搴︼級鈫� 闇�瑕佹棆杞�270搴�
- // 杩愬姩鏂瑰悜鍚戜笅锛�270搴︼級鈫� 闇�瑕佹棆杞�180搴�
- // 鍏紡锛歨eading = (90 - atan2Angle + 360) % 360
- double heading = (90.0 - atan2Angle + 360.0) % 360.0;
+ // 鍥炬爣榛樿鏈濅笅锛�270搴︼級锛岄渶瑕佽绠楁棆杞搴︿娇杞﹀ご鏈濆悜琛岄┒鏂瑰悜
+ // 濡傛灉杩愬姩鏂瑰悜鍚戜笂锛�90搴︼級鈫� 闇�瑕佹棆杞�180搴︼紙270 - 90 = 180锛�
+ // 濡傛灉杩愬姩鏂瑰悜鍚戝彸锛�0搴︼級鈫� 闇�瑕佹棆杞�270搴︼紙270 - 0 = 270锛�
+ // 濡傛灉杩愬姩鏂瑰悜鍚戝乏锛�180搴︼級鈫� 闇�瑕佹棆杞�90搴︼紙270 - 180 = 90锛�
+ // 濡傛灉杩愬姩鏂瑰悜鍚戜笅锛�270搴︼級鈫� 闇�瑕佹棆杞�0搴︼紙270 - 270 = 0锛�
+ // 鍏紡锛歨eading = (270 - atan2Angle + 360) % 360
+ double heading = (270.0 - atan2Angle + 360.0) % 360.0;
return heading;
}
@@ -456,6 +495,11 @@
*/
private void speedUp() {
currentSpeed += SPEED_MULTIPLIER;
+ // 鍚屾鍒拌澶噛aw灞炴�э紙km/h锛�
+ try {
+ double kmh = currentSpeed * 3.6;
+ Device.getActiveDevice().setYaw(String.valueOf(kmh));
+ } catch (Exception ignore) { }
updateSpeedDisplay();
}
@@ -469,6 +513,11 @@
currentSpeed = 0.1;
}
}
+ // 鍚屾鍒拌澶噛aw灞炴�э紙km/h锛�
+ try {
+ double kmh = currentSpeed * 3.6;
+ Device.getActiveDevice().setYaw(String.valueOf(kmh));
+ } catch (Exception ignore) { }
updateSpeedDisplay();
}
@@ -476,8 +525,11 @@
* 鏇存柊閫熷害鏄剧ず
*/
private void updateSpeedDisplay() {
- // 鍙互鍦ㄥ湴鍥句笂鏄剧ず褰撳墠閫熷害
- // 杩欓噷鏆傛椂涓嶅疄鐜帮紝濡傛灉闇�瑕佸彲浠ュ湪MapRenderer涓坊鍔犻�熷害鏄剧ず
+ // 瀵艰埅棰勮涓嬪湪鍓茶崏鏈哄浘鏍囦笂鏂瑰疄鏃舵樉绀洪�熷害锛堝浐瀹氬儚绱犻棿璺濓級
+ if (mapRenderer != null) {
+ mapRenderer.setNavigationPreviewSpeed(currentSpeed);
+ mapRenderer.repaint();
+ }
}
/**
@@ -509,6 +561,11 @@
mapRenderer.setNavigationPreviewSpeed(0.0); // 娓呴櫎閫熷害鏄剧ず
mapRenderer.repaint();
}
+
+ // 閫�鍑烘椂灏嗚澶噛aw灞炴�ч噸缃负0
+ try {
+ Device.getActiveDevice().setYaw("0");
+ } catch (Exception ignore) { }
// 鎭㈠鍦板潡绠＄悊椤甸潰
// 鍦ㄦ竻绌篶urrentDikuai涔嬪墠淇濆瓨鍦板潡缂栧彿锛屼娇鐢╢inal鍙橀噺浠ヤ究鍦╨ambda涓娇鐢�
@@ -548,3 +605,4 @@
}
+
--
Gitblit v1.10.0