From 574987222a84b535eaac980ab2505d2721834235 Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Tue, 30 Jun 2026 22:20:10 +0800 Subject: [PATCH] Update MultiWheel fleet sync and restore clamp support --- MultiWheel/MultiWheelC/AGV.cs | 15 ++ .../MovementTests.TireFollowing.cs | 46 ++++ MultiWheel/MultiWheelC/Movements.cs | 44 ++++ MultiWheel/MultiWheelC/PilotConfig.cs | 12 +- MultiWheel/MultiWheelC/PilotDefinition.cs | 198 +++++++++++++++--- MultiWheel/MultiWheelC/VehicleSyncModels.cs | 5 + deploy/chassis.json | 15 -- deploy/clumsy_agv1/clumsy.json | 60 ------ deploy/clumsy_agv1/clumsyconsole.json | 11 - deploy/clumsy_agv2/clumsy.json | 60 ------ deploy/clumsy_agv2/clumsyconsole.json | 11 - deploy/clumsy_diff/clumsy.json | 17 -- deploy/start_clumsy_agv1.bat | 9 - deploy/start_clumsy_agv2.bat | 9 - 14 files changed, 287 insertions(+), 225 deletions(-) delete mode 100644 deploy/chassis.json delete mode 100644 deploy/clumsy_agv1/clumsy.json delete mode 100644 deploy/clumsy_agv1/clumsyconsole.json delete mode 100644 deploy/clumsy_agv2/clumsy.json delete mode 100644 deploy/clumsy_agv2/clumsyconsole.json delete mode 100644 deploy/clumsy_diff/clumsy.json delete mode 100644 deploy/start_clumsy_agv1.bat delete mode 100644 deploy/start_clumsy_agv2.bat diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 5413d17..10fb797 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -210,6 +210,21 @@ namespace MultiWheelC DLog.Log("驱动器下使能完成", "TireFollowing"); } + // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。 + public void ClamptoTarget(bool close) + { + if (PilotDefinition.Self.GhostMode) + { + Thread.Sleep(2000); + Console.WriteLine("夹抱完成"); + return; + } + new DriveTask(new ClampToTarget() + { + LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos, + RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos + }.Get()).Wait(); + } public void LineTracking(int srcId, int dstId, float LineDistance) { while (!TryLock(dstId)) diff --git a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs index abcbdae..23fa73a 100644 --- a/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs +++ b/MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs @@ -210,6 +210,52 @@ namespace MultiWheelC private DriveTask _dt; } + [MovementTest(name = "鎶卞す鍏抽棴")] + public class ClampTest1 : MovementTest + { + public override void TestStop() + { + _dt?.Stop(); + PilotDefinition.Self.SpeedLeftArm = 0; + PilotDefinition.Self.SpeedRightArm = 0; + } + + public override void Test() + { + _dt = new DriveTask(new ClampToTarget() + { + LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos, + RightClampTarget = PilotDefinition.Self.RightArmUpperPos + }.Get()); + _dt.Wait(); + } + + private DriveTask _dt; + } + + [MovementTest(name = "鎶卞す鎵撳紑")] + public class ClampTest2 : MovementTest + { + public override void TestStop() + { + _dt?.Stop(); + PilotDefinition.Self.SpeedLeftArm = 0; + PilotDefinition.Self.SpeedRightArm = 0; + } + + public override void Test() + { + _dt = new DriveTask(new ClampToTarget() + { + LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos, + RightClampTarget = PilotDefinition.Self.RightArmLowerPos + }.Get()); + _dt.Wait(); + } + + private DriveTask _dt; + } + [MovementTest(name = "娴嬭瘯鍓嶈繘鍩轰簬杞噷绋")] public class LineTrackingTest : MovementTest { diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs index b0abcad..a9da6f0 100644 --- a/MultiWheel/MultiWheelC/Movements.cs +++ b/MultiWheel/MultiWheelC/Movements.cs @@ -73,6 +73,50 @@ namespace MultiWheelC } + public class ClampToTarget : MovementDefinition + { + public float LeftClampTarget; + public float RightClampTarget; + + public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed; + public float ClampKp = PilotDefinition.Conf.ClampControlKp; + public float ClampKi = PilotDefinition.Conf.ClampControlKi; + public float ClampKd = PilotDefinition.Conf.ClampControlKd; + public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI; + public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc; + public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone; + private PIDController leftpid, rightpid; + + public override IEnumerable Get() + { + leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd, + ClampMaxI, ClampDeadZone, MaxClampSpeed) + { SpeedAccPerSec = ClampSpeedAcc }; + + rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd, + ClampMaxI, ClampDeadZone, MaxClampSpeed) + { SpeedAccPerSec = ClampSpeedAcc }; + + while (true) + { + var leftspeed = leftpid.GetResponse(LeftClampTarget); + var rightspeed = rightpid.GetResponse(RightClampTarget); + Console.WriteLine($"left arm speed:{leftspeed} right arm speed:{rightspeed}"); + PilotDefinition.Self.SpeedLeftArm = leftspeed; + PilotDefinition.Self.SpeedRightArm = rightspeed; + if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0; + if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0; + + if (leftpid.IsArrived() && rightpid.IsArrived()) break; + yield return true; + } + + PilotDefinition.Self.SpeedLeftArm = 0; + PilotDefinition.Self.SpeedRightArm = 0; + Console.WriteLine($"left clamp to target:{LeftClampTarget} right clamp to target:{RightClampTarget}"); + } + } + public class Sleep : MovementDefinition { public float Second = 2; diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index 5945614..1a41616 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -12,7 +12,8 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "[sync] 缂栭槦甯冨眬鍋忚(deg)")] public float TestCarSyncTh = 0f; // 鎵嬪姩閬ユ帶 Vx 宸叉槸 m/s銆乂th 宸叉槸杞悜瑙(deg)锛屾澶勭郴鏁颁繚鎸 1锛堢洿閫氾級锛屼笉瑕佸啀娆＄缉鏀俱 [FieldMember(desc = "[sync] 鎵嬪姩Vx绯绘暟")] public float ManualCarSyncVxFac = 1f; - [FieldMember(desc = "[sync] 鎵嬪姩Vy绯绘暟(锜硅妯悜)")] public float ManualCarSyncVyFac = 1f; + // Manual crab mode: VxFac scales linear speed, VyFac maps steer stick ratio to crab steer angle in degrees. + [FieldMember(desc = "[sync] 鎵嬪姩Vy绯绘暟(锜硅婊℃潌鑸佃deg)")] public float ManualCarSyncVyFac = 60f; [FieldMember(desc = "[sync] 鎵嬪姩Vth绯绘暟")] public float ManualCarSyncVthFac = 1f; [FieldMember(desc = "[sync] 锜硅鑸佃涓婇檺(deg锛屽簲涓嶮edulla鑸佃疆瑙掑害闄愬埗鍖归厤锛岄粯璁120)")] public float MultiVehicleCrabSteerLimitDeg = 120f; [FieldMember(desc = "[sync] 妫娴嬩腑蹇冨亸绉(mm)")] public float DeltaDetectCenter = 350f; @@ -238,6 +239,15 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "杞儙璇嗗埆:鍚庨浄杈惧弬鏁")] public int TireBackTwoLegSgnDir = 1; [FieldMember(desc = "杞儙璇嗗埆:鍚庨浄杈惧弬鏁")] public float TireBackTwoLegCenterChangeX = 0; + [FieldMember(desc = "鎶卞す鎺у埗pid:Kp")] public float ClampControlKp = 0.1f; + [FieldMember(desc = "鎶卞す鎺у埗pid:Ki")] public float ClampControlKi = 0f; + [FieldMember(desc = "鎶卞す鎺у埗pid:Kd")] public float ClampControlKd = 0f; + [FieldMember(desc = "鎶卞す鎺у埗pid:MaxI")] public float ClampControlMaxI = 0f; + [FieldMember(desc = "鎶卞す鎺у埗pid:Acc")] public float ClampControlSpeedAcc = 1f; + [FieldMember(desc = "鎶卞す鎺у埗pid:Thresh")] public float ClampControlThresh = 0.2f; + [FieldMember(desc = "鎶卞す鎺у埗pid:DeadZone")] public float ClampControlDeadZone = 5f; + [FieldMember(desc = "鎶卞す鏈澶ч熷害")] public float MaxClampSpeed = 1.5f; + [FieldMember(desc = "鐩寸嚎琛岃蛋璺濈")] public float LineTrackDistance = 1000f; [FieldMember(desc = "鐩寸嚎琛岃蛋鏈澶ч熷害")] public float LineTrackMaxSpeed = 0.3f; [FieldMember(desc = "鐩寸嚎琛岃蛋Kp")] public float LineTrackKp = 0.2f; diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index 0e6e01a..018ad2d 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -100,6 +100,18 @@ public class PilotDefinition : MultiWheelPilotDefinition _neighborDetects = new(); @@ -427,6 +465,13 @@ public class PilotDefinition : MultiWheelPilotDefinition v.Aligned); } + var fleetReady = false; + var fleetCount = 0; + lock (FleetLock) + { + fleetCount = MultiVehicleFleet.Count; + fleetReady = fleetCount == Conf.MultiVehicleFleetNum; + } + // 瀹夊叏闂細寮鍚簰璇嗗埆鏃讹紝鏈溅鎴栦换涓鍏跺畠杞︽娴嬩笉鍒伴偦杞﹀垯鏁撮槦鍋滆溅锛堥熷害缃浂锛夈 // ownDetectOk 鏄湰杞柊椴滃硷紱鍏跺畠杞︾殑 DetectOk 鏉ヨ嚜鍏朵笂鎶/涓昏溅涓嬪彂锛堟粦鍔ㄧ獥鍙ｅ凡缁 1s 鍘绘姈锛夈 var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid; bool othersDetectOk; + string otherDetectLostCars; + List> motionInfeasibleMembers; lock (FleetLock) + { othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk); - var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk); + otherDetectLostCars = string.Join(",", MultiVehicleFleet + .Where(kv => kv.Key != CarNum && !kv.Value.DetectOk) + .Select(kv => kv.Key.ToString())); + motionInfeasibleMembers = MultiVehicleFleet + .Where(kv => kv.Key != CarNum && !kv.Value.MotionFeasible) + .ToList(); + } + + var fleetStopActive = false; + var fleetStopReason = ""; + var fleetStopSourceCar = 0; + void AddFleetStop(string reason, int sourceCar = 0) + { + if (string.IsNullOrWhiteSpace(reason)) return; + if (!fleetStopActive) + { + fleetStopReason = reason; + fleetStopSourceCar = sourceCar; + } + else + { + fleetStopReason += " | " + reason; + if (fleetStopSourceCar == 0) fleetStopSourceCar = sourceCar; + } + fleetStopActive = true; + } + + if (!fleetReady) + AddFleetStop($"member not ready/stale fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}"); + if (autoCommandTimedOut) + AddFleetStop("auto command timeout"); + if (Conf.MultiVehicleUseDetect && !ownDetectOk) + AddFleetStop("own two-leg detection lost", CarNum); + if (Conf.MultiVehicleUseDetect && !othersDetectOk) + AddFleetStop($"other two-leg detection lost cars=[{otherDetectLostCars}]"); + if (notificationStopActive) + AddFleetStop($"master stop from car{notificationStopSourceCar}: {notificationStopReason}", + notificationStopSourceCar); + if (!_multiVehicleMotionFeasible) + AddFleetStop($"car{CarNum} motion infeasible: {_multiVehicleMotionInfeasibleReason}", CarNum); + foreach (var kv in motionInfeasibleMembers) + AddFleetStop($"car{kv.Key} motion infeasible: {kv.Value.MotionInfeasibleReason}", kv.Key); + + var canMove = !fleetStopActive; // H: 鑷姩妯″紡锛堥潪鎵嬪姩锛夊繀椤绘湁鏈夋晥杞﹂槦涓績鈥斺斾富杞︾敱 SLAM 鍙嶆帹銆佷粠杞︿緷璧栦富杞﹀箍鎾 fleetPosValid銆 // 鑷姩妯″紡鏁撮槦濮挎佸缁堜緷璧 Detour锛堜笌 MultiVehicleSyncUseDetour 鏃犲叧锛夛細瀹氫綅鍏ㄧ▼涓㈠け鏃跺己鍒跺仠杞︺ @@ -665,6 +773,7 @@ public class PilotDefinition : MultiWheelPilotDefinition0 鐢ㄩ厤缃硷紝鍚﹀垯鍙 syncDistance/2锛堜笌缂栭槦鍑犱綍涓鑷达級锛屼笉鍐嶇‖缂栫爜 510銆 @@ -813,10 +926,18 @@ public class PilotDefinition : MultiWheelPilotDefinition(MultiVehicleFleet), - FleetVx = fleetVx, - FleetFrontTh = fleetFrontTh, - FleetRearTh = fleetRearTh, + FleetVx = fleetStopActive ? 0 : fleetVx, + FleetFrontTh = fleetStopActive ? 0 : fleetFrontTh, + FleetRearTh = fleetStopActive ? 0 : fleetRearTh, Mode = fleetMode, - FleetOmega = fleetOmega, + FleetOmega = fleetStopActive ? 0 : fleetOmega, + FleetStopActive = fleetStopActive, + FleetStopReason = fleetStopReason, + FleetStopSourceCar = fleetStopSourceCar, AutoEnabled = MultiVehicleAutoEnabled, // 鑴氭湰椹卞姩绛変环浜庢墜鍔ㄨ仈鍔紝骞挎挱涓 ManualEnabled 璁╀粠杞﹁В閿佽窡闅忋 ManualEnabled = manualEnabled, @@ -1106,7 +1238,9 @@ public class PilotDefinition : MultiWheelPilotDefinitionnul -if not exist clumsy.json copy /Y ..\..\deploy\clumsy_agv1\clumsy.json clumsy.json >nul -if not exist clumsyconsole.json copy /Y ..\..\deploy\clumsy_agv1\clumsyconsole.json clumsyconsole.json >nul -echo [Clumsy AGV1] Master, tag Multi1. Start Medulla build\Medulla first. -ClumsyLite.exe -pause diff --git a/deploy/start_clumsy_agv2.bat b/deploy/start_clumsy_agv2.bat deleted file mode 100644 index 856875e..0000000 --- a/deploy/start_clumsy_agv2.bat +++ /dev/null @@ -1,9 +0,0 @@ -@echo off -setlocal -cd /d "%~dp0" -if not exist chassis.json copy /Y ..\..\deploy\chassis.json chassis.json >nul -if not exist clumsy.json copy /Y ..\..\deploy\clumsy_agv2\clumsy.json clumsy.json >nul -if not exist clumsyconsole.json copy /Y ..\..\deploy\clumsy_agv2\clumsyconsole.json clumsyconsole.json >nul -echo [Clumsy AGV2] Slave, tag Multi2. Start Medulla build\Medulla_AGV2 first. -ClumsyLite.exe -pause