集中停车控制配置并完善终点逼近和周期诊断
This commit is contained in:
@@ -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