Initial tutorial project

2026-06-19 23:12:05 +08:00
commit e5aeae9c33
11 changed files with 553 additions and 0 deletions
@@ -0,0 +1,175 @@
+using CartActivator;
+using CommonUsage.Chassis;
+using System.Numerics;
+
+namespace MultiWheelM;
+
+[UseManualController(manualController = typeof(Remote))]
+[UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
+public class CartDefinition : CartActivator.CartDefinition
+{
+    [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;
+
+    [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 = "舵轮前后半轴距(mm)")]
+    public float WheelBaseHalfLength = 750f;
+
+    [AsInitParam(desc = "舵轮左右半轮距(mm)")]
+    public float WheelTrackHalfWidth = 500f;
+
+    [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.");
+    }
+}