First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

增加原地旋转动作

Browse Source
This commit is contained in:
shuai.li
2026-07-07 20:00:57 +08:00
parent 68dd6fc061
commit 6ca401cf68
4 changed files with 82 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/AGV.cs
+29 -1
View File
@@ -69,6 +69,25 @@ namespace MultiWheelC
PilotDefinition.Self.IOObstacleArea = area;
}
}
public void RotateToTarget(float target)
{
//if (!needrotate) return;
var dl = new DriveTask(new MultiWheelRotateInPlace()
{
AngleTarget = target,
PidparamsRead = () => new PIDParams()
{
Kp = PilotDefinition.Conf.TireFollowingThkp,
Ki = PilotDefinition.Conf.TireFollowingThki,
Kd = PilotDefinition.Conf.TireFollowingThkd,
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
}
}.Get());
dl.Wait();
}
//参数1：tireNum 需要钻过的轮胎对数量
//参数2：frontLidarDetect true:前雷达识别 false:后雷达识别
@@ -192,7 +211,7 @@ namespace MultiWheelC
},
CarDirection = 180f,
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
MaxSpeed = 0.25f,
EnableHandover = true,
HandoverDistance = 200f,
HandoverSpeed = 0.3f,
@@ -246,6 +265,9 @@ namespace MultiWheelC
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
LeaveSrcFunction = Leave,
SrcId = srcId,
EnableHandover = true,
HandoverDistance = 200,
HandoverSpeed = 0.3f,
}.Get())
{
if (!running) break;
@@ -253,6 +275,12 @@ namespace MultiWheelC
}
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
while (!TryLock(426))
{
Thread.Sleep(20);
}
Leave(dstId);
DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
foreach (var running in new DstTracker()
{
Src = new Vector2(srcX, srcY),
MultiWheel/MultiWheelC/MovementTests.TireFollowing.cs
+29
View File
@@ -520,6 +520,35 @@ namespace MultiWheelC
}
}
[MovementTest(name = "底盘旋转测试")]
public class RotateToAngleTest : MovementTest
{
public override void TestStop()
{
throw new NotImplementedException();
}
public override void Test()
{
var target = UI.GetInput("输入旋转角度：");
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
new DriveTask(new MultiWheelRotateInPlace()
{
AngleTarget = float.Parse(target),
PidparamsRead = () => new PIDParams()
{
Kp = PilotDefinition.Conf.TireFollowingThkp,
Ki = PilotDefinition.Conf.TireFollowingThki,
Kd = PilotDefinition.Conf.TireFollowingThkd,
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
}
}.Get()).Wait();
}
}
public class utils
{
public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
MultiWheel/MultiWheelC/PilotConfig.cs
+9 -1
View File
@@ -306,6 +306,14 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
[FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
[FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
#endregion
[FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
[FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
[FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f;
[FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f;
[FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
[FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
[FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
}
MultiWheel/MultiWheelC/TireFollowing.cs
+15 -6
View File
@@ -297,7 +297,8 @@ namespace MultiWheelC
if (EnableHandover)
{
controller.SlowDistance = float.MinValue;
controller.FinishDistance = 20;
controller.FinishSpeed = 0.2f;
controller.FinishDistance = 50;
}
(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
@@ -416,6 +417,14 @@ namespace MultiWheelC
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
rd = _remainDistanceList.Average();
_painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
{
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
{
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
}
}
if (detectorIndex < detectors.Count - 1)
{
@@ -423,11 +432,11 @@ namespace MultiWheelC
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
{
WalkBlindStage1 = true;
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
{
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
}
//if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
//{
// detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
// DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
//}
WalkBlindCarPathDstX = trackDst.X;
WalkBlindCarPathDstY = trackDst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;