diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs index c4b2bae..b8b96fc 100644 --- a/MultiWheel/MultiWheelC/MovementTests.cs +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -212,7 +212,7 @@ public class FleetRotateInPlace : MovementDefinition if (accel > 1e-3) estDuration += maxOmega / (2 * accel); DLog.Log( - $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " + + $"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " + $"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " + $"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}", "FleetRotateDbg"); @@ -224,6 +224,35 @@ public class FleetRotateInPlace : MovementDefinition self.MultiVehicleScriptMode = 2; self.MultiVehicleScriptVth = 0; self.MultiVehicleScriptEnabled = true; + self.MultiVehicleRotateWheelsReady = false; + self.MultiVehicleRotateFleetReady = false; + + DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg"); + while (!self.MultiVehicleRotateFleetReady) + { + self.MultiVehicleScriptVx = 0; + self.MultiVehicleScriptVy = 0; + self.MultiVehicleScriptMode = 2; + self.MultiVehicleScriptVth = 0; + self.MultiVehicleScriptEnabled = true; + Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate"); + yield return true; + } + + if (hasPos) + { + prevTh = (float)DetourInterface.getCartLocation().th; + startTh = prevTh; + } + accumulated = 0f; + start = DateTime.Now; + lastTime = start; + lastLog = DateTime.MinValue; + cmdMag = 0f; + DLog.Log( + $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " + + $"fleetAligned={self.MultiVehicleRotateFleetReady}", + "FleetRotateDbg"); var stopReason = "stop()"; while (true) diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index 018ad2d..c0960ae 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -13,6 +13,7 @@ using ClumsyCore.Pilot; using ClumsyCore.Utilities; using CommonUsage; using CommonUsage.Chassis; +using CommonUsage.Mathematics; using FundamentalLib; using FundamentalLib.Utilities; using MDCSToolBox.Clumsy.Pilot.MultiWheel; @@ -99,6 +100,10 @@ public class PilotDefinition : MultiWheelPilotDefinition max) return max; + return value; + } + + private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega) + { + var rotateActive = active && fleetMode == 2; + if (!rotateActive) + { + _multiVehicleRotateModeActive = false; + MultiVehicleRotateWheelsReady = true; + MultiVehicleRotateFleetReady = true; + _multiVehicleRotateDirectionHint = 1f; + _multiVehicleRotateAlignDetail = ""; + return; + } + + if (!_multiVehicleRotateModeActive) + { + MultiVehicleRotateWheelsReady = false; + MultiVehicleRotateFleetReady = false; + _multiVehicleRotateAlignDetail = "rotate mode just entered"; + } + + _multiVehicleRotateModeActive = true; + if (Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega) + _multiVehicleRotateDirectionHint = Math.Sign(requestedOmega); + } + + private bool PrepareMultiVehicleRotateWheels(MultiWheelChassis chassis, float requestedOmega, + float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f, + bool rampStop = true) + { + var hint = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega + ? Math.Sign(requestedOmega) + : Math.Sign(_multiVehicleRotateDirectionHint); + if (hint == 0) hint = 1; + _multiVehicleRotateDirectionHint = hint; + + // Reuse SendRotateMotion's decomposition and angle-limit checks, but keep wheel speed at zero. + if (rampStop) + chassis.RampStop(); + var alignOmega = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega + ? requestedOmega + : 0.01f * hint; + var motionOk = chassis.SendRotateMotion(alignOmega, TimeSpan.Zero, + localCompensateX: localCompensateX, localCompensateY: localCompensateY, + localCompensateTh: localCompensateTh); + var wheelAligned = TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg, + out var alignDetail); + var aligned = motionOk && wheelAligned; + if (!motionOk) + alignDetail = string.IsNullOrWhiteSpace(chassis.LastMotionDecomposeFailureReason) + ? alignDetail + : chassis.LastMotionDecomposeFailureReason; + + MultiVehicleRotateWheelsReady = aligned; + _multiVehicleRotateAlignDetail = alignDetail; + + var now = DateTime.Now; + if ((now - _multiVehicleRotateAlignLastLog).TotalMilliseconds >= 200) + { + _multiVehicleRotateAlignLastLog = now; + DLog.Log( + $"car{CarNum} ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} " + + $"hint={hint} comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " + + $"rampStop={rampStop} ok={motionOk} aligned={aligned} detail={alignDetail} " + + $"chassisReason={chassis.LastMotionDecomposeFailureReason}", + "MultiVehicleRemoteDbg"); + FleetDiag( + $"ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} hint={hint} " + + $"comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " + + $"ok={motionOk} aligned={aligned} detail={alignDetail}"); + } + + Hedingben.ToastText( + aligned ? "车队原地旋转舵轮已对齐" : $"车队原地旋转舵轮预对齐中 {alignDetail}", + $"MultiVehicle{CarNum}-rotate-align"); + + return motionOk; + } + + private static bool TryCheckRotateWheelAlignment(MultiWheelChassis chassis, float toleranceDeg, out string detail) + { + try + { +#pragma warning disable CS0612, CS0618 + var wheels = chassis.GetSteerWheels(); +#pragma warning restore CS0612, CS0618 + var maxDth = 0f; + var parts = new List(); + for (var i = 0; i < wheels.Count; i++) + { + var sw = wheels[i]; + var dth = Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())); + maxDth = Math.Max(maxDth, dth); + parts.Add($"w{i}:{dth:0.0}"); + } + + detail = $"maxDth={maxDth:0.0} tol={toleranceDeg:0.0} {string.Join(",", parts)}"; + return maxDth <= toleranceDeg; + } + catch (Exception e) + { + detail = $"read alignment failed: {e.Message}"; + return chassis.LastRotateAligned; + } + } + private void LogMultiVehicleStop(string reason, bool force = false) { var now = DateTime.Now; @@ -354,6 +476,17 @@ public class PilotDefinition : MultiWheelPilotDefinition= 200) + { + _mvNotifyApplyLastLog = applyNow; + DLog.Log( + $"car{CarNum} NOTIFY_APPLY seq={notification.Seq} mode={notification.Mode} " + + $"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " + + $"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " + + $"reason={notification.FleetStopReason}", + "MultiVehicleRemoteDbg"); + } return JsonConvert.SerializeObject(new { code = 200, message = "ok" }); } catch (Exception e) @@ -472,6 +605,7 @@ public class PilotDefinition : MultiWheelPilotDefinition 1e-6f || !notificationMotionReleased) + ? notificationRequestedFleetOmega + : fleetOmega) + : fleetOmega; + UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega); + var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance); if (isMaster) @@ -716,6 +862,7 @@ public class PilotDefinition : MultiWheelPilotDefinition> motionInfeasibleMembers; + List> rotateUnalignedMembers; lock (FleetLock) { othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk); @@ -725,7 +872,21 @@ public class PilotDefinition : MultiWheelPilotDefinition kv.Key != CarNum && !kv.Value.MotionFeasible) .ToList(); + rotateUnalignedMembers = MultiVehicleFleet + .Where(kv => !kv.Value.RotateWheelsAligned) + .ToList(); } + if (!MultiVehicleRotateWheelsReady && rotateUnalignedMembers.All(kv => kv.Key != CarNum)) + { + rotateUnalignedMembers.Add(new KeyValuePair(CarNum, new VehicleSyncInfo + { + RotateWheelsAligned = false, + RotateWheelAlignDetail = _multiVehicleRotateAlignDetail + })); + } + var rotateUnalignedCars = string.Join(",", rotateUnalignedMembers.Select(kv => kv.Key.ToString())); + var rotateFleetWheelsAligned = fleetMode != 2 || (fleetReady && rotateUnalignedMembers.Count == 0); + MultiVehicleRotateFleetReady = rotateFleetWheelsAligned; var fleetStopActive = false; var fleetStopReason = ""; @@ -794,6 +955,26 @@ public class PilotDefinition : MultiWheelPilotDefinition comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " + $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " + $"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " + - $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2})"; + $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " + + $"| ROT_ALIGN hold:{rotateHoldForAlignment} wheelReady:{MultiVehicleRotateWheelsReady} fleetReady:{MultiVehicleRotateFleetReady} " + + $"unaligned:[{rotateUnalignedCars}] reqOmega:{requestedFleetOmega:F1} detail:{_multiVehicleRotateAlignDetail}"; DLog.Log(dbg, "MultiVehicleDbg"); FleetDiag(dbg); @@ -1046,6 +1277,8 @@ public class PilotDefinition : MultiWheelPilotDefinition