First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

初始化 shenyuxiang 分支

Browse Source
This commit is contained in:
yuxiang.shen
2026-07-21 17:38:16 +08:00
parent 7da7a3cb99
commit 2d8726efa2
118 changed files with 854 additions and 14364 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/MotorRoutine.cs
+1 -233
View File
@@ -1,233 +1 @@
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 DateTime _lastMoveTime = DateTime.Now;
private DateTime _transmitterFleetDbgLastLog = DateTime.MinValue;
private DateTime _transmitterFleetResetLastLog = DateTime.MinValue;
private void LogTransmitterFleet(string branch, bool force = false)
{
var now = DateTime.Now;
if (!force && (now - _transmitterFleetDbgLastLog).TotalMilliseconds < 200) return;
_transmitterFleetDbgLastLog = now;
DLog.Log(
$"branch={branch} connected={cart.TransmitterConnected} ctrl={cart.TransmitterControlEnable} " +
$"SA={cart.Transmitter_SA} SC={cart.Transmitter_SC} SB={cart.Transmitter_SB} SD={cart.Transmitter_SD} car={cart.CarNum} " +
$"joyLx={cart.TransmitterLeftJoystickValX:0.000} joyRy={cart.TransmitterRightJoystickValY:0.000} joyRx={cart.TransmitterRightJoystickValX:0.000} " +
$"rawLx={cart.TransmitterLeftJoystickValXRaw} rawRy={cart.TransmitterRightJoystickValYRaw} " +
$"remotePanel={cart.MultiVehicleRemoteManualEnabled} outEn={cart.MultiVehicleManualEnabled} outMode={cart.MultiVehicleManualMode} " +
$"outVx={cart.MultiVehicleManualVx:0.000} outVy={cart.MultiVehicleManualVy:0.000} outVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
"TransmitterFleetDbg");
}
private void LogTransmitterFleetReset(string reason)
{
var now = DateTime.Now;
if ((now - _transmitterFleetResetLastLog).TotalMilliseconds < 500) return;
_transmitterFleetResetLastLog = now;
DLog.Log(
$"reset reason={reason} remotePanel={cart.MultiVehicleRemoteManualEnabled} beforeEn={cart.MultiVehicleManualEnabled} " +
$"beforeMode={cart.MultiVehicleManualMode} beforeVx={cart.MultiVehicleManualVx:0.000} beforeVy={cart.MultiVehicleManualVy:0.000} " +
$"beforeVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
"TransmitterFleetDbg");
}
public override void Operation(int iteration)
{
void ResetMultiVehicle(string reason)
{
if (cart.MultiVehicleRemoteManualEnabled)
{
LogTransmitterFleet("reset-skipped-remote-panel");
return;
}
if (cart.MultiVehicleManualEnabled || cart.MultiVehicleManualVx != 0 || cart.MultiVehicleManualVy != 0 ||
cart.MultiVehicleManualVth != 0 || cart.MultiVehicleHold)
LogTransmitterFleetReset(reason);
cart.MultiVehicleManualEnabled = false;
cart.MultiVehicleManualVx = 0;
cart.MultiVehicleManualVy = 0;
cart.MultiVehicleManualVth = 0;
cart.MultiVehicleHold = false;
}
TriggerOnce(cart.TransmitterConnected && cart.Transmitter_SA, 700,
() =>
{
Console.WriteLine("遥控器使能!");
cart.TransmitterControlEnable = true;
cart.TransmitterLastTime = DateTime.Now;
LogTransmitterFleet("enable", true);
});
TriggerOnce(cart.TransmitterConnected && !cart.Transmitter_SA, 700,
() =>
{
Console.WriteLine("遥控器断使能!");
cart.TransmitterControlEnable = false;
LogTransmitterFleet("disable", true);
});
if (cart.TransmitterControlEnable && cart.Transmitter_SC == cart.CarNum)
{
CartDefinition.testPriority(5, "TransmitterMode");
ResetMultiVehicle("single-car-control");
LogTransmitterFleet("single-car-control");
TransmitterChassisControl();
cart.TransmitterLastTime = DateTime.Now;
cart.CarStatu = "Transmitter控制模式使能";
}
else if (cart.TransmitterControlEnable && cart.Transmitter_SC == 3)
{
cart.CarStatu = "Transmitter双车联动模式使能";
cart.MultiVehicleManualEnabled = true;
var fleetMode = 0;
if (cart.Transmitter_SB == TransmitterState.Mode0) fleetMode = 0;
else if (cart.Transmitter_SB == TransmitterState.Mode1) fleetMode = 1;
else if (cart.Transmitter_SB == TransmitterState.Mode2) fleetMode = 2;
cart.ApplyFleetManualCommand(fleetMode, -cart.TransmitterLeftJoystickValX,
cart.TransmitterRightJoystickValY, 1f);
// hold multi-vehicle sync, make speed 0
cart.MultiVehicleHold = cart.Transmitter_SD == TransmitterState.Mode1;
LogTransmitterFleet("fleet-control");
}
else
{
ResetMultiVehicle("idle-or-sc-mismatch");
LogTransmitterFleet("idle-or-sc-mismatch");
cart.CarStatu = "正常运行";
}
cart.ClumsyControl = true;
cart.LeftFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
cart.LeftRearPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
cart.RightFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
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 diffRf = cart.RightFrontPid.GetResponse(cart.ThRightFront, false, false, "RF");
var diffLr = cart.LeftRearPid.GetResponse(cart.ThLeftRear, false, false, "LR");
var diffRr = cart.RightRearPid.GetResponse(cart.ThRightRear, false, false, "RR");
// Hedingben.ToastText($"LF:{diffLf:0.00},LR:{diffLr:0.00},RF:{diffRf:0.00},RR:{diffRr:0.00}", "wheel-pid");
DLog.Log($"[wheel pid] {cart.ActualThLeftFront:0.00}->{cart.ThLeftFront:0.00} diff:{CommonMath.ThDiff(cart.ThLeftFront, cart.ActualThLeftFront):0.00} pid:{diffLf:0.00}");
cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
List<float> wheelSpeed = new float[]
{
Math.Abs(cart.SpeedLFL), Math.Abs(cart.SpeedLFR), Math.Abs(cart.SpeedRFL),
Math.Abs(cart.SpeedRFR), Math.Abs(cart.SpeedLRL),Math.Abs(cart.SpeedLRR), Math.Abs(cart.SpeedRRL),
Math.Abs(cart.SpeedRRR)
}.ToList();
var wheelMaxSpeed = wheelSpeed.Max();
var speedSign = Math.Sign(cart.ThresSpeed - cart.SendThresSpeed);
var acc = Math.Abs(cart.ThresSpeed) > Math.Abs(cart.SendThresSpeed) ? cart.Chassis.AccPerSecond : cart.Chassis.DeAccPerSecond;
cart.SendThresSpeed += speedSign * Math.Min(Math.Abs(cart.ThresSpeed - cart.SendThresSpeed),
acc * (float)(DateTime.Now - _lastMoveTime).TotalSeconds);
if (cart.ThresSpeed < wheelMaxSpeed && cart.SendThresSpeed > wheelMaxSpeed)
cart.SendThresSpeed = Math.Min(cart.SendThresSpeed, wheelMaxSpeed);
_lastMoveTime = DateTime.Now;
//灯光控制
if (cart.AlarmLevel == 2)
{
//故障红灯常亮
cart.LightMode = 2;
}
else if (cart.ThresSpeed != 1)
{
//避障黄灯常亮
cart.LightMode = 3;
}
else if (!cart.WheelAbleState)
{
FlipFlop(ref cart.LightMode, 500, 0, 3);
}
else if (cart.Soc < cart.LowBatteryAlarmThreshold && cart.ElectricCurrent >= -10)
{
cart.LightMode = 3;
}
else if (cart.ElectricCurrent < -10)
{
FlipFlop(ref cart.LightMode, 500, 0, 1);
}
else
{
FlipFlop(ref cart.LightMode, 500, 0, 1);
}
}
public void TransmitterChassisControl()
{
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode0, 100,
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Normal);
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode1, 100,
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Crab);
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode2, 100,
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Spin);
Hedingben.ToastText($"X方向速度: {cart.TransmitterLeftJoystickValX} Y方向速度: {cart.TransmitterRightJoystickValY}");
if (cart.Transmitter_SD == TransmitterState.Mode1)
{
if(cart.Transmitter_T3 == 1694)
cart.WheelReset();
else if(cart.Transmitter_T3 == 151)
cart.WheelDisable();
}
if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValXRaw == 353)
{
TriggerOnce(cart.TransmitterRightJoystickValYRaw == 353, 100, () => cart.TransmitterSpeed += 0.1f);
TriggerOnce(cart.TransmitterRightJoystickValYRaw == 1694, 100, () => cart.TransmitterSpeed -= 0.1f);
}
cart.TransmitterSpeed = Math.Max(Math.Min(cart.TransmitterSpeed, cart.TransmitterSpeedUpperLimit), cart.TransmitterSpeedLowerLimit);
//cart.TransmitterSpeed = Math.Max(cart.TransmitterSpeed, cart.TransmitterSpeedLowerLimit);
if (!cart.Transmitter_SA)
{
cart.ManualControl(cart.TransmitterControlMode, 0,
0, 0, cart.TransmitterSpeed,
DateTime.Now - cart.TransmitterLastTime);
cart.SpeedLeftArm = 0;
cart.SpeedRightArm = 0;
}
//摇杆控制底盘
else if (cart.Transmitter_SD == TransmitterState.Mode0)
{
cart.ManualControl(cart.TransmitterControlMode, cart.TransmitterLeftJoystickValX,
cart.TransmitterRightJoystickValY, 0, cart.TransmitterSpeed,
DateTime.Now - cart.TransmitterLastTime);
cart.SpeedLeftArm = 0;
cart.SpeedRightArm = 0;
}
//右摇杆同时控制左右夹臂
else if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValX == 0)
{
cart.ManualControl(cart.TransmitterControlMode, 0,
0, 0, cart.TransmitterSpeed,
DateTime.Now - cart.TransmitterLastTime);
cart.SpeedLeftArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
cart.SpeedRightArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
}
}
}
}
// 计算8个驱动电机的目标速度和舵角PID