234 lines
12 KiB
C#
234 lines
12 KiB
C#
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;
|
|
}
|
|
}
|
|
}
|
|
}
|