拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,178 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user