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;
using ClumsyCore.Interfaces;
using FundamentalLib; using FundamentalLib;
using MDCSToolBox.Clumsy.AgvInterfaces; using MDCSToolBox.Clumsy.AgvInterfaces;
using MDCSToolBox.Clumsy.MotionControllers; using MDCSToolBox.Clumsy.MotionControllers;
@@ -132,7 +133,7 @@ namespace MultiWheelC
0) 0)
}, },
}; };
DLog.Log($"检测器数量为{detectors.Count}", "TireFollowing"); DLog.Log($"钻胎为{tireNum}", "TireFollowing");
var following = new TireFollowing() var following = new TireFollowing()
{ {
GetController = () => new ChassisController().Get(), GetController = () => new ChassisController().Get(),
@@ -141,7 +142,7 @@ namespace MultiWheelC
detectors = detectors, detectors = detectors,
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance, SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed, MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
TireNum = detectors.Count, TireNum = tireNum,
CarDirection = frontLidarDetect ? 0f : 180f, CarDirection = frontLidarDetect ? 0f : 180f,
WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh, WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
}; };
@@ -272,7 +273,7 @@ namespace MultiWheelC
if (setLocationRes != null && setLocationRes.l_step == 2) break; if (setLocationRes != null && setLocationRes.l_step == 2) break;
} }
}); });
} }
public float baseSpeed = 0;
} }
} }
MultiWheel/MultiWheelC/Movements.cs
+30 -9
View File
@@ -181,24 +181,45 @@ namespace MultiWheelC
public Action<int> LeaveSrcFunction = null; public Action<int> LeaveSrcFunction = null;
private PIDController pid; private PIDController pid;
// GhostMode 虚拟里程计
private float _ghostDistance;
private DateTime _lastTick;
public override IEnumerable<bool> Get() public override IEnumerable<bool> Get()
{ {
pid = new PIDController(() => (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0, bool isGhost = PilotDefinition.Self.GhostMode;
DeadZone, MaxSpeed) 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 }; { SpeedAccPerSec = MaxSpeed / 2f };
var chassis = (MultiWheelChassis)PilotDefinition.Chassis; var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
while (true) while (true)
{ {
var speed = pid.GetResponse(Target); var speed = pid.GetResponse(Target);
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}"); Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2; }
var current = isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
chassis.SendXYThSpeed(speed, 0, 0); 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; if (pid.IsArrived()) break;
yield return true; yield return true;
} }