Add Clumsy plugins and dual-vehicle fleet sync demo.
Restructure into DiffWheel/MultiWheel folders, add DiffWheelC and MultiWheelC with MovementTests, FleetRemote multi-vehicle manual sync, deploy templates, and documentation. Update dependency paths to D:\MDCS\Release and mirror motor feedback in MotorRoutine for simulation. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,201 @@
|
||||
using CartActivator;
|
||||
using CommonUsage.Chassis;
|
||||
using System.Numerics;
|
||||
|
||||
namespace MultiWheelM;
|
||||
|
||||
[UseManualController(manualController = typeof(Remote))]
|
||||
[UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
|
||||
public partial class CartDefinition : CartActivator.CartDefinition
|
||||
{
|
||||
[AsLowerIO(desc = "GhostMode")] public bool GhostMode = false;
|
||||
|
||||
[AsUpperIO(desc = "左前轮速度", timeOutReset = true)]
|
||||
[Playground]
|
||||
public float SpeedLeftFront;
|
||||
|
||||
[AsUpperIO(desc = "左后轮速度", timeOutReset = true)]
|
||||
[Playground]
|
||||
public float SpeedLeftRear;
|
||||
|
||||
[AsUpperIO(desc = "右前轮速度", timeOutReset = true)]
|
||||
[Playground]
|
||||
public float SpeedRightFront;
|
||||
|
||||
[AsUpperIO(desc = "右后轮速度", timeOutReset = true)]
|
||||
[Playground]
|
||||
public float SpeedRightRear;
|
||||
|
||||
[AsLowerIO(desc = "左前轮速度反馈")]
|
||||
[Playground]
|
||||
public float ActualSpeedLeftFront;
|
||||
|
||||
[AsLowerIO(desc = "左后轮速度反馈")]
|
||||
[Playground]
|
||||
public float ActualSpeedLeftRear;
|
||||
|
||||
[AsLowerIO(desc = "右前轮速度反馈")]
|
||||
[Playground]
|
||||
public float ActualSpeedRightFront;
|
||||
|
||||
[AsLowerIO(desc = "右后轮速度反馈")]
|
||||
[Playground]
|
||||
public float ActualSpeedRightRear;
|
||||
|
||||
[AsUpperIO(desc = "左前轮转向角度", timeOutReset = true)]
|
||||
[Playground]
|
||||
public float ThLeftFront;
|
||||
|
||||
[AsUpperIO(desc = "左后轮转向角度", timeOutReset = true)]
|
||||
[Playground]
|
||||
public float ThLeftRear;
|
||||
|
||||
[AsUpperIO(desc = "右前轮转向角度", timeOutReset = true)]
|
||||
[Playground]
|
||||
public float ThRightFront;
|
||||
|
||||
[AsUpperIO(desc = "右后轮转向角度", timeOutReset = true)]
|
||||
[Playground]
|
||||
public float ThRightRear;
|
||||
|
||||
[AsLowerIO(desc = "左前轮转向角度反馈")]
|
||||
[Playground]
|
||||
public float ActualThLeftFront;
|
||||
|
||||
[AsLowerIO(desc = "左后轮转向角度反馈")]
|
||||
[Playground]
|
||||
public float ActualThLeftRear;
|
||||
|
||||
[AsLowerIO(desc = "右前轮转向角度反馈")]
|
||||
[Playground]
|
||||
public float ActualThRightFront;
|
||||
|
||||
[AsLowerIO(desc = "右后轮转向角度反馈")]
|
||||
[Playground]
|
||||
public float ActualThRightRear;
|
||||
|
||||
[AsLowerIO(desc = "遥控模式")]
|
||||
public int ManualMode = -1;
|
||||
|
||||
// 车号：在 Medulla 初始化参数面板可视化配置(AsInitParam)，并作为下位信号上报给 Clumsy(AsLowerIO)。
|
||||
[AsLowerIO(desc = "车号")]
|
||||
[AsInitParam(desc = "车号")]
|
||||
public int CarNum = 1;
|
||||
|
||||
// 手动多车联动指令由遥控器在 Medulla 侧产生、上报给 Clumsy，方向为下位->上位，故统一为 AsLowerIO。
|
||||
[AsLowerIO(desc = "启用（手动）多车联动")]
|
||||
public bool MultiVehicleManualEnabled;
|
||||
|
||||
[AsLowerIO(desc = "（手动）多车联动模式")]
|
||||
public int MultiVehicleManualMode;
|
||||
|
||||
[AsLowerIO(desc = "多车联动：遥控器Vx")]
|
||||
public float MultiVehicleManualVx;
|
||||
|
||||
[AsLowerIO(desc = "多车联动：遥控器Vy")]
|
||||
public float MultiVehicleManualVy;
|
||||
|
||||
[AsLowerIO(desc = "多车联动：遥控器Vth")]
|
||||
public float MultiVehicleManualVth;
|
||||
|
||||
[AsInitParam(desc = "手动最大速度(m/s)")]
|
||||
public float MaxManualSpeed = 0.3f;
|
||||
|
||||
[AsInitParam(desc = "手动最大自旋角速度(deg/s)")]
|
||||
public float MaxManualAngularSpeed = 45f;
|
||||
|
||||
[AsInitParam(desc = "手动最大转向角度(deg)")]
|
||||
public float MaxManualTheta = 45f;
|
||||
|
||||
[AsInitParam(desc = "遥控转向输入幂数")]
|
||||
public float ManualThetaPow = 2f;
|
||||
|
||||
[AsInitParam(desc = "遥控转向安全系数(相对奇异转角的比例,0~1)")]
|
||||
public float ManualSteerSafetyRatio = 0.8f;
|
||||
|
||||
[AsInitParam(desc = "舵轮前后半轴距(mm)")]
|
||||
public float WheelBaseHalfLength = 525f;
|
||||
|
||||
[AsInitParam(desc = "舵轮左右半轮距(mm)")]
|
||||
public float WheelTrackHalfWidth = 200f;
|
||||
|
||||
[AsInitParam(desc = "舵轮最小角度(deg)")]
|
||||
public float WheelAngleLowerLimit = -120f;
|
||||
|
||||
[AsInitParam(desc = "舵轮最大角度(deg)")]
|
||||
public float WheelAngleUpperLimit = 120f;
|
||||
|
||||
[AsInitParam(desc = "底盘速度加速度(m/s^2)")]
|
||||
public float ChassisAccPerSecond = 0.3f;
|
||||
|
||||
[AsInitParam(desc = "底盘速度减速度(m/s^2)")]
|
||||
public float ChassisDeAccPerSecond = 0.5f;
|
||||
|
||||
[AsInitParam(desc = "转向最低速度系数")]
|
||||
public float ChassisMinTurnSpeedFac = 0.25f;
|
||||
|
||||
[AsInitParam(desc = "几何控制点半径(mm)")]
|
||||
public float ChassisControlPointRadius = 500f;
|
||||
|
||||
[AsInitParam(desc = "几何控制点转角速度(deg/s)")]
|
||||
public float ChassisGcpThetaPerSecond = 10f;
|
||||
|
||||
[AsInitParam(desc = "阿克曼最小转向角(deg)")]
|
||||
public float ChassisMinimumTurningAngleForAckermann = 60f;
|
||||
|
||||
public MultiWheelChassis Chassis = null!;
|
||||
|
||||
public override void Init()
|
||||
{
|
||||
Chassis = new MultiWheelChassis
|
||||
{
|
||||
MaxSpeed = MaxManualSpeed,
|
||||
AccPerSecond = ChassisAccPerSecond,
|
||||
DeAccPerSecond = ChassisDeAccPerSecond,
|
||||
MinTurnSpeedFac = ChassisMinTurnSpeedFac,
|
||||
ControlPointRadius = ChassisControlPointRadius,
|
||||
GcpThetaPerSecond = ChassisGcpThetaPerSecond,
|
||||
MinimumTurningAngleForAckermann = ChassisMinimumTurningAngleForAckermann
|
||||
};
|
||||
|
||||
Chassis.AddWheel(new SteerWheel(
|
||||
new Vector2(WheelBaseHalfLength, WheelTrackHalfWidth),
|
||||
WheelAngleLowerLimit,
|
||||
WheelAngleUpperLimit,
|
||||
speed => SpeedLeftFront = speed,
|
||||
() => ActualSpeedLeftFront,
|
||||
angle => ThLeftFront = angle,
|
||||
() => ActualThLeftFront));
|
||||
|
||||
Chassis.AddWheel(new SteerWheel(
|
||||
new Vector2(-WheelBaseHalfLength, WheelTrackHalfWidth),
|
||||
WheelAngleLowerLimit,
|
||||
WheelAngleUpperLimit,
|
||||
speed => SpeedLeftRear = speed,
|
||||
() => ActualSpeedLeftRear,
|
||||
angle => ThLeftRear = angle,
|
||||
() => ActualThLeftRear));
|
||||
|
||||
Chassis.AddWheel(new SteerWheel(
|
||||
new Vector2(WheelBaseHalfLength, -WheelTrackHalfWidth),
|
||||
WheelAngleLowerLimit,
|
||||
WheelAngleUpperLimit,
|
||||
speed => SpeedRightFront = speed,
|
||||
() => ActualSpeedRightFront,
|
||||
angle => ThRightFront = angle,
|
||||
() => ActualThRightFront));
|
||||
|
||||
Chassis.AddWheel(new SteerWheel(
|
||||
new Vector2(-WheelBaseHalfLength, -WheelTrackHalfWidth),
|
||||
WheelAngleLowerLimit,
|
||||
WheelAngleUpperLimit,
|
||||
speed => SpeedRightRear = speed,
|
||||
() => ActualSpeedRightRear,
|
||||
angle => ThRightRear = angle,
|
||||
() => ActualThRightRear));
|
||||
|
||||
Chassis.Initialize();
|
||||
|
||||
Console.WriteLine("[MultiWheelM] Minimal multi-wheel Medulla cart initialized with CommonUsage chassis geometry.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,183 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using CartActivator;
|
||||
using CycleGUI;
|
||||
using CycleGUI.API;
|
||||
using Medulla.Types;
|
||||
|
||||
namespace MultiWheelM;
|
||||
|
||||
public partial class CartDefinition
|
||||
{
|
||||
private bool _fleetRemoteActive;
|
||||
private DateTime _fleetDiagLastStick = DateTime.MinValue;
|
||||
|
||||
// 写盘诊断日志（Medulla 侧）。文件落在 Medulla 工作目录，便于复现后离线分析。
|
||||
// 每次进程启动清空一次（_fleetDiagInit），同一次运行内追加，避免跨多次运行累积。
|
||||
private static bool _fleetDiagInit;
|
||||
private void FleetDiag(string msg)
|
||||
{
|
||||
try
|
||||
{
|
||||
const string path = "fleet_diag_medulla.log";
|
||||
var line = $"{DateTime.Now:HH:mm:ss.fff} car{CarNum} {msg}{Environment.NewLine}";
|
||||
if (!_fleetDiagInit)
|
||||
{
|
||||
_fleetDiagInit = true;
|
||||
File.WriteAllText(path,
|
||||
$"=== session start {DateTime.Now:yyyy-MM-dd HH:mm:ss} ==={Environment.NewLine}" + line);
|
||||
}
|
||||
else
|
||||
File.AppendAllText(path, line);
|
||||
}
|
||||
catch
|
||||
{
|
||||
// 诊断日志失败不影响主流程
|
||||
}
|
||||
}
|
||||
|
||||
private void ZeroFleetManualFields()
|
||||
{
|
||||
MultiVehicleManualEnabled = false;
|
||||
MultiVehicleManualMode = 0;
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
}
|
||||
|
||||
[IOObjectUtility]
|
||||
public void FleetRemote()
|
||||
{
|
||||
if (_fleetRemoteActive)
|
||||
{
|
||||
GUI.GetPanelInstancingObject(this)?.BringToFront();
|
||||
return;
|
||||
}
|
||||
|
||||
_fleetRemoteActive = true;
|
||||
var fleetOn = false;
|
||||
// 速度比例 0~1，作用于摇杆输出的线速度与转向角。
|
||||
var speedRatio = 1f;
|
||||
UseGesture? manip = null;
|
||||
|
||||
void CleanupFleetRemote()
|
||||
{
|
||||
manip?.End();
|
||||
manip = null;
|
||||
fleetOn = false;
|
||||
ZeroFleetManualFields();
|
||||
_fleetRemoteActive = false;
|
||||
}
|
||||
|
||||
manip = new UseGesture();
|
||||
|
||||
manip.AddWidget(new UseGesture.ToggleWidget
|
||||
{
|
||||
name = "fleet_enable",
|
||||
text = "车队联动",
|
||||
position = "12.5%+10px, 75%+10px",
|
||||
size = "25%-10px, 12.5%-10px",
|
||||
OnValue = b =>
|
||||
{
|
||||
fleetOn = b;
|
||||
MultiVehicleManualEnabled = b;
|
||||
MultiVehicleManualMode = 0;
|
||||
if (!b)
|
||||
ZeroFleetManualFields();
|
||||
FleetDiag($"TOGGLE fleetOn={b} -> ManualEnabled={MultiVehicleManualEnabled}");
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ThrottleWidget
|
||||
{
|
||||
name = "fleet_speed_ratio",
|
||||
text = "速度比例",
|
||||
position = "50%+10px, 75%+10px",
|
||||
size = "50%-10px, 12.5%-10px",
|
||||
bounceBack = false,
|
||||
OnValue = (val, _) => speedRatio = Math.Clamp(val, 0, 1)
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ButtonWidget
|
||||
{
|
||||
name = "fleet_stop",
|
||||
text = "急停/归零",
|
||||
position = "25%+10px, 87.5%+10px",
|
||||
size = "25%-10px, 12.5%-10px",
|
||||
OnPressed = _ =>
|
||||
{
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
FleetDiag("BUTTON stop/zero");
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.StickWidget
|
||||
{
|
||||
name = "fleet_stick",
|
||||
text = "速度摇杆",
|
||||
position = "37.5%+10px, 25%+10px",
|
||||
size = "37.5%-10px, 37.5%-10px",
|
||||
bounceBack = true,
|
||||
keyboard = "Up,Down,Left,Right",
|
||||
joystick = "Axis0,Axis1",
|
||||
OnValue = (pos, manipulating) =>
|
||||
{
|
||||
if (!fleetOn)
|
||||
{
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
|
||||
{
|
||||
_fleetDiagLastStick = DateTime.Now;
|
||||
FleetDiag($"STICK(ignored,fleetOff) pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating}");
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// pos.Y(前后) → 车队线速度(m/s)；pos.X(左右) → 车队转向角(deg)。松手 bounceBack 自动回零。
|
||||
var ratio = Math.Clamp(speedRatio, 0, 1);
|
||||
MultiVehicleManualVx = Math.Clamp(pos.Y, -1, 1) * MaxManualSpeed * ratio;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = Math.Clamp(pos.X, -1, 1) * MaxManualAngularSpeed * ratio;
|
||||
|
||||
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
|
||||
{
|
||||
_fleetDiagLastStick = DateTime.Now;
|
||||
FleetDiag($"STICK pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
|
||||
$"maxV={MaxManualSpeed:0.00} maxW={MaxManualAngularSpeed:0.0} " +
|
||||
$"-> Vx={MultiVehicleManualVx:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
manip.ChangeState(new SetAppearance { drawGuizmo = false });
|
||||
manip.Start();
|
||||
|
||||
GUI.PromptOrBringToFront(pb =>
|
||||
{
|
||||
if (pb.Closing())
|
||||
{
|
||||
CleanupFleetRemote();
|
||||
pb.Panel.Exit();
|
||||
return;
|
||||
}
|
||||
|
||||
pb.Panel.TopMost(true)
|
||||
.SetDefaultDocking(Panel.Docking.None)
|
||||
.ShowTitle("车队联动遥控")
|
||||
.InitSize(320, 160)
|
||||
.InitPos(false, -32, 32, 1, 0, 1, 0);
|
||||
|
||||
pb.SeparatorText("状态");
|
||||
pb.Label($"车队联动: {MultiVehicleManualEnabled}");
|
||||
pb.Label($"速度比例: {speedRatio:0.00}");
|
||||
pb.Label($"Vx={MultiVehicleManualVx:0.000} m/s, Vth={MultiVehicleManualVth:0.0} deg/s");
|
||||
pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
|
||||
pb.Label("请勿同时打开「手动控制」面板");
|
||||
pb.Panel.Repaint();
|
||||
}, instancingObject: this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
using CartActivator;
|
||||
|
||||
namespace MultiWheelM;
|
||||
|
||||
public class MotorRoutine : LadderLogic<CartDefinition>
|
||||
{
|
||||
public override void Operation(int iteration)
|
||||
{
|
||||
cart.ActualSpeedLeftFront = cart.SpeedLeftFront;
|
||||
cart.ActualSpeedLeftRear = cart.SpeedLeftRear;
|
||||
cart.ActualSpeedRightFront = cart.SpeedRightFront;
|
||||
cart.ActualSpeedRightRear = cart.SpeedRightRear;
|
||||
cart.ActualThLeftFront = cart.ThLeftFront;
|
||||
cart.ActualThLeftRear = cart.ThLeftRear;
|
||||
cart.ActualThRightFront = cart.ThRightFront;
|
||||
cart.ActualThRightRear = cart.ThRightRear;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
<Platforms>AnyCPU</Platforms>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>..\..\build\Medulla\plugins\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="CartActivator">
|
||||
<HintPath>D:\MDCS\Release\Medulla\RefCartActivator.dll</HintPath>
|
||||
<Private>False</Private>
|
||||
</Reference>
|
||||
<Reference Include="MedullaCore">
|
||||
<HintPath>D:\MDCS\Release\Medulla\RefMedullaCore.dll</HintPath>
|
||||
<Private>False</Private>
|
||||
</Reference>
|
||||
<Reference Include="Medulla">
|
||||
<HintPath>D:\MDCS\Release\Medulla\RefMedulla2.dll</HintPath>
|
||||
<Private>False</Private>
|
||||
</Reference>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>D:\MDCS\Release\deps\RefFundamentalLib.dll</HintPath>
|
||||
<Private>False</Private>
|
||||
</Reference>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>D:\MDCS\Release\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="CycleGUI">
|
||||
<HintPath>D:\MDCS\Release\CycleGUI\RefCycleGUI.dll</HintPath>
|
||||
<Private>False</Private>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,82 @@
|
||||
using CartActivator;
|
||||
|
||||
namespace MultiWheelM;
|
||||
|
||||
public class Remote : ManualController<CartDefinition>
|
||||
{
|
||||
[AsControlItem(name = "速度摇杆", keyboard = "Up,Down,Left,Right", joystick = "Axis0,Axis1")]
|
||||
public Stick SpeedPad;
|
||||
|
||||
[AsControlItem(name = "速度比例")]
|
||||
public Throttle SpeedRatio;
|
||||
|
||||
[AsControlItem(name = "旋转模式")]
|
||||
public Switch SpinMode;
|
||||
|
||||
[AsControlItem(name = "横移模式")]
|
||||
public Switch CrabMode;
|
||||
|
||||
[AsControlItem(name = "斜行模式")]
|
||||
public Switch SwayMode;
|
||||
|
||||
[AsControlItem(name = "车头方向")]
|
||||
public DThrottle FrontDirection;
|
||||
|
||||
public override void Operation()
|
||||
{
|
||||
cart.MultiVehicleManualEnabled = false;
|
||||
cart.MultiVehicleManualVx = cart.MultiVehicleManualVy = cart.MultiVehicleManualVth = 0;
|
||||
|
||||
var speedThreshold = Math.Clamp(SpeedRatio.val, 0, 1) * Math.Max(0, cart.MaxManualSpeed);
|
||||
var speed = Math.Clamp(SpeedPad.y, -1, 1) * speedThreshold;
|
||||
var steerInput = Math.Clamp(SpeedPad.x, -1, 1);
|
||||
var steering = (float)Math.Pow(Math.Abs(steerInput), cart.ManualThetaPow) * Math.Sign(steerInput);
|
||||
|
||||
var steerLimit = ComputeSafeSteerLimit();
|
||||
var axleTheta = Math.Clamp(steering * cart.MaxManualTheta, -steerLimit, steerLimit);
|
||||
var frontTh = -axleTheta;
|
||||
var rearTh = axleTheta;
|
||||
var frontDirection = Math.Clamp(FrontDirection.dval, -1, 1) * 180;
|
||||
|
||||
if (SpinMode.on)
|
||||
{
|
||||
var direction = steerInput == 0 ? Math.Sign(speed) : steerInput;
|
||||
cart.Chassis.SendRotateMotion(direction * cart.MaxManualAngularSpeed * Math.Clamp(SpeedRatio.val, 0, 1));
|
||||
cart.ManualMode = 1;
|
||||
statusText = $"spin, w={direction * cart.MaxManualAngularSpeed:0.0}deg/s";
|
||||
return;
|
||||
}
|
||||
|
||||
if (CrabMode.on)
|
||||
{
|
||||
cart.Chassis.SetOriginBias(0, 0, 90);
|
||||
cart.Chassis.SendMotion(speed, frontTh, rearTh);
|
||||
cart.ManualMode = 2;
|
||||
statusText = $"crab, v={speed:0.000}, front={frontTh:0.0}, rear={rearTh:0.0}";
|
||||
return;
|
||||
}
|
||||
|
||||
if (SwayMode.on)
|
||||
{
|
||||
cart.Chassis.SetOriginBias(0, 0, frontDirection);
|
||||
cart.Chassis.SendMotion(speed, frontTh, -rearTh);
|
||||
cart.ManualMode = 3;
|
||||
statusText = $"sway, v={speed:0.000}, front={frontTh:0.0}, rear={-rearTh:0.0}";
|
||||
return;
|
||||
}
|
||||
|
||||
cart.Chassis.SetOriginBias(0, 0, frontDirection);
|
||||
cart.Chassis.SendMotion(speed, frontTh, rearTh);
|
||||
cart.ManualMode = 0;
|
||||
statusText = $"normal, v={speed:0.000}, front={frontTh:0.0}, rear={rearTh:0.0}";
|
||||
}
|
||||
|
||||
private float ComputeSafeSteerLimit()
|
||||
{
|
||||
var radius = Math.Max(1f, cart.ChassisControlPointRadius);
|
||||
var halfTrack = Math.Max(1f, cart.WheelTrackHalfWidth);
|
||||
var singularTheta = (float)(Math.Atan2(radius, halfTrack) * 180.0 / Math.PI);
|
||||
var ratio = Math.Clamp(cart.ManualSteerSafetyRatio, 0.1f, 1f);
|
||||
return Math.Min(cart.MaxManualTheta, singularTheta * ratio);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user