124 lines
3.9 KiB
C#
124 lines
3.9 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;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
// }
|