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

增加dstTracker

Browse Source
This commit is contained in:
shuai.li
2026-07-04 14:58:03 +08:00
parent fc7c8e243b
commit 22670d916b
4 changed files with 221 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/Movements.cs
+43 -31
View File
@@ -25,6 +25,7 @@ using System.Numerics;
using System.Reflection;
using System.Text;
using System.Threading;
using static ClumsyCore.DTools.Painter;
namespace MultiWheelC
{
@@ -208,43 +209,20 @@ namespace MultiWheelC
public Action<int> LeaveSrcFunction = null;
private PIDController pid;
// GhostMode 虚拟里程计
private float _ghostDistance;
private DateTime _lastTick;
public override IEnumerable<bool> Get()
{
bool isGhost = PilotDefinition.Self.GhostMode;
if (isGhost)
{
_ghostDistance = 0f;
_lastTick = DateTime.Now;
}
pid = new PIDController(() =>
isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
{ SpeedAccPerSec = MaxSpeed / 2f };
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
while (true)
{
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}");
}
var current = isGhost ? _ghostDistance : (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
chassis.SendXYThSpeed(speed, 0, 0);
if (pid.IsArrived()) break;
@@ -261,26 +239,60 @@ namespace MultiWheelC
public class DriverAble : MovementDefinition
{
public int WaitTimeoutMs = 2000;
public int PollIntervalMs = 50;
public override IEnumerable<bool> Get()
{
Console.WriteLine("驱动器上使能");
PilotDefinition.Self.ResetFromC = true;
Thread.Sleep(200);
var start = DateTime.Now;
var timeoutMs = Math.Max(0, WaitTimeoutMs);
var pollMs = Math.Max(1, PollIntervalMs);
var success = PilotDefinition.Self.WheelAbleState;
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
{
Thread.Sleep(pollMs);
success = PilotDefinition.Self.WheelAbleState;
if (!success) yield return true;
}
PilotDefinition.Self.ResetFromC = false;
Console.WriteLine("驱动器上使能完成");
if (success)
Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
else
Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
yield return false;
}
}
public class DriverDisable : MovementDefinition
{
public int WaitTimeoutMs = 3000;
public int PollIntervalMs = 20;
public override IEnumerable<bool> Get()
{
Console.WriteLine("驱动器下使能");
PilotDefinition.Self.DisableFromC = true;
Thread.Sleep(200);
var start = DateTime.Now;
var timeoutMs = Math.Max(0, WaitTimeoutMs);
var pollMs = Math.Max(1, PollIntervalMs);
var success = !PilotDefinition.Self.WheelAbleState;
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
{
Thread.Sleep(pollMs);
success = !PilotDefinition.Self.WheelAbleState;
if (!success) yield return true;
}
PilotDefinition.Self.DisableFromC = false;
Console.WriteLine("驱动器下使能完成");
if (success)
Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
else
Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
yield return false;
}
}