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/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;
}