124 lines
3.6 KiB
C#
124 lines
3.6 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Threading;
|
||
using ClumsyCore.Pilot;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
public class Sleep : MovementDefinition
|
||
{
|
||
public float Second = 2f;
|
||
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
if (Second <= 0)
|
||
{
|
||
yield return false;
|
||
yield break;
|
||
}
|
||
|
||
var endTime = DateTime.UtcNow.AddSeconds(Second);
|
||
while (DateTime.UtcNow < endTime)
|
||
{
|
||
Thread.Sleep(50);
|
||
yield return true;
|
||
}
|
||
|
||
yield return false;
|
||
}
|
||
}
|
||
|
||
public class DriverAble : MovementDefinition
|
||
{
|
||
public int WaitTimeoutMs = 2000;
|
||
public int PollIntervalMs = 50;
|
||
|
||
// C层单车硬件：请求全部驱动轮复位并恢复使能。
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
PilotDefinition.Self.ResetFromC = true;
|
||
|
||
try
|
||
{
|
||
var start = DateTime.Now;
|
||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||
var pollMs = Math.Max(1, PollIntervalMs);
|
||
|
||
// 至少保留一个调度周期，确保M层能收到复位请求。
|
||
yield return true;
|
||
|
||
while (!PilotDefinition.Self.WheelAbleState &&
|
||
(DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||
{
|
||
Thread.Sleep(pollMs);
|
||
yield return true;
|
||
}
|
||
}
|
||
finally
|
||
{
|
||
PilotDefinition.Self.ResetFromC = false;
|
||
}
|
||
}
|
||
}
|
||
public class DriverDisable : MovementDefinition
|
||
{
|
||
public int WaitTimeoutMs = 3000;
|
||
public int PollIntervalMs = 20;
|
||
|
||
// C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||
var pollMs = Math.Max(1, PollIntervalMs);
|
||
var startTime = DateTime.UtcNow;
|
||
var success = false;
|
||
|
||
PilotDefinition.Self.DisableFromC = true;
|
||
|
||
try
|
||
{
|
||
// 至少保持一个C层调度周期，确保M层能收到下使能请求。
|
||
yield return true;
|
||
|
||
success = !PilotDefinition.Self.WheelAbleState;
|
||
|
||
while (!success &&
|
||
(DateTime.UtcNow - startTime).TotalMilliseconds <
|
||
timeoutMs)
|
||
{
|
||
Thread.Sleep(pollMs);
|
||
|
||
success =
|
||
!PilotDefinition.Self.WheelAbleState;
|
||
|
||
if (!success)
|
||
{
|
||
yield return true;
|
||
}
|
||
}
|
||
}
|
||
finally
|
||
{
|
||
// 无论正常完成、超时、异常还是任务被停止，都撤销请求。
|
||
PilotDefinition.Self.DisableFromC = false;
|
||
}
|
||
if (success)
|
||
{
|
||
Console.WriteLine(
|
||
$"驱动器下使能完成，" +
|
||
$"WheelAbleState=" +
|
||
$"{PilotDefinition.Self.WheelAbleState}");
|
||
}
|
||
else
|
||
{
|
||
Console.WriteLine(
|
||
$"驱动器下使能超时，" +
|
||
$"WheelAbleState=" +
|
||
$"{PilotDefinition.Self.WheelAbleState}，" +
|
||
$"等待{timeoutMs}ms");
|
||
}
|
||
yield return false;
|
||
}
|
||
}
|
||
}
|