实现单车底盘通信、轮组控制与遥控流程

MedullaAdapter/DiverCartDefinition.cs

@@ -9,6 +9,10 @@ using System.Threading;
 
 namespace MedullaAdapter
 {
+    [UseLadderLogic(logic = typeof(AlarmRoutine), scanInterval = 50)]
+    [UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
+    [UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)]
+    [UseManualController(manualController = typeof(Remote))]
     public class DiverCartDefinition : MultiWheelCartDefinition
     {
         #region 基本成员
@@ -17,16 +21,14 @@ namespace MedullaAdapter
         internal enum ManualControlMode
         {
             Normal = 0, // 正常模式
-            Spin = 1, // 自旋模式
-            Crab = 2, // 螃蟹模式
+            Crab = 1, // 螃蟹模式
+            Spin = 2, // 自旋模式
         }
         internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
-        internal DateTime TransmitterLastTime; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
+        internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
         #endregion
 
         #region AsUpperIO
-        [AsUpperIO(desc = "从M上复位")] public bool ResetFromM;
-        [AsUpperIO(desc = "从M将驱动轮下使能")] public bool DisableFromM;
         [AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
         [AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
         #endregion
@@ -53,6 +55,8 @@ namespace MedullaAdapter
         #endregion
 
         #region 监控参数
+        [IOObjectMonitor(desc = "从M上复位")] public bool ResetFromM;
+        [IOObjectMonitor(desc = "从M将驱动轮下使能")] public bool DisableFromM;
         [IOObjectMonitor(desc = "左前左轮PID修正后速度")] public float SpeedLFL;
         [IOObjectMonitor(desc = "左前右轮PID修正后速度")] public float SpeedLFR;
         [IOObjectMonitor(desc = "右前左轮PID修正后速度")] public float SpeedRFL;
@@ -175,5 +179,124 @@ namespace MedullaAdapter
             State = 0;
         }
 
+        internal void ManualControl(
+            ManualControlMode mode,
+            float x,
+            float y,
+            float frontDirection,
+            float speedThreshold,
+            TimeSpan? interval = null)
+        {
+            if (Chassis == null) return;
+            var speed = speedThreshold * y;
+            var thPow = (float)Math.Pow(Math.Abs(x), ManualThetaPow) * Math.Sign(x);
+            var frontTh = -thPow * MaxManualTheta;
+            var rearTh = thPow * MaxManualTheta;
+            ManualMode = (int)mode;
+            switch (mode)
+            {
+                case ManualControlMode.Normal:
+                    SetChassisDirection(frontDirection);
+                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
+                    break;
+                case ManualControlMode.Crab:
+                    SetChassisDirection(90);
+                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
+                    break;
+                case ManualControlMode.Spin:
+                    Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
+                    break;
+                default:
+                    ManualMode = -1;
+                    Chassis.PredefinedDriveStop();
+                    break;
+            }
+        }
+
+        private void SetChassisDirection(float direction)
+        {
+            var origin = Chassis.GetOriginBias();
+            // 方向没有变化时不重复计算轮组几何关系。
+            if (Math.Abs(origin.Z - direction) < 0.001f)
+                return;
+            Chassis.SetOriginBias(
+                origin.X,
+                origin.Y,
+                direction);
+            // 坐标系改变后重新等待四个舵轮对齐。
+            Chassis.AfterDirectionChanged();
+        }
+
+        #region MCURoutine兼容参数（暂保留原硬件协议）
+
+        // 保存MCU读取到的原始输入字节，供M层监控和硬件排查使用。
+        [AsLowerIO(desc = "MCU原始输入字节")]
+        public float test;
+
+        // 保留原MCU灯光分支；单车默认值-1表示使用本车LightMode。
+        [AsUpperIO(desc = "多车灯光同步兼容值，-1使用本车灯光")]
+        public int MultiVehicleLightSync = -1;
+
+        // 以下夹臂字段用于兼容原MCU的0x209、0x20A等CAN协议；当前不使用时目标速度保持为0。
+        [AsUpperIO(desc = "左夹臂目标速度", timeOutReset = true)]
+        public float SpeedLeftArm;
+
+        [AsUpperIO(desc = "右夹臂目标速度", timeOutReset = true)]
+        public float SpeedRightArm;
+
+        [AsLowerIO(desc = "左夹臂实际速度")]
+        public float ActualSpeedLeftArm;
+
+        [AsLowerIO(desc = "右夹臂实际速度")]
+        public float ActualSpeedRightArm;
+
+        [AsLowerIO(desc = "左夹臂实际位置")]
+        public float ActualPosLeftArm;
+
+        [AsLowerIO(desc = "右夹臂实际位置")]
+        public float ActualPosRightArm;
+
+        [AsLowerIO(desc = "左夹臂状态字")]
+        public int LeftArmStateCode;
+
+        [AsLowerIO(desc = "右夹臂状态字")]
+        public int RightArmStateCode;
+
+        [AsLowerIO(desc = "左夹臂错误字")]
+        public int LeftArmErrorCode;
+
+        [AsLowerIO(desc = "右夹臂错误字")]
+        public int RightArmErrorCode;
+
+        [AsLowerIO(desc = "左夹臂电流")]
+        public float LeftArmElectric;
+
+        [AsLowerIO(desc = "右夹臂电流")]
+        public float RightArmElectric;
+
+        [AsInitParam(desc = "左夹臂低限位")]
+        [AsLowerIO]
+        public int LeftArmLowerPos = -10000;
+
+        [AsInitParam(desc = "左夹臂高限位")]
+        [AsLowerIO]
+        public int LeftArmUpperPos = 5927610;
+
+        [AsInitParam(desc = "右夹臂低限位")]
+        [AsLowerIO]
+        public int RightArmLowerPos = -17295;
+
+        [AsInitParam(desc = "右夹臂高限位")]
+        [AsLowerIO]
+        public int RightArmUpperPos = 5927610;
+
+        [AsLowerIO(desc = "左夹臂驱动器远程帧709")]
+        public byte LArmRemoteCode;
+
+        [AsLowerIO(desc = "右夹臂驱动器远程帧70A")]
+        public byte RArmRemoteCode;
+
+        #endregion
+
     }
-}
+}