128 lines
3.7 KiB
C#
128 lines
3.7 KiB
C#
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;
|
|
|
|
namespace MultiWheelC
|
|
{
|
|
public class DstTracker : MovementDefinition
|
|
{
|
|
public Vector2 Src;
|
|
public Vector2 Dst;
|
|
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().Get();
|
|
var linePath = new LineTrack(Src, Dst)
|
|
{
|
|
CarDirectionBias = CarDirectionBias,
|
|
Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
|
|
};
|
|
tracker.AddTrack(linePath);
|
|
task = new DriveTask(tracker.Track());
|
|
task.Wait();
|
|
yield return false;
|
|
}
|
|
finally
|
|
{
|
|
task?.Stop();
|
|
chassis.SendXYThSpeed(0f, 0f, 0f);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
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 MultiWheelRotateInPlace : MovementDefinition
|
|
{
|
|
/// <summary>
|
|
/// 旋转目标角度
|
|
/// </summary>
|
|
public float AngleTarget;
|
|
|
|
public float MaxSpeed;
|
|
|
|
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
|
|
|
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
|
|
|
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
|
|
|
public PIDController thPid;
|
|
|
|
// 将角度归一化到零到三百六十度范围内。
|
|
private static float RangeAngle(float theta)
|
|
{
|
|
return (float)(theta - Math.Round(theta / 360.0f) * 360);
|
|
}
|
|
|
|
// 使用 PID 控制原地旋转到目标角度。
|
|
public override IEnumerable<bool> Get()
|
|
{
|
|
try
|
|
{
|
|
var targetAngle = RangeAngle(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);
|
|
|
|
while (true)
|
|
{
|
|
var s = thPid.GetResponse(targetAngle, true);
|
|
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
|
Chassis.SendXYThSpeed(0, 0, s);
|
|
if (thPid.IsArrived()) break;
|
|
yield return true;
|
|
}
|
|
|
|
Console.WriteLine($"final rotate to {targetAngle}");
|
|
}
|
|
finally
|
|
{
|
|
Chassis.SendXYThSpeed(0, 0, 0);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|