421 lines
24 KiB
C#
421 lines
24 KiB
C#
using DG.Tweening;
|
||
using System;
|
||
using System.Collections;
|
||
using System.Collections.Generic;
|
||
using System.Net;
|
||
using System.Net.Sockets;
|
||
using System.Threading.Tasks;
|
||
using UnityEngine;
|
||
using UnityEngine.UIElements;
|
||
|
||
class MBOXCXRegAddress
|
||
{
|
||
//MBOX CX Reg Address
|
||
public static int CX_ID_MboxPlayControlWord = 0x0; //平台复位寄存器
|
||
}
|
||
|
||
//UDP指令的功能码
|
||
class UDPDataFunctionCode
|
||
{
|
||
//Read Reg Object Channel Code
|
||
public static int FunctionCode_Report_DnReg = 0x1001; //MBOX上报功能码
|
||
public static int FunctionCode_Read_DnFnReg = 0x1101; //读寄存器功能码
|
||
public static int FunctionCode_Write_DnFnReg = 0x1201; //写寄存器功能码
|
||
public static int FunctionCode_AbsTimePlay = 0x1301; //绝对时间播放
|
||
public static int FunctionCode_DeltaTimePlay = 0x1401; //相对时间播放
|
||
}
|
||
|
||
//UDP指令的通道号
|
||
class UDPDataObiectChannel
|
||
{
|
||
//Read Reg Object Channel Code
|
||
public static int ChannelCode_Read_DnReg = 0; //读Dn寄存器通道
|
||
public static int ChannelCode_Read_FnReg = 1; //读Fn寄存器通道
|
||
|
||
//Write Reg Object Channel Code
|
||
public static int ChannelCode_Write_FnReg_NOSave = 0; //写Fn寄存器但掉电不保存通道
|
||
public static int ChannelCode_Write_FnReg_Save = 1; //写Fn寄存器掉电保存通道
|
||
public static int ChannelCode_Write_CXReg = 2; //写CX寄存器通道
|
||
|
||
//Play Action Data（六轴数据支持1-6轴平台，建议在编程时直接使用6轴数据）
|
||
public static int ChannelCode_Play_ActionData_Three = 0; //播放3轴数据通道
|
||
public static int ChannelCode_Play_ActionData_Six = 1; //播放6轴数据通道
|
||
}
|
||
|
||
//UDP指令里的字偏移量
|
||
class UDPDataWordOffset
|
||
{
|
||
// UDP Tx Buffer WordOffset
|
||
public static int WordOffset000_UdpApp_ConfirmCode = 0;
|
||
public static int WordOffset001_UdpApp_PassWord = 1;
|
||
public static int WordOffset002_UdpApp_FunctionCode = 2;
|
||
public static int WordOffset003_UdpApp_ObjectChannel = 3;
|
||
public static int WordOffset004_UdpApp_WhoAcceptCode = 4;
|
||
public static int WordOffset005_UdpApp_WhoReplyCode = 5;
|
||
|
||
public static int WordOffset006_UdpApp_RegStartAddress = 6;
|
||
public static int WordOffset007_UdpApp_RegVisitNum = 7;
|
||
public static int WordOffset008_UdpApp_RegVisitDataBase = 8;
|
||
public static int WordOffset009_UdpApp_RegVisitErrorCode = 9;
|
||
|
||
public static int WordOffset006_UdpApp_PlayLineHigh = 6;
|
||
public static int WordOffset007_UdpApp_PlayLineLow = 7;
|
||
public static int WordOffset008_UdpApp_PlayTimeHigh = 8;
|
||
public static int WordOffset009_UdpApp_PlayTimeLow = 9;
|
||
|
||
public static int WordOffset010_UdpApp_PlayXPosHigh = 10;
|
||
public static int WordOffset011_UdpApp_PlayXPosLow = 11;
|
||
|
||
public static int WordOffset012_UdpApp_PlayYPosHigh = 12;
|
||
public static int WordOffset013_UdpApp_PlayYPosLow = 13;
|
||
|
||
public static int WordOffset014_UdpApp_PlayZPosHigh = 14;
|
||
public static int WordOffset015_UdpApp_PlayZPosLow = 15;
|
||
|
||
public static int WordOffset016_UdpApp_PlayUPosHigh = 16;
|
||
public static int WordOffset017_UdpApp_PlayUPosLow = 17;
|
||
|
||
public static int WordOffset018_UdpApp_PlayVPosHigh = 18;
|
||
public static int WordOffset019_UdpApp_PlayVPosLow = 19;
|
||
|
||
public static int WordOffset020_UdpApp_PlayWPosHigh = 20;
|
||
public static int WordOffset021_UdpApp_PlayWPosLow = 21;
|
||
|
||
public static int WordOffset022_UdpApp_PlayBaseDo = 22;
|
||
public static int WordOffset023_UdpApp_PlayDac1 = 23;
|
||
public static int WordOffset024_UdpApp_PlayDac2 = 24;
|
||
public static int WordOffset025_UdpApp_PlayExtDo = 25;
|
||
}
|
||
|
||
|
||
class ELECTRIC_CYCLINDER
|
||
{
|
||
public float F32AccessDistanceUnitMm = 50f; //电动缸行程
|
||
public float F32LeadDistanceUnitMm = 5f; //电动缸导程
|
||
public uint U32OneTurnPulseNum = 10000; //一圈脉冲数
|
||
public float F32Gear = 1f; //减速比
|
||
}
|
||
|
||
// Udp App Data Structure
|
||
class UDP_APP_DATA
|
||
{
|
||
public short I16HostTxPort = 8410;
|
||
public short I16HostRxPort = 8410;
|
||
public short I16MboxTxPort = 7408;
|
||
public short I16MboxRxPort = 7408;
|
||
|
||
public ushort I16ConfirmCode = 0x55aa; //确认码
|
||
public ushort I16PassCode = 0x0000; //通过码
|
||
public ushort I16FunctionCode = 0x1401; //功能码：相对对时间码
|
||
public ushort I16ChannelCode = 0x0001; //通道号：六轴模式
|
||
|
||
public ushort I16WhoAcceptCode = 0xffff; //谁接收
|
||
public ushort I16WhoReplyCode = 0xffff; //谁回复
|
||
|
||
public int I32PlayLine = 0x00000000; //指令编号
|
||
public int I32PlayTime = 0x00000000; //时间（单位：毫秒，1401相对时间为指令的执行时间，1301绝对时间下相邻两帧指令里的时间相减为执行时间）
|
||
public int I32PlayXpos = 0x00000000; //第1轴位置信息
|
||
public int I32PlayYpos = 0x00000000; //第2轴位置信息
|
||
public int I32PlayZpos = 0x00000000; //第3轴位置信息
|
||
public int I32PlayUpos = 0x00000000; //第4轴位置信息
|
||
public int I32PlayVpos = 0x00000000; //第5轴位置信息
|
||
public int I32PlayWpos = 0x00000000; //第6轴位置信息
|
||
|
||
public ushort I16BaseDoutCode = 0x0000; //基础开关量输出
|
||
|
||
public ushort I16DacOneCode = 0x0000; //第1路模拟量输出
|
||
public ushort I16DacTwoCode = 0x0000; //第2路模拟量输出
|
||
public ushort I16ExtDoutCode = 0x0000; //扩展12路模拟量输出
|
||
}
|
||
|
||
//Sine Parameter
|
||
class SINE_ACT_DATA
|
||
{
|
||
public float F32SamplingPeriods = 0.01f;
|
||
|
||
public float F32PeakMm = 50.0f;
|
||
public float F32FrequencyHz = 0.1f;
|
||
|
||
public float F32SineDataMm = 0.0f;
|
||
|
||
public float F32XMm = 0.0f;
|
||
public float F32YMm = 0.0f;
|
||
public float F32ZMm = 0.0f;
|
||
public float F32UMm = 0.0f;
|
||
public float F32VMm = 0.0f;
|
||
public float F32WMm = 0.0f;
|
||
|
||
public short I16Delta = 0;
|
||
public int I32Iine = 0;
|
||
|
||
public int I32CyclePoint = 0;
|
||
}
|
||
|
||
public class MBOXManager : MonoBehaviour
|
||
{
|
||
public UdpClient HostUDPPort; //打开8410端口
|
||
public short[] I16UdpTxBuffer; //存放UDP数据信息
|
||
public byte[] U8UdpDataSend; //存放发送数据Byte信息
|
||
//定时器
|
||
|
||
ELECTRIC_CYCLINDER ElectricCylinder;
|
||
UDP_APP_DATA UdpAppData;
|
||
SINE_ACT_DATA SineActData = new SINE_ACT_DATA();
|
||
|
||
|
||
public static MBOXManager Instance;
|
||
|
||
void Awake()
|
||
{
|
||
Instance = this;
|
||
}
|
||
|
||
/// <summary>
|
||
/// X,U 右边
|
||
/// Y,V 前面
|
||
/// Z,W 左边
|
||
/// </summary>
|
||
void Start()
|
||
{
|
||
UdpAppData = new UDP_APP_DATA();
|
||
ElectricCylinder = new ELECTRIC_CYCLINDER();
|
||
|
||
I16UdpTxBuffer = new short[25];
|
||
U8UdpDataSend = new byte[50];
|
||
|
||
|
||
UdpAppData.I16HostTxPort = (short)8413; //主机 发送UDP数据端口号
|
||
UdpAppData.I16MboxRxPort = (short)7408; //MBOX 接收UDP数据端口号
|
||
UdpAppData.I16WhoAcceptCode = (ushort)ByteToWord((int)255, (int)255); //IP Group和IP Node合并为谁接收
|
||
UdpAppData.I16WhoReplyCode = (ushort)ByteToWord((int)255, (int)255); //IP Group和IP Node合并为谁接收
|
||
ElectricCylinder.F32AccessDistanceUnitMm = (float)33; //电动缸行程
|
||
ElectricCylinder.F32LeadDistanceUnitMm = (float)5.00; //电动缸导程
|
||
ElectricCylinder.U32OneTurnPulseNum = (uint)10000; //一圈脉冲数
|
||
ElectricCylinder.F32Gear = (float)1.0000; //减速比
|
||
HostUDPPort = new UdpClient(UdpAppData.I16HostTxPort);
|
||
IPEndPoint ipendpoint = new IPEndPoint(IPAddress.Any, UdpAppData.I16MboxRxPort);
|
||
HostUDPPort.Connect("255.255.255.255", UdpAppData.I16MboxRxPort);
|
||
|
||
|
||
Debug.Log(HostUDPPort.Client);
|
||
|
||
UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
|
||
UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
|
||
UdpAppData.I32PlayTime = 100; //时间单位为毫秒
|
||
UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
PlayActionCmd();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 撞击
|
||
/// </summary>
|
||
public async void Bonk()
|
||
{
|
||
UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
|
||
UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
|
||
UdpAppData.I32PlayTime = 100; //时间单位为毫秒
|
||
UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(20), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(20), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
PlayActionCmd();
|
||
|
||
await Task.Delay(TimeSpan.FromSeconds(0.2f));
|
||
|
||
UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
|
||
UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
|
||
UdpAppData.I32PlayTime = 100; //时间单位为毫秒
|
||
UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(60), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(60), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
PlayActionCmd();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 左边
|
||
/// </summary>
|
||
/// <param name="val"></param>
|
||
public void Left(float val)
|
||
{
|
||
if (val >= 20)
|
||
return;
|
||
|
||
UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
|
||
UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
|
||
UdpAppData.I32PlayTime = 100; //时间单位为毫秒
|
||
UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm - val), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm - val), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
PlayActionCmd();
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 右边
|
||
/// </summary>
|
||
/// <param name="val"></param>
|
||
public void Right(float val)
|
||
{
|
||
if (val >= 20)
|
||
return;
|
||
UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
|
||
UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
|
||
UdpAppData.I32PlayTime = 100; //时间单位为毫秒
|
||
UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm - val), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm - val), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
PlayActionCmd();
|
||
}
|
||
|
||
|
||
//public void A()
|
||
//{
|
||
// Debug.Log((ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay);
|
||
// UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
|
||
// UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
|
||
// UdpAppData.I32PlayTime = 100;//时间单位为毫秒
|
||
// UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// PlayActionCmd();
|
||
//}
|
||
//public void B()
|
||
//{
|
||
// UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
|
||
// UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
|
||
// UdpAppData.I32PlayTime = 100;//时间单位为毫秒
|
||
// UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// PlayActionCmd();
|
||
//}
|
||
|
||
//public void C()
|
||
//{
|
||
// UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
|
||
// UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
|
||
// UdpAppData.I32PlayTime = 100;//时间单位为毫秒
|
||
// UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
|
||
// PlayActionCmd();
|
||
//}
|
||
|
||
private void PlayActionCmd()
|
||
{
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset000_UdpApp_ConfirmCode] = (short)Swap(UdpAppData.I16ConfirmCode); //确认码
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset001_UdpApp_PassWord] = (short)Swap(UdpAppData.I16PassCode); //通过码
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset002_UdpApp_FunctionCode] = (short)Swap(UdpAppData.I16FunctionCode); //功能码
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset003_UdpApp_ObjectChannel] = (short)Swap(UdpAppData.I16ChannelCode); //通道号
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset004_UdpApp_WhoAcceptCode] = (short)Swap(UdpAppData.I16WhoAcceptCode); //谁接收
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset005_UdpApp_WhoReplyCode] = (short)Swap(UdpAppData.I16WhoReplyCode); //谁回复
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset006_UdpApp_PlayLineHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayLine); // 指令编号
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset007_UdpApp_PlayLineLow] = (short)U32LowWord((uint)UdpAppData.I32PlayLine);
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset008_UdpApp_PlayTimeHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayTime); //时间
|
||
|
||
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset009_UdpApp_PlayTimeLow] = (short)U32LowWord((uint)UdpAppData.I32PlayTime);
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset010_UdpApp_PlayXPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayXpos); //X位置
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset011_UdpApp_PlayXPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayXpos);
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset012_UdpApp_PlayYPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayYpos); //Y位置
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset013_UdpApp_PlayYPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayYpos);
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset014_UdpApp_PlayZPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayZpos); //Z位置
|
||
|
||
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset015_UdpApp_PlayZPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayZpos);
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset016_UdpApp_PlayUPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayUpos); //U位置
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset017_UdpApp_PlayUPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayUpos);
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset018_UdpApp_PlayVPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayVpos); //V位置
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset019_UdpApp_PlayVPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayVpos);
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset020_UdpApp_PlayWPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayWpos); //W位置
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset021_UdpApp_PlayWPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayWpos);
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset022_UdpApp_PlayBaseDo] = (short)Swap(UdpAppData.I16BaseDoutCode); //基本开关量
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset023_UdpApp_PlayDac1] = (short)Swap(UdpAppData.I16DacOneCode); // 模拟量1输出
|
||
I16UdpTxBuffer[UDPDataWordOffset.WordOffset024_UdpApp_PlayDac2] = (short)Swap(UdpAppData.I16DacTwoCode); // 模拟量2输出
|
||
Buffer.BlockCopy(I16UdpTxBuffer, 0, U8UdpDataSend, 0, U8UdpDataSend.Length); // 将short数组转化为byte数组
|
||
HostUDPPort.BeginSend(U8UdpDataSend, (UDPDataWordOffset.WordOffset024_UdpApp_PlayDac2 + 1) * 2, (state => { Debug.Log(state.IsCompleted); }), ""); //发送UDP数据
|
||
}
|
||
|
||
//将两个char数据合并为字节数据
|
||
private int ByteToWord(int x, int y)
|
||
{
|
||
int z;
|
||
z = ((x << 8) | y);
|
||
return z;
|
||
}
|
||
|
||
//将short类型数据的高低位进行交换
|
||
private ushort Swap(ushort x)
|
||
{
|
||
ushort highbit;
|
||
ushort lowbit;
|
||
ushort swap;
|
||
highbit = (ushort)(x >> 8);
|
||
lowbit = (ushort)((x & 0x00ff) << 8);
|
||
swap = (ushort)(highbit | lowbit);
|
||
return swap;
|
||
}
|
||
|
||
///取Uint32的高１６位，并将Uint16的高８位和低８位交换
|
||
private ushort U32HighWord(uint x)
|
||
{
|
||
ushort highbit;
|
||
|
||
highbit = (ushort)(x >> 16);
|
||
highbit = Swap(highbit);
|
||
|
||
return (highbit);
|
||
}
|
||
|
||
///取Uint32的低１６位，并将Uint16的高８位和低８位交换
|
||
private ushort U32LowWord(uint x)
|
||
{
|
||
ushort lowbit;
|
||
lowbit = (ushort)(x & 0x0000ffff);
|
||
lowbit = Swap(lowbit);
|
||
return (lowbit);
|
||
}
|
||
|
||
///计算点动脉冲数
|
||
private uint JogPulse(float Gear, float AcessDistance, float LeadDistance, uint OneTurnPulse)
|
||
{
|
||
uint NUM_Pulse;
|
||
NUM_Pulse = (uint)(Gear * AcessDistance * OneTurnPulse / LeadDistance);
|
||
return NUM_Pulse;
|
||
}
|
||
|
||
//正弦波形
|
||
private float SineDataMm(float peak, float frequency, float t)
|
||
{
|
||
float sinedata;
|
||
|
||
sinedata = (float)((peak / 2) + (peak / 2) * Math.Sin(2 * Math.PI * frequency * t + 270 * Math.PI / 180));
|
||
|
||
return sinedata;
|
||
}
|
||
} |