/// <summary>
/// 伺服运动模式设置并开始
/// </summary>
/// <param name="EnableMoveDirections">允许的移动方向</param>
/// <param name="MaxSpeed">单轴移动最大速度</param>
/// <param name="MinSpeed">单轴移动最小速度</param>
/// <param name="MaxForce">单轴允许最大输入力</param>
/// <param name="MinForce">单轴允许最小输入力</param>
/// <param name="StopDistance">移动最远距离</param>
/// <param name="IfVibrate">是否在移动时摆动姿态</param>
/// <param name="VibrateAxis">姿态摆动轴</param>
/// <param name="VibrateAngle">姿态摆动角</param>
/// <param name="ControlPeriod">伺服运动周期</param>
/// <param name="LookAheadTime">伺服运动预计时间</param>
/// <param name="Gain">伺服运动增益</param>
public void ServoMotionSetAndBegin(ServoDirectionAtTcp EnableMoveDirections,
double MaxSpeed,
double MinSpeed,
double MaxForce,
double MinForce,
double StopDistance,
bool IfVibrate,
ServoDirectionAtTcp VibrateAxis,
double VibrateAngle,
double ControlPeriod = 0.008,
double LookAheadTime = 0.1,
double Gain = 200)
{
// 设置可移动的方向
servoMotionIfDirectionXEnabled = EnableMoveDirections.HasFlag(ServoDirectionAtTcp.DirectionX);
servoMotionIfDirectionYEnabled = EnableMoveDirections.HasFlag(ServoDirectionAtTcp.DirectionY);
servoMotionIfDirectionZEnabled = EnableMoveDirections.HasFlag(ServoDirectionAtTcp.DirectionZ);
// 设置移动的最大和最小速度，以及对应的力值
servoMotionMaxIncrement = MaxSpeed;
servoMotionMinIncrement = MinSpeed;
servoMotionMaxAvailableForce = MaxForce;
servoMotionMinAvailableForce = MinForce;
// 设置能够移动的最远距离
servoMotionStopDistance = StopDistance;
// 设置摆动的相关条件
servoMotionIfAttitudeVibrate = IfVibrate;
servoMotionAttitudeVibrateDirection = VibrateAxis;
servoMotionVibrateAngle = VibrateAngle;
// 初始化力保持的值
servoMotionPreservedForce[0] = 0.0;
servoMotionPreservedForce[1] = 0.0;
servoMotionPreservedForce[2] = 0.0;
// 设置伺服参数并重写下位机控制程序
SetServoMotionParameters(ControlPeriod, LookAheadTime, Gain);
internalProcessor.WriteStringToControlCode(ControlPeriod, LookAheadTime, Gain);
// 打开伺服模块时对一般逻辑的处理
internalProcessor.ServoSwitchMode(true);
// 开始本伺服模式
ServoMotionBegin();
}
/// <summary>
/// 伺服运动模块设置并开始
/// </summary>
/// <param name="MoveDirectionAtTcp">相对Tcp坐标系的既定运动方向</param>
/// <param name="DetectDirectionAtTcp">相对Tcp坐标系的既定力保持方向</param>
/// <param name="tcpCurrentPosition">实时Tcp坐标</param>
/// <param name="MainStopDirectionAtTcp">相对Tcp坐标系的既定主运动停止方向</param>
/// <param name="SubStopDirectionAtTcp">相对Tcp坐标系的既定副运动停止方向</param>
/// <param name="MainStopDirectionStopDistance">在主运动方向上的终止距离</param>
/// <param name="SubStopDirectionStopDistance">在副运动方向上的终止距离</param>
/// <param name="SubStopDirectionRecurrentStopDistance">在副运动方向上的回环检查的终止距离</param>
/// <param name="StopWay">使用的终止条件</param>
/// <param name="ForwardSpeed">在运动方向上的速度</param>
/// <param name="ForceMaxSpeed">在力保持方向上的最大速度</param>
/// <param name="ForceMinSpeed">在力保持方向上的最小速度</param>
/// <param name="ForceMaxValue">在力保持方向上的最大可接受力值</param>
/// <param name="ForceMinValue">在力保持方向上的最小可接受力值</param>
/// <param name="VibrateMaxValue">在摆动方向上的最大速度</param>
/// <param name="VibrateForceToAngle">在摆动方向上的力误差对应的角度偏差</param>
/// <param name="VibrateAngleToForce">在摆动方向上的角度误差对应的力偏差</param>
/// <param name="TargetForceKeep">目标保持力</param>
/// <param name="IfRotate">是否改变姿态</param>
/// <param name="UpAngle">姿态改变角度上限</param>
/// <param name="IfFindInitialAngle">是否寻找初始姿态角</param>
/// <param name="IfRecorrectRotateAngle">是否矫正姿态角</param>
/// <param name="JudgeStep">矫正步长，隔多少距离矫正一次</param>
/// <param name="IgnoreJudgeSpan">忽略矫正长度，开始的时候隔多少距离开始进入矫正状态</param>
/// <param name="OverlappingRate">矫正采样重叠范围</param>
/// <param name="ControlPeriod">伺服运动周期</param>
/// <param name="LookAheadTime">伺服运动预计时间</param>
/// <param name="Gain">伺服运动增益</param>
public void ServoMotionSetAndBegin(ServoDirectionAtTcp MoveDirectionAtTcp,
ServoDirectionAtTcp DetectDirectionAtTcp,
double[] tcpCurrentPosition,
ServoDirectionAtTcp MainStopDirectionAtTcp,
ServoDirectionAtTcp SubStopDirectionAtTcp,
double MainStopDirectionStopDistance,
double SubStopDirectionStopDistance,
double SubStopDirectionRecurrentStopDistance,
ServoStopMode StopWay,
double ForwardSpeed,
double ForceMaxSpeed,
double ForceMinSpeed,
double ForceMaxValue,
double ForceMinValue,
double VibrateMaxValue,
double VibrateForceToAngle,
double VibrateAngleToForce,
double TargetForceKeep,
bool IfRotate,
double UpAngle,
bool IfFindInitialAngle = false,
bool IfRecorrectRotateAngle = false,
double IgnoreJudgeSpan = 0.001,
double JudgeStep = 0.003,
double OverlappingRate = 0.5,
double ControlPeriod = 0.008,
double LookAheadTime = 0.1,
double Gain = 200)
{
// 设定运动和力保持方向
servoMotionMovingDirectionAtTcp = MoveDirectionAtTcp;
servoMotionDetectingDirectionAtTcp = DetectDirectionAtTcp;
// 获得运动和力保持方向的Tcp系和Base系表示
double[] tcpToBase = URMath.ReverseReferenceRelationship(tcpCurrentPosition);
Quatnum qTcpToBase = URMath.AxisAngle2Quatnum(new double[] { tcpToBase[3], tcpToBase[4], tcpToBase[5] });
switch (servoMotionMovingDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
servoMotionMovingDirectionArrayAtTcp = new double[] { 1.0, 0.0, 0.0 };
break;
case ServoDirectionAtTcp.NegativeX:
servoMotionMovingDirectionArrayAtTcp = new double[] { -1.0, 0.0, 0.0 };
break;
case ServoDirectionAtTcp.PositiveY:
servoMotionMovingDirectionArrayAtTcp = new double[] { 0.0, 1.0, 0.0 };
break;
case ServoDirectionAtTcp.NegativeY:
servoMotionMovingDirectionArrayAtTcp = new double[] { 0.0, -1.0, 0.0 };
break;
case ServoDirectionAtTcp.PositiveZ:
servoMotionMovingDirectionArrayAtTcp = new double[] { 0.0, 0.0, 1.0 };
break;
case ServoDirectionAtTcp.NegativeZ:
servoMotionMovingDirectionArrayAtTcp = new double[] { 0.0, 0.0, -1.0 };
break;
default:
break;
}
servoMotionInitialMovingDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(servoMotionMovingDirectionArrayAtTcp, qTcpToBase);
switch (servoMotionDetectingDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
case ServoDirectionAtTcp.NegativeX:
servoMotionDetectingDirectionArrayAtTcp = new double[] { 1.0, 0.0, 0.0 };
break;
case ServoDirectionAtTcp.PositiveY:
case ServoDirectionAtTcp.NegativeY:
servoMotionDetectingDirectionArrayAtTcp = new double[] { 0.0, 1.0, 0.0 };
break;
case ServoDirectionAtTcp.PositiveZ:
case ServoDirectionAtTcp.NegativeZ:
servoMotionDetectingDirectionArrayAtTcp = new double[] { 0.0, 0.0, 1.0 };
break;
default:
break;
}
servoMotionInitialDetectingDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(servoMotionDetectingDirectionArrayAtTcp, qTcpToBase);
// 设定摆动方向
servoMotionVibratingDirectionArrayAtTcp = URMath.VectorCrossMultiply(servoMotionMovingDirectionArrayAtTcp, servoMotionDetectingDirectionArrayAtTcp);
servoMotionInitialVibratingDirectionArrayAtBase = URMath.VectorCrossMultiply(servoMotionInitialMovingDirectionArrayAtBase, servoMotionInitialDetectingDirectionArrayAtBase);
// 设定主运动停止方向和副运动停止方向
servoMotionMainStopDirectionAtTcp = MainStopDirectionAtTcp;
servoMotionSubStopDirectionAtTcp = SubStopDirectionAtTcp;
// 获得主运动和副运动停止方向的Base系表示
switch (servoMotionMainStopDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeX:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { -1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveY:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeY:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, -1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveZ:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, 1.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeZ:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, -1.0 }, qTcpToBase);
break;
default:
break;
}
switch (servoMotionSubStopDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeX:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { -1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveY:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeY:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, -1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveZ:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, 1.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeZ:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, -1.0 }, qTcpToBase);
break;
default:
break;
}
// 设定主运动和副运动停止方向的最大行进距离、回环停止距离以及停止条件的选择
servoMotionMainStopDirectionStopDistance = MainStopDirectionStopDistance;
servoMotionSubStopDirectionStopDistance = SubStopDirectionStopDistance;
servoMotionSubStopDirectionRecurrentCheckStopDistance = SubStopDirectionRecurrentStopDistance;
servoMotionActiveDistanceOrRecurrentCondition = StopWay;
// 设定运动速度
servoMotionMovingPeriodicalIncrement = ForwardSpeed;
// 设定力保持方向速度和力限制
servoMotionDetectingPeriodicalMaxIncrement = ForceMaxSpeed;
servoMotionDetectingPeriodicalMinIncrement = ForceMinSpeed;
servoMotionDetectingMaxAvailableForce = ForceMaxValue;
servoMotionDetectingMinAvailableForce = ForceMinValue;
// 设定摆动方向的速度限制
servoMotionVibratingPeriodicalMaxIncrement = VibrateMaxValue;
// 设定摆动方向的角度与力之间的转换关系
servoMotionFromFrictionToAngle = VibrateForceToAngle;
servoMotionFromAngleToFriction = VibrateAngleToForce;
// 设定目标保持力
servoMotionTargetForceKeep = TargetForceKeep;
// 设定姿态是否更改，以及更改的上下限
servoMotionIfAttitudeChange = IfRotate;
servoMotionUpBoundAngle = UpAngle;
// 设定是否寻找初始姿态角
servoMotionIfFindInitialAngle = IfFindInitialAngle;
// 设定是否矫正姿态角
servoMotionIfRecorrectRotateAngle = IfRecorrectRotateAngle;
servoMotionIgnoreJudgeSpan = IgnoreJudgeSpan;
servoMotionJudgeStep = JudgeStep;
servoMotionOverlappingRate = OverlappingRate;
// 设置伺服参数并重写下位机控制程序
SetServoMotionParameters(ControlPeriod, LookAheadTime, Gain);
internalProcessor.WriteStringToControlCode(ControlPeriod, LookAheadTime, Gain);
// 准备查找初始姿态角
RotateAboutInitialAngle();
}
/// <summary>
/// 伺服运动模块设置并开始
/// </summary>
/// <param name="MoveDirectionAtTcp">相对Tcp坐标系的既定运动方向</param>
/// <param name="DetectDirectionAtTcp">相对Tcp坐标系的既定力保持方向</param>
/// <param name="tcpCurrentPosition">实时Tcp坐标</param>
/// <param name="MoveDirectionStopDistance">在运动方向上的终止距离</param>
/// <param name="DetectDirectionStopDistance">在力保持方向上的终止距离</param>
/// <param name="DetectDirectionRecurrentStopDistance">在力保持方向上的回环检查的终止距离</param>
/// <param name="StopWay">使用的终止条件</param>
/// <param name="ForwardSpeed">在运动方向上的速度</param>
/// <param name="ForceMaxSpeed">在力保持方向上的最大速度</param>
/// <param name="ForceMinSpeed">在力保持方向上的最小速度</param>
/// <param name="ForceMaxValue">在力保持方向上的最大可接受力值</param>
/// <param name="ForceMinValue">在力保持方向上的最小可接受力值</param>
/// <param name="IfRotate">是否改变姿态</param>
/// <param name="ThatAngle">终点姿态改变角度</param>
/// <param name="RelyDirection">姿态改变依赖方向，可填m或者d表示运动方向或者力保持方向</param>
/// <param name="ControlPeriod">伺服运动周期</param>
/// <param name="LookAheadTime">伺服运动预计时间</param>
/// <param name="Gain">伺服运动增益</param>
public void ServoMotionSetAndBegin(ServoDirectionAtTcp MoveDirectionAtTcp,
ServoDirectionAtTcp DetectDirectionAtTcp,
double[] tcpCurrentPosition,
double MoveDirectionStopDistance,
double DetectDirectionStopDistance,
double DetectDirectionRecurrentStopDistance,
ServoStopMode StopWay,
double ForwardSpeed,
double ForceMaxSpeed,
double ForceMinSpeed,
double ForceMaxValue,
double ForceMinValue,
bool IfRotate,
double ThatAngle,
char RelyDirection,
double ControlPeriod = 0.008,
double LookAheadTime = 0.1,
double Gain = 200)
{
// 设定运动和力保持方向
servoMotionMovingDirectionAtTcp = MoveDirectionAtTcp;
servoMotionDetectingDirectionAtTcp = DetectDirectionAtTcp;
// 将运动和力保持方向转换到Base坐标系中
double[] tcpToBase = URMath.ReverseReferenceRelationship(tcpCurrentPosition);
Quatnum qTcpToBase = URMath.AxisAngle2Quatnum(new double[] { tcpToBase[3], tcpToBase[4], tcpToBase[5] });
double[] moveDirectionAtBase = new double[3];
double[] detectDirectionAtBase = new double[3];
switch (servoMotionMovingDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeX:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { -1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveY:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeY:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, -1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveZ:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, 1.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeZ:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, -1.0 }, qTcpToBase);
break;
default:
break;
}
switch (servoMotionDetectingDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
case ServoDirectionAtTcp.NegativeX:
detectDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveY:
case ServoDirectionAtTcp.NegativeY:
detectDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveZ:
case ServoDirectionAtTcp.NegativeZ:
detectDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, 1.0 }, qTcpToBase);
break;
default:
break;
}
// 伺服运动模块设置并开始
ServoMotionSetAndBegin(moveDirectionAtBase,
detectDirectionAtBase,
MoveDirectionStopDistance,
DetectDirectionStopDistance,
DetectDirectionRecurrentStopDistance,
StopWay,
ForwardSpeed,
ForceMaxSpeed,
ForceMinSpeed,
ForceMaxValue,
ForceMinValue,
IfRotate,
ThatAngle,
RelyDirection,
ControlPeriod,
LookAheadTime,
Gain);
}
