Files
ParkingRobot/MultiWheelC/Movements.cs
T

630 lines
23 KiB
C#
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Tracks;
using MDCSToolBox.Commons.Controllers;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Numerics;
using System.Threading;
using FundamentalLib;
using MyParking.Shared;
namespace MultiWheelC
{
// C层测试准备:停车并等待四个舵轮稳定回到车体前向0°。
public class PrepareWheelsForward : MovementDefinition
{
public float ToleranceDegrees = 2f;
public float StableSeconds = 0.3f;
public float TimeoutSeconds = 10f;
public bool Completed { get; private set; }
public override IEnumerable<bool> Get()
{
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis,无法执行舵轮回正。");
}
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
adapter.ResetToBodyFrame();
var toleranceRadians =
AngleMath.DegreesToRadians(ToleranceDegrees);
var startTime = DateTime.UtcNow;
DateTime? alignedSince = null;
Completed = false;
if (!adapter.PrepareParallelDirection(0.0))
{
throw new InvalidOperationException(
"无法将所有舵轮下发到车体前向0°。");
}
try
{
while (true)
{
var aligned =
adapter.AreParallelWheelsAligned(
0.0,
toleranceRadians);
if (aligned)
{
if (!alignedSince.HasValue)
alignedSince = DateTime.UtcNow;
if ((DateTime.UtcNow -
alignedSince.Value).TotalSeconds >=
StableSeconds)
{
Completed = true;
yield break;
}
}
else
{
alignedSince = null;
}
if (TimeoutSeconds > 0f &&
(DateTime.UtcNow - startTime).TotalSeconds >
TimeoutSeconds)
{
throw new TimeoutException(
$"舵轮回正超过{TimeoutSeconds:F1}s" +
"测试已经取消。");
}
yield return true;
}
}
finally
{
// 只清零驱动速度,保留已经下发的0°舵角。
adapter.StopImmediately();
}
}
}
#region 功能项
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;
}
}
#endregion
#region 直线运动
//在世界坐标系下,从路径起点追踪到终点并停车
public class DstTracker : MovementDefinition
{
public Vector2 Src;
public Vector2 Dst;
// 本次轨迹的巡航速度上限,单位m/s。
public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
public float CarDirectionBias = 0f;
public Painter Painter = UI.GetPainter("DstTracker");
public override IEnumerable<bool> Get()
{
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
DriveTask task = null;
try
{
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
var tracker = new ChassisController
{
BaseSpeed = MaxSpeed
}.Get();
// 要求路径末端速度下降到零。
tracker.FinishSpeed = 0f;
var linePath = new LineTrack(Src, Dst)
{
CarDirectionBias = CarDirectionBias,
Speed = MaxSpeed
};
tracker.AddTrack(linePath);
task = new DriveTask(tracker.Track());
task.Wait();
yield return false;
}
finally
{
task?.Stop();
chassis.PredefinedDriveStop();
}
}
}
//直线行走基于轮里程
// C层单车底盘:按照车轮里程行驶指定的相对距离。
public class LineTracking : MovementDefinition
{
// 相对动作启动位置的行驶距离,单位mm。
// 正数表示前进,负数表示后退。
public float TargetDistance;
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
public float Kp = PilotDefinition.Conf.LineTrackKp;
public float Ki = PilotDefinition.Conf.LineTrackKi;
public float Kd = PilotDefinition.Conf.LineTrackKd;
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
public int SrcId = -1;
public int DstId = -1;
public Action<int> LeaveSrcFunction;
// 接近目标后是否保留速度,交给下一个动作接管。
public bool EnableHandover;
// 进入动作衔接的剩余距离,单位mm。
public float HandoverDistance = 80f;
// HandoverSpeed小于0时,使用MaxSpeed的此比例。
public float HandoverSpeedRatio = 0.5f;
// 大于等于0时,直接作为衔接速度,单位m/s。
public float HandoverSpeed = -1f;
public float MinHandoverSpeed = 0.05f;
private PIDController _pid;
// 读取当前单车直线行驶里程,单位mm。
private static float ReadPosition()
{
return
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
}
// 根据动作启动位置和目标距离执行直线里程闭环。
public override IEnumerable<bool> Get()
{
if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
{
throw new ArgumentOutOfRangeException(
nameof(TargetDistance),
"目标行驶距离必须是有限值。");
}
if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
{
throw new ArgumentOutOfRangeException(
nameof(MaxSpeed),
"最大速度必须是大于零的有限值。");
}
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
// 每次启动动作时重新读取起始编码器位置。
var startPosition = ReadPosition();
// PID仍然控制绝对编码器位置,但绝对目标由动作自动计算。
var targetPosition = startPosition + TargetDistance;
_pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
{
SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
};
var handoverRequested = false;
var keepHandoverSpeed = false;
DLog.Log(
$"直线里程动作:" +
$"起点={startPosition:F1}mm" +
$"距离={TargetDistance:F1}mm" +
$"目标={targetPosition:F1}mm",
"straight_line");
try
{
while (true)
{
var currentPosition = ReadPosition();
var remainingDistance = targetPosition - currentPosition;
// 接近目标后,保留一定速度交给后续动作。
if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
{
var direction = Math.Sign(remainingDistance);
if (direction == 0)
{
direction = Math.Sign(TargetDistance);
}
var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
var maximumSpeed = Math.Abs(MaxSpeed);
var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
var handoverSpeed = limitedSpeed * direction;
chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
handoverRequested = true;
// 保持一个调度周期,让速度命令实际生效。
yield return true;
break;
}
var speed = _pid.GetResponse(targetPosition);
chassis.SendXYThSpeed(speed, 0f, 0f);
if (_pid.IsArrived())
{
break;
}
yield return true;
}
if (SrcId != -1 &&
LeaveSrcFunction != null)
{
LeaveSrcFunction(SrcId);
DLog.Log($"释放放车点{SrcId}", "straight_line");
}
// 只有正常完成动作衔接时才允许保留非零速度。
keepHandoverSpeed = handoverRequested;
}
finally
{
// 普通完成、人工停止或异常退出时都必须停车。
if (!keepHandoverSpeed)
{
chassis.SendXYThSpeed(0f, 0f, 0f);
}
}
yield return false;
}
}
//直线行走基于detour
public class LineTracking_based_detour : MovementDefinition
{
public float LineDistance = 1000f;
public int SrcId = -1;
public int DstId = -1;
public Action<int> LeaveSrcFunction = null;
public Painter painter = UI.GetPainter("Line", false);
// C层单车轨迹:执行早期版本的两点直线跟踪动作。
public override IEnumerable<bool> Get()
{
var curpose = DetourInterface.getCartLocation();
Console.WriteLine($"curpose.th:{curpose.th}");
var src = new Vector2((float)curpose.x, (float)curpose.y);
var headingRadians =
AngleMath.DegreesToRadians(curpose.th);
var dst = new Vector2(
(float)(curpose.x +
LineDistance * Math.Cos(headingRadians)),
(float)(curpose.y +
LineDistance * Math.Sin(headingRadians)));
// var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
// (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
Console.WriteLine($"src:{src.X} {src.Y}");
Console.WriteLine($"dst:{dst.X} {dst.Y}");
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
var tracker = new ChassisController().Get();
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
tracker.AddTrack(linePath);
var _dt = new DriveTask(tracker.Track());
_dt.Wait();
if (SrcId != -1 && LeaveSrcFunction != null)
{
LeaveSrcFunction(SrcId);
DLog.Log($"释放放车点{SrcId}", "straight_line");
}
yield return false;
}
}
#endregion
#region 旋转运动
public class MultiWheelRotateInPlace : MovementDefinition
{
/// <summary>
/// 旋转目标角度
/// </summary>
public float AngleTarget;
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
public PIDController thPid;
// 将本周期PID角速度输出提供给实验记录器,单位deg/s。
public Action<float> CommandAngularSpeedObserver;
// 自转前舵轮实际角度允许误差,单位deg。
public float WheelAlignmentToleranceDegrees = 2f;
// 自转舵轮连续保持到位的时间,单位s。
public float WheelAlignmentStableSeconds = 0.3f;
// 自转舵轮准备超时时间,单位s。
public float WheelAlignmentTimeoutSeconds = 10f;
// 先准备自转舵角,再通过安全版SendXYThSpeed闭环旋转到目标角度。
public override IEnumerable<bool> Get()
{
if (Chassis == null)
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis,无法执行原地自转。");
var adapter = new MultiWheelChassisAdapter(
Chassis,
PilotDefinition.Self.CarNum);
adapter.ResetToBodyFrame();
try
{
var alignmentStarted = DateTime.Now;
DateTime? alignedSince = null;
while (true)
{
if (!adapter.PrepareSpin())
throw new InvalidOperationException(
"无法生成原地自转舵轮目标:" +
adapter.LastFailureReason);
if (adapter.AreSpinWheelsAligned)
{
if (alignedSince == null)
alignedSince = DateTime.Now;
if ((DateTime.Now - alignedSince.Value)
.TotalSeconds >=
WheelAlignmentStableSeconds)
break;
}
else
{
alignedSince = null;
}
if ((DateTime.Now - alignmentStarted)
.TotalSeconds >
WheelAlignmentTimeoutSeconds)
throw new TimeoutException(
"原地自转舵轮在限定时间内未稳定到位。");
yield return true;
}
var targetAngle =
(float)AngleMath.NormalizeDegrees(AngleTarget);
var p = PidparamsRead();
thPid = new PIDController(ThetaReader, p.Kp);
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
p.OutputUpperThreshold, p.SpeedAccPerSec);
var lastCommandTime = DateTime.Now;
while (true)
{
var s = thPid.GetResponse(targetAngle, true);
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
CommandAngularSpeedObserver?.Invoke(s);
var now = DateTime.Now;
var interval = now - lastCommandTime;
lastCommandTime = now;
// PID输出s为deg/sShared命令统一使用rad/s。
// adapter.Send最终调用普通安全版SendXYThSpeed。
var omegaRadiansPerSecond =
(float)AngleMath.DegreesToRadians(s);
if (!adapter.Send(
new ChassisCommand(
PilotDefinition.Self.CarNum,
new Twist2D(
0.0,
0.0,
omegaRadiansPerSecond)),
interval))
{
throw new InvalidOperationException(
"安全XYTh原地旋转底盘解算失败:" +
adapter.LastFailureReason);
}
if (thPid.IsArrived()) break;
yield return true;
}
Console.WriteLine($"final rotate to {targetAngle}");
}
finally
{
CommandAngularSpeedObserver?.Invoke(0f);
adapter.StopImmediately();
}
}
}
#endregion
#region 夹臂运动
public class ClampToTarget : MovementDefinition
{
public float LeftClampTarget;
public float RightClampTarget;
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
public float TimeoutSeconds = 30f;
private PIDController leftpid, rightpid;
// C层单车业务:驱动左右夹臂运动到夹紧或松开目标。
public override IEnumerable<bool> Get()
{
try
{
leftpid = new PIDController(
() => PilotDefinition.Self.ActualPosLeftArm,
ClampKp, ClampKi, ClampKd, ClampMaxI,
ClampDeadZone, MaxClampSpeed)
{
SpeedAccPerSec = ClampSpeedAcc
};
rightpid = new PIDController(
() => PilotDefinition.Self.ActualPosRightArm,
ClampKp, ClampKi, ClampKd, ClampMaxI,
ClampDeadZone, MaxClampSpeed)
{
SpeedAccPerSec = ClampSpeedAcc
};
var startTime = DateTime.UtcNow;
while (true)
{
if (TimeoutSeconds > 0f &&
(DateTime.UtcNow - startTime).TotalSeconds >
TimeoutSeconds)
{
Console.WriteLine(
$"夹臂运动超时({TimeoutSeconds:F1}s)" +
"停止左右夹臂。");
yield break;
}
var leftspeed =
leftpid.GetResponse(LeftClampTarget);
var rightspeed =
rightpid.GetResponse(RightClampTarget);
Console.WriteLine(
$"left arm speed:{leftspeed} " +
$"right arm speed:{rightspeed}");
PilotDefinition.Self.SpeedLeftArm = leftspeed;
PilotDefinition.Self.SpeedRightArm = rightspeed;
var leftArrived = leftpid.IsArrived();
var rightArrived = rightpid.IsArrived();
if (leftArrived)
PilotDefinition.Self.SpeedLeftArm = 0f;
if (rightArrived)
PilotDefinition.Self.SpeedRightArm = 0f;
if (leftArrived && rightArrived)
break;
yield return true;
}
Console.WriteLine(
$"left clamp to target:{LeftClampTarget} " +
$"right clamp to target:{RightClampTarget}");
}
finally
{
PilotDefinition.Self.SpeedLeftArm = 0f;
PilotDefinition.Self.SpeedRightArm = 0f;
}
}
}
#endregion
}