diff --git a/MultiWheel/MultiWheelC/AGV.cs b/MultiWheel/MultiWheelC/AGV.cs index 8d5713f..e857a63 100644 --- a/MultiWheel/MultiWheelC/AGV.cs +++ b/MultiWheel/MultiWheelC/AGV.cs @@ -5,6 +5,7 @@ using CommonUsage.Chassis; using CommonUsage.Mathematics; using FundamentalLib; using MDCSToolBox.Clumsy.AgvInterfaces; +using MDCSToolBox.Clumsy.Calibration; using MDCSToolBox.Clumsy.MotionControllers; using MDCSToolBox.Clumsy.Tracks; using MDCSToolBox.Commons.Controllers; @@ -164,7 +165,8 @@ namespace MultiWheelC } DLog.Log($"锁点{dstId}完成", "TireFollowing"); DLog.Log($"开始钻车动作,通过后雷达识别结果钻1对轮胎", "TireFollowing"); - + var chassis = (MultiWheelChassis)PilotDefinition.Chassis; + chassis.SetOriginBias(0, 0, 0); var following = new TireFollowing() { GetController = () => new ChassisController().Get(), @@ -230,6 +232,8 @@ namespace MultiWheelC { Thread.Sleep(50); } + var chassis = (MultiWheelChassis)PilotDefinition.Chassis; + chassis.SetOriginBias(0, 0, 0); DLog.Log($"锁点{dstId}完成", "TireFollowing"); IEnumerable TrackThenFollow() { diff --git a/MultiWheel/MultiWheelC/Movements.cs b/MultiWheel/MultiWheelC/Movements.cs index 043a679..0998960 100644 --- a/MultiWheel/MultiWheelC/Movements.cs +++ b/MultiWheel/MultiWheelC/Movements.cs @@ -211,12 +211,14 @@ namespace MultiWheelC public override IEnumerable Get() { + pid = new PIDController(() => (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0, DeadZone, MaxSpeed) { SpeedAccPerSec = MaxSpeed / 2f }; var chassis = (MultiWheelChassis)PilotDefinition.Chassis; + //chassis.SetOriginBias(0, 0, 0); DLog.Log($"直线行驶距离:{Target}", "TireFollowing"); while (true) {