添加单车底盘仿真平台并完善运动控制与夹臂功能

MedullaAdapter/MotorRoutine.cs

@@ -56,6 +56,7 @@ namespace MedullaAdapter
                         cart.TransmitterSpeed,
                         DateTime.Now - cart.TransmitterLastTime);
 
+                    StopClampArms();
                     cart.TransmitterLastTime = DateTime.Now;
                 }
 
@@ -95,6 +96,7 @@ namespace MedullaAdapter
                         0, 0, 0,
                         cart.TransmitterSpeed,
                         interval);
+                    StopClampArms();
                     return;
             }
             // SA关闭后立即停车。
@@ -105,6 +107,7 @@ namespace MedullaAdapter
                     0, 0, 0,
                     cart.TransmitterSpeed,
                     interval);
+                StopClampArms();
                 return;
             }
             // 限制实体遥控器的最大速度。
@@ -116,6 +119,9 @@ namespace MedullaAdapter
             // SD的Mode0作为底盘驾驶档。
             if (cart.Transmitter_SD == TransmitterState.Mode0)
             {
+                // 底盘驾驶档不允许保留上一周期的夹臂速度。
+                StopClampArms();
+
                 cart.ManualControl(
                     cart.TransmitterControlMode,
                     cart.TransmitterLeftJoystickValX,
@@ -126,12 +132,37 @@ namespace MedullaAdapter
 
                 return;
             }
+            if (cart.Transmitter_SD == TransmitterState.Mode1)
+            {
+                // 切换到夹臂档时，先确保底盘停止。
+                cart.ManualControl(
+                    cart.TransmitterControlMode,
+                    0, 0, 0,
+                    cart.TransmitterSpeed,
+                    interval);
+
+                var armSpeed =
+                    cart.TransmitterRightJoystickValX *
+                    cart.ManualArmSpeedFac;
+
+                cart.SpeedLeftArm = armSpeed;
+                cart.SpeedRightArm = armSpeed;
+                return;
+            }
             // 非驾驶档必须主动停车，防止上一条运动指令残留。
             cart.ManualControl(
                 cart.TransmitterControlMode,
                 0, 0, 0,
                 cart.TransmitterSpeed,
                 interval);
+            StopClampArms();
+        }
+
+        // M层单车夹臂安全：清除物理遥控器留下的左右夹臂速度命令。
+        private void StopClampArms()
+        {
+            cart.SpeedLeftArm = 0;
+            cart.SpeedRightArm = 0;
         }
 
         // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
@@ -328,4 +359,4 @@ namespace MedullaAdapter
         }
 
     }
-}
+}