365 lines
13 KiB
C#
365 lines
13 KiB
C#
// 计算8个驱动电机的目标速度和舵角PID
|
|
using CartActivator;
|
|
using FundamentalLib;
|
|
using MDCSToolBox.Commons;
|
|
using System;
|
|
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
|
|
|
|
namespace MedullaAdapter
|
|
{
|
|
public class MotorRoutine : LadderLogic<DiverCartDefinition>
|
|
{
|
|
private bool _wasTransmitterControlling;
|
|
private DateTime _lastMoveTime = DateTime.Now;
|
|
public override void Operation(int iteration)
|
|
{
|
|
if (!cart.GhostMode && cart.State == -1) return;
|
|
// SA稳定打开后，使能实体遥控器。
|
|
TriggerOnce(
|
|
cart.TransmitterConnected && cart.Transmitter_SA,
|
|
300,
|
|
() =>
|
|
{
|
|
cart.TransmitterControlEnable = true;
|
|
cart.TransmitterLastTime = DateTime.Now;
|
|
});
|
|
// 遥控器断连或SA关闭时，立即撤销遥控使能。
|
|
if (!cart.TransmitterConnected || !cart.Transmitter_SA)
|
|
cart.TransmitterControlEnable = false;
|
|
var transmitterSelected =
|
|
cart.TransmitterConnected &&
|
|
cart.TransmitterControlEnable &&
|
|
cart.Transmitter_SA &&
|
|
cart.Transmitter_SC == cart.CarNum;
|
|
var transmitterControlling =
|
|
transmitterSelected &&
|
|
CartDefinition.testPriority(5, "TransmitterMode");
|
|
if (transmitterControlling)
|
|
{
|
|
TransmitterChassisControl();
|
|
cart.TransmitterLastTime = DateTime.Now;
|
|
cart.CarStatu = "实体遥控器控制";
|
|
}
|
|
else
|
|
{
|
|
// 只在遥控器刚刚退出时发送一次停车，
|
|
// 不能每周期停车，否则会覆盖C层轨迹控制。
|
|
if (_wasTransmitterControlling)
|
|
{
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
0, 0, 0,
|
|
cart.TransmitterSpeed,
|
|
DateTime.Now - cart.TransmitterLastTime);
|
|
|
|
StopClampArms();
|
|
cart.TransmitterLastTime = DateTime.Now;
|
|
}
|
|
|
|
cart.CarStatu = "正常运行";
|
|
}
|
|
_wasTransmitterControlling = transmitterControlling;
|
|
// 当前是否由C层控制。
|
|
cart.ClumsyControl = CartDefinition.currentPriority == 0;
|
|
// 计算四个舵轮PID和8个驱动电机最终速度。
|
|
UpdateDiffSteerWheelSpeeds();
|
|
// 平滑更新硬件速度限制。
|
|
UpdateSendSpeedLimit();
|
|
// 更新红黄绿灯状态。
|
|
UpdateLightMode();
|
|
}
|
|
// 物理遥控器设置
|
|
public void TransmitterChassisControl()
|
|
{
|
|
var interval = DateTime.Now - cart.TransmitterLastTime;
|
|
switch (cart.Transmitter_SB)
|
|
{
|
|
case TransmitterState.Mode0:
|
|
cart.TransmitterControlMode =
|
|
DiverCartDefinition.ManualControlMode.Normal;
|
|
break;
|
|
case TransmitterState.Mode1:
|
|
cart.TransmitterControlMode =
|
|
DiverCartDefinition.ManualControlMode.Crab;
|
|
break;
|
|
case TransmitterState.Mode2:
|
|
cart.TransmitterControlMode =
|
|
DiverCartDefinition.ManualControlMode.Spin;
|
|
break;
|
|
default:
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
0, 0, 0,
|
|
cart.TransmitterSpeed,
|
|
interval);
|
|
StopClampArms();
|
|
return;
|
|
}
|
|
// SA关闭后立即停车。
|
|
if (!cart.Transmitter_SA)
|
|
{
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
0, 0, 0,
|
|
cart.TransmitterSpeed,
|
|
interval);
|
|
StopClampArms();
|
|
return;
|
|
}
|
|
// 限制实体遥控器的最大速度。
|
|
cart.TransmitterSpeed = Math.Max(
|
|
cart.TransmitterSpeedLowerLimit,
|
|
Math.Min(
|
|
cart.TransmitterSpeed,
|
|
cart.TransmitterSpeedUpperLimit));
|
|
// SD的Mode0作为底盘驾驶档。
|
|
if (cart.Transmitter_SD == TransmitterState.Mode0)
|
|
{
|
|
// 底盘驾驶档不允许保留上一周期的夹臂速度。
|
|
StopClampArms();
|
|
|
|
cart.ManualControl(
|
|
cart.TransmitterControlMode,
|
|
cart.TransmitterLeftJoystickValX,
|
|
cart.TransmitterRightJoystickValY,
|
|
0,
|
|
cart.TransmitterSpeed,
|
|
interval);
|
|
|
|
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个驱动电机速度。
|
|
private void UpdateDiffSteerWheelSpeeds()
|
|
{
|
|
if (cart.LeftFrontPid == null ||
|
|
cart.LeftRearPid == null ||
|
|
cart.RightFrontPid == null ||
|
|
cart.RightRearPid == null)
|
|
{
|
|
cart.SpeedLFL = 0;
|
|
cart.SpeedLFR = 0;
|
|
cart.SpeedRFL = 0;
|
|
cart.SpeedRFR = 0;
|
|
cart.SpeedLRL = 0;
|
|
cart.SpeedLRR = 0;
|
|
cart.SpeedRRL = 0;
|
|
cart.SpeedRRR = 0;
|
|
return;
|
|
}
|
|
|
|
// 更新左前舵轮PID参数。
|
|
cart.LeftFrontPid.ChangeParameters(
|
|
cart.DiffSteerKp,
|
|
cart.DiffSteerKi,
|
|
cart.DiffSteerKd,
|
|
cart.DiffSteerMaxI,
|
|
cart.DiffSteerDeadZone,
|
|
cart.DiffSteerThresh,
|
|
cart.DiffSteerSpeedAcc);
|
|
|
|
// 更新左后舵轮PID参数。
|
|
cart.LeftRearPid.ChangeParameters(
|
|
cart.DiffSteerKp,
|
|
cart.DiffSteerKi,
|
|
cart.DiffSteerKd,
|
|
cart.DiffSteerMaxI,
|
|
cart.DiffSteerDeadZone,
|
|
cart.DiffSteerThresh,
|
|
cart.DiffSteerSpeedAcc);
|
|
|
|
// 更新右前舵轮PID参数。
|
|
cart.RightFrontPid.ChangeParameters(
|
|
cart.DiffSteerKp,
|
|
cart.DiffSteerKi,
|
|
cart.DiffSteerKd,
|
|
cart.DiffSteerMaxI,
|
|
cart.DiffSteerDeadZone,
|
|
cart.DiffSteerThresh,
|
|
cart.DiffSteerSpeedAcc);
|
|
|
|
// 更新右后舵轮PID参数。
|
|
cart.RightRearPid.ChangeParameters(
|
|
cart.DiffSteerKp,
|
|
cart.DiffSteerKi,
|
|
cart.DiffSteerKd,
|
|
cart.DiffSteerMaxI,
|
|
cart.DiffSteerDeadZone,
|
|
cart.DiffSteerThresh,
|
|
cart.DiffSteerSpeedAcc);
|
|
|
|
// 根据实际舵角计算四条腿的差速修正量。
|
|
var diffLf = cart.LeftFrontPid.GetResponse(
|
|
cart.ThLeftFront, false, false, "LF");
|
|
|
|
var diffLr = cart.LeftRearPid.GetResponse(
|
|
cart.ThLeftRear, false, false, "LR");
|
|
|
|
var diffRf = cart.RightFrontPid.GetResponse(
|
|
cart.ThRightFront, false, false, "RF");
|
|
|
|
var diffRr = cart.RightRearPid.GetResponse(
|
|
cart.ThRightRear, false, false, "RR");
|
|
|
|
// 保存四个转向PID的本周期修正量，供M层监控和舵轮响应CSV记录使用。
|
|
cart.DiffSteerOutputLeftFront = diffLf;
|
|
cart.DiffSteerOutputLeftRear = diffLr;
|
|
cart.DiffSteerOutputRightFront = diffRf;
|
|
cart.DiffSteerOutputRightRear = diffRr;
|
|
|
|
// 左前腿：左右电机施加方向相反的PID修正量。
|
|
cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
|
|
cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
|
|
|
|
// 左后腿。
|
|
cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
|
|
cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
|
|
|
|
// 右前腿。
|
|
cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
|
|
cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
|
|
|
|
// 右后腿。
|
|
cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
|
|
cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
|
|
}
|
|
|
|
// M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
|
|
private void UpdateSendSpeedLimit()
|
|
{
|
|
if (cart.Chassis == null)
|
|
{
|
|
cart.SendThresSpeed = 0;
|
|
return;
|
|
}
|
|
|
|
var now = DateTime.Now;
|
|
var elapsedSeconds = (float)(now - _lastMoveTime).TotalSeconds;
|
|
_lastMoveTime = now;
|
|
|
|
// 防止调试暂停或线程卡顿后，一次产生过大的速度跳变。
|
|
elapsedSeconds = Math.Clamp(elapsedSeconds, 0f, 0.2f);
|
|
|
|
// 限速值不允许小于零。
|
|
var targetLimit = Math.Max(0f, cart.ThresSpeed);
|
|
var currentLimit = Math.Max(0f, cart.SendThresSpeed);
|
|
|
|
// 增大速度上限时用加速度，减小时用减速度。
|
|
var speedChangingRate =
|
|
targetLimit > currentLimit
|
|
? cart.Chassis.AccPerSecond
|
|
: cart.Chassis.DeAccPerSecond;
|
|
|
|
speedChangingRate = Math.Max(0f, speedChangingRate);
|
|
|
|
var maxChange = speedChangingRate * elapsedSeconds;
|
|
var speedDifference = targetLimit - currentLimit;
|
|
|
|
if (Math.Abs(speedDifference) <= maxChange)
|
|
{
|
|
currentLimit = targetLimit;
|
|
}
|
|
else
|
|
{
|
|
currentLimit += Math.Sign(speedDifference) * maxChange;
|
|
}
|
|
|
|
// 计算当前8个电机目标速度中的最大绝对值。
|
|
var wheelMaxSpeed = 0f;
|
|
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFL));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFR));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFL));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFR));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRL));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRR));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRL));
|
|
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRR));
|
|
|
|
// 没有必要让平滑限速值高于当前所有车轮需要的速度。
|
|
if (targetLimit < wheelMaxSpeed &&
|
|
currentLimit > wheelMaxSpeed)
|
|
{
|
|
currentLimit = wheelMaxSpeed;
|
|
}
|
|
|
|
cart.SendThresSpeed = currentLimit;
|
|
}
|
|
|
|
// M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。
|
|
private void UpdateLightMode()
|
|
{
|
|
// 二级报警：红灯常亮。
|
|
if (cart.AlarmLevel == 2)
|
|
{
|
|
cart.LightMode = 2;
|
|
return;
|
|
}
|
|
|
|
// 一级报警：黄灯常亮。
|
|
if (cart.AlarmLevel == 1)
|
|
{
|
|
cart.LightMode = 3;
|
|
return;
|
|
}
|
|
|
|
// 驱动轮未使能：黄灯闪烁。
|
|
if (!cart.WheelAbleState)
|
|
{
|
|
FlipFlop(ref cart.LightMode, 500, 0, 3);
|
|
return;
|
|
}
|
|
|
|
// C层正在进行限速：黄灯常亮。
|
|
if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f)
|
|
{
|
|
cart.LightMode = 3;
|
|
return;
|
|
}
|
|
|
|
// 电量低于预警值：黄灯常亮。
|
|
if (cart.Soc <= cart.LowBatteryAlarmThreshold)
|
|
{
|
|
cart.LightMode = 3;
|
|
return;
|
|
}
|
|
|
|
// 正常运行：绿灯闪烁。
|
|
FlipFlop(ref cart.LightMode, 500, 0, 1);
|
|
}
|
|
|
|
}
|
|
}
|