92 lines
3.4 KiB
C#
92 lines
3.4 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using ClumsyCore.Pilot;
|
||
using MDCSToolBox.Commons.Controllers;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
public class ClampToTarget : MovementDefinition
|
||
{
|
||
public float LeftClampTarget;
|
||
public float RightClampTarget;
|
||
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
|
||
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
|
||
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
|
||
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
|
||
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
|
||
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
|
||
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||
public float TimeoutSeconds = 30f;
|
||
private PIDController leftpid, rightpid;
|
||
|
||
// C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
try
|
||
{
|
||
leftpid = new PIDController(
|
||
() => PilotDefinition.Self.ActualPosLeftArm,
|
||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||
ClampDeadZone, MaxClampSpeed)
|
||
{
|
||
SpeedAccPerSec = ClampSpeedAcc
|
||
};
|
||
|
||
rightpid = new PIDController(
|
||
() => PilotDefinition.Self.ActualPosRightArm,
|
||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||
ClampDeadZone, MaxClampSpeed)
|
||
{
|
||
SpeedAccPerSec = ClampSpeedAcc
|
||
};
|
||
|
||
var startTime = DateTime.UtcNow;
|
||
while (true)
|
||
{
|
||
if (TimeoutSeconds > 0f &&
|
||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||
TimeoutSeconds)
|
||
{
|
||
Console.WriteLine(
|
||
$"夹臂运动超时({TimeoutSeconds:F1}s)，" +
|
||
"停止左右夹臂。");
|
||
yield break;
|
||
}
|
||
|
||
var leftspeed =
|
||
leftpid.GetResponse(LeftClampTarget);
|
||
var rightspeed =
|
||
rightpid.GetResponse(RightClampTarget);
|
||
Console.WriteLine(
|
||
$"left arm speed:{leftspeed} " +
|
||
$"right arm speed:{rightspeed}");
|
||
|
||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||
|
||
var leftArrived = leftpid.IsArrived();
|
||
var rightArrived = rightpid.IsArrived();
|
||
if (leftArrived)
|
||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||
if (rightArrived)
|
||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||
|
||
if (leftArrived && rightArrived)
|
||
break;
|
||
|
||
yield return true;
|
||
}
|
||
|
||
Console.WriteLine(
|
||
$"left clamp to target:{LeftClampTarget} " +
|
||
$"right clamp to target:{RightClampTarget}");
|
||
}
|
||
finally
|
||
{
|
||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||
}
|
||
}
|
||
}
|
||
}
|