实现单车底盘通信、轮组控制与遥控流程
This commit is contained in:
@@ -1 +1,331 @@
|
||||
// 计算8个驱动电机的目标速度和舵角PID
|
||||
// 计算8个驱动电机的目标速度和舵角PID
|
||||
using CartActivator;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Commons;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
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);
|
||||
|
||||
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);
|
||||
return;
|
||||
}
|
||||
// SA关闭后立即停车。
|
||||
if (!cart.Transmitter_SA)
|
||||
{
|
||||
cart.ManualControl(
|
||||
cart.TransmitterControlMode,
|
||||
0, 0, 0,
|
||||
cart.TransmitterSpeed,
|
||||
interval);
|
||||
return;
|
||||
}
|
||||
// 限制实体遥控器的最大速度。
|
||||
cart.TransmitterSpeed = Math.Max(
|
||||
cart.TransmitterSpeedLowerLimit,
|
||||
Math.Min(
|
||||
cart.TransmitterSpeed,
|
||||
cart.TransmitterSpeedUpperLimit));
|
||||
// SD的Mode0作为底盘驾驶档。
|
||||
if (cart.Transmitter_SD == TransmitterState.Mode0)
|
||||
{
|
||||
cart.ManualControl(
|
||||
cart.TransmitterControlMode,
|
||||
cart.TransmitterLeftJoystickValX,
|
||||
cart.TransmitterRightJoystickValY,
|
||||
0,
|
||||
cart.TransmitterSpeed,
|
||||
interval);
|
||||
|
||||
return;
|
||||
}
|
||||
// 非驾驶档必须主动停车，防止上一条运动指令残留。
|
||||
cart.ManualControl(
|
||||
cart.TransmitterControlMode,
|
||||
0, 0, 0,
|
||||
cart.TransmitterSpeed,
|
||||
interval);
|
||||
}
|
||||
|
||||
// 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修正量。
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user