First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
shuai.li
2026-07-01 22:47:59 +08:00
parent 574987222a
commit b1c50a0916
2 changed files with 35 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/AGV.cs
+4 -3
View File
@@ -1,4 +1,5 @@
using ClumsyCore;
using ClumsyCore.Interfaces;
using FundamentalLib;
using MDCSToolBox.Clumsy.AgvInterfaces;
using MDCSToolBox.Clumsy.MotionControllers;
@@ -132,7 +133,7 @@ namespace MultiWheelC
0)
},
};
DLog.Log($"检测器数量为{detectors.Count}", "TireFollowing");
DLog.Log($"钻胎为{tireNum}", "TireFollowing");
var following = new TireFollowing()
{
GetController = () => new ChassisController().Get(),
@@ -141,7 +142,7 @@ namespace MultiWheelC
detectors = detectors,
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
TireNum = detectors.Count,
TireNum = tireNum,
CarDirection = frontLidarDetect ? 0f : 180f,
WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
};
@@ -272,7 +273,7 @@ namespace MultiWheelC
if (setLocationRes != null && setLocationRes.l_step == 2) break;
}
});
}
public float baseSpeed = 0;
}
}
MultiWheel/MultiWheelC/Movements.cs
+31 -10
View File
@@ -181,24 +181,45 @@ namespace MultiWheelC
public Action<int> LeaveSrcFunction = null;
private PIDController pid;
// GhostMode 虚拟里程计
private float _ghostDistance;
private DateTime _lastTick;
public override IEnumerable<bool> Get()
{
pid = new PIDController(() => (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0,
DeadZone, MaxSpeed)
bool isGhost = PilotDefinition.Self.GhostMode;
if (isGhost)
{
_ghostDistance = 0f;
_lastTick = DateTime.Now;
}
pid = new PIDController(() =>
isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
{ SpeedAccPerSec = MaxSpeed / 2f };
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
while (true)
{
var speed = pid.GetResponse(Target);
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
if (isGhost)
{
var now = DateTime.Now;
float dt = (float)(now - _lastTick).TotalSeconds;
_ghostDistance += speed * dt * 1000f;
_lastTick = now;
Console.WriteLine($"[Ghost] output: {speed:F2} current: {_ghostDistance:F2}");
}
else
{
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
}
var current = isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
chassis.SendXYThSpeed(speed, 0, 0);
//if (Math.Abs(current - Target) < pid.DeadZone)
//{
// chassis.SendXYThSpeed(0f, 0f, 0f);
// Console.WriteLine($"调整退出:当前({current:f2}) ,目标:({Target:f2})");
// break;
//}
if (pid.IsArrived()) break;
yield return true;
}