zhengxuan.zhang
1611 lines
55 KiB
C#
1611 lines
55 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.IO;
|
||
using System.Net;
|
||
using System.Runtime.InteropServices;
|
||
using System.Threading;
|
||
using System.Windows.Forms;
|
||
using ACS.SPiiPlusNET;
|
||
using HexcalMC.Base;
|
||
using HexcalMC.Form;
|
||
using HexcalMC.Hexcal;
|
||
using HexcalMC.Properties;
|
||
using Telerik.WinControls.UI;
|
||
using static HexcalMC.EtalonForm;
|
||
|
||
namespace HexcalMC
|
||
{
|
||
//定一个 回家状态枚举，包括 从未回家，正在回家，已经回家
|
||
public enum HomeStates
|
||
{
|
||
None, //默认状态
|
||
NotHome, //未回家
|
||
Homing, //回家中
|
||
Homed //回家完成
|
||
,
|
||
}
|
||
|
||
//定义 运动状态枚举，包括 正在运动，运动到位，Jog运动
|
||
public enum MotionStates
|
||
{
|
||
None, //默认状态
|
||
Moving, //运动中
|
||
InPos, //运动到位
|
||
Jogging //jog中
|
||
,
|
||
}
|
||
|
||
public partial class MainFrom : RadRibbonForm
|
||
{
|
||
private readonly List<Point3D> _pointCloud = new List<Point3D>(); //运动中点集合
|
||
|
||
private bool _mBHexcalConnected;
|
||
|
||
private TcpIpServer _mTcpIpServer; //创建tcpserver，用于接收hexcal传来的指令，并解析传递平台
|
||
|
||
private int m_nTotalAxis; //定义总轴数
|
||
private Axis[] m_arrAxisList = null;
|
||
|
||
//定一个运动到位次数的变量和三个方法，开始统计运动到位次数，停止统计运动到位次数，获取运动到位次数
|
||
|
||
#region 运动到位次数
|
||
|
||
|
||
// 记录运动到位次数的变量
|
||
private int m_nInPosCount = 0;
|
||
private bool isCounting = false;
|
||
private bool isInPose = false; //运动到位状态
|
||
|
||
// 开始统计运动到位次数
|
||
public void StartCounting()
|
||
{
|
||
isCounting = true;
|
||
m_nInPosCount = 0; // 重置计数器
|
||
}
|
||
|
||
// 停止统计运动到位次数
|
||
public void StopCounting()
|
||
{
|
||
isCounting = false;
|
||
m_nInPosCount = 0; // 重置计数器
|
||
}
|
||
|
||
// 获取运动到位次数
|
||
public int GetInPosCount()
|
||
{
|
||
return m_nInPosCount;
|
||
}
|
||
|
||
public bool GetIsMoving()
|
||
{
|
||
return isInPose;
|
||
}
|
||
#endregion
|
||
|
||
public MainFrom()
|
||
{
|
||
InitializeComponent();
|
||
|
||
// 处理未被捕获的线程异常
|
||
Application.ThreadException += Application_ThreadException;
|
||
|
||
// 处理未被捕获的非UI线程异常
|
||
AppDomain.CurrentDomain.UnhandledException += CurrentDomain_UnhandledException;
|
||
}
|
||
|
||
private void MainFrom_Load(object sender, EventArgs e)
|
||
{
|
||
FormBorderStyle = FormBorderStyle.FixedSingle; // 设置窗体边框样式为固定大小
|
||
MaximizeBox = false; // 禁用窗体的最大化按钮
|
||
DebugDfn.textBox_Msg = TextBoxMsg;
|
||
|
||
//加载配置文件
|
||
LoadConfig();
|
||
_acs = new Api(); //初始化 ACS运动控制类
|
||
|
||
//启动界面刷新
|
||
timer_RefreshUI.Start();
|
||
|
||
Point3D point3D = new Point3D(800, 980, -290);
|
||
IsWithinLimit(point3D);
|
||
|
||
}
|
||
|
||
private void MainFrom_Shown(object sender, EventArgs e) //窗体显示准备好接受用户输入时发生
|
||
{
|
||
////启动服务端,用于接收hexcal传来的指令
|
||
//StartServer();
|
||
|
||
//if (_enableAcs)
|
||
//{
|
||
// Btn_ACSStart_Click(null, null); //模拟连接运动平台
|
||
//}
|
||
}
|
||
|
||
private void MainFrom_FormClosed(object sender, FormClosedEventArgs e)
|
||
{
|
||
MyBase.TraceWriteLine("关闭程序");
|
||
DebugDfn._strEndTime = DateTime.Now.ToString("yyyy.MM.dd HH-mm-ss");
|
||
timer_RefreshUI.Stop();
|
||
|
||
Btn_StopServer_Click(null, null);
|
||
Btn_ACSStop_Click(null, null); //关闭ACS
|
||
|
||
string copyFileName =
|
||
DebugDfn.StrDebugSavePath
|
||
+ "\\Debug("
|
||
+ DebugDfn._strStartTime
|
||
+ " To "
|
||
+ DebugDfn._strEndTime
|
||
+ ")"
|
||
+ ".txt";
|
||
if (!File.Exists(DebugDfn.StrDebugSavePath))
|
||
{
|
||
//创建文件夹 DebugDfn.StrDebugSavePath
|
||
Directory.CreateDirectory(DebugDfn.StrDebugSavePath);
|
||
}
|
||
|
||
File.Copy(DebugDfn.StrDebugFile, copyFileName);
|
||
|
||
if (Errors.ErrorWrite != null)
|
||
Errors.ErrorWrite.Close();
|
||
if (Errors.OtherWrite != null)
|
||
Errors.OtherWrite.Close();
|
||
if (Errors.StatusWrite != null)
|
||
Errors.StatusWrite.Close();
|
||
}
|
||
|
||
private void LoadConfig() //加载配置文件
|
||
{
|
||
//判断配置文件是否存在
|
||
if (!File.Exists(StrConfigFile))
|
||
{
|
||
MessageBox.Show(
|
||
"配置文件不存在，请检查配置文件",
|
||
"异常",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Error
|
||
);
|
||
return;
|
||
}
|
||
|
||
MotionSpeedxy = FileIni.ReadDouble(StrConfigFile, "MOTOR", "MOTION_SPEEDXY"); //运动定位速度
|
||
MotionSpeedz = FileIni.ReadDouble(StrConfigFile, "MOTOR", "MOTION_SPEEDZ");
|
||
|
||
//正限位
|
||
XMaxstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "X_MAXSTROKESW");
|
||
YMaxstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Y_MAXSTROKESW");
|
||
ZMaxstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Z_MAXSTROKESW");
|
||
|
||
//负限位
|
||
XMinstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "X_MINSTROKESW");
|
||
YMinstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Y_MINSTROKESW");
|
||
ZMinstrokesw = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Z_MINSTROKESW");
|
||
|
||
port = FileIni.ReadInt(StrConfigFile, "MOTOR", "Port");
|
||
DebugDfn.AddLogText($"当前监听端口配置为: {port}");
|
||
DebugDfn.AddLogText($"当前xy运动速度配置为: {MotionSpeedxy}");
|
||
DebugDfn.AddLogText($"当前z运动速度配置为: {MotionSpeedz}");
|
||
}
|
||
|
||
private void Plot2D(List<Point3D> pointCloud)
|
||
{
|
||
// 清空画布
|
||
formsPlot1.Plot.Clear();
|
||
|
||
//pointCloud 是否为空
|
||
if (pointCloud.Count <= 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
List<double> dataX = new List<double>();
|
||
List<double> dataY = new List<double>();
|
||
|
||
foreach (Point3D point3D in pointCloud)
|
||
{
|
||
dataX.Add(point3D.X);
|
||
dataY.Add(point3D.Y);
|
||
}
|
||
|
||
formsPlot1.Plot.AddScatter(dataX.ToArray(), dataY.ToArray());
|
||
formsPlot1.Refresh();
|
||
}
|
||
|
||
#region 运动平台变量区
|
||
|
||
public Api _acs;
|
||
|
||
private const int MaxUiLimitCnt = 24;
|
||
private int _mNTotalAxis;
|
||
|
||
//private int _mNTotalBuffer = 0;
|
||
//private Axis[] _mArrAxisList = null;
|
||
public bool _mAcsConnected; //ACS通讯状态
|
||
|
||
// For update values
|
||
private MotorStates _mNMotorState; //运动状态
|
||
|
||
private ProgramStates _mNProgramState; //程序状态
|
||
private object _mObjReadVar;
|
||
private Array _mArrReadVector;
|
||
private double _mLfRPos,
|
||
_mLfFPos,
|
||
_mLfPe,
|
||
_mLfFvel; //参考位置，反馈位置 位置误差 反馈速度 double类型
|
||
private int _mNValues,
|
||
_mNOutputState;
|
||
|
||
private Label[] _mLblLeftLimit; //左限位
|
||
private Label[] _mLblRightLimit; //右限位
|
||
private Label[] _mlblMoving; //运动中
|
||
private Label[] _mlblAcc; //加速中
|
||
private Label[] _mlblInPos; //轴就位
|
||
private Label[] _mlblEnable; //使能
|
||
private bool[] axisEnabled = new bool[MaxUiLimitCnt]; //轴使能状态
|
||
public bool totalAxisEnabled = false;
|
||
|
||
private HomeStates _homeStates; //回家状态
|
||
private MotionStates _currentMotionState; //当前运动状态
|
||
private MotionStates _currentMotorStateLast;
|
||
private readonly int _motionTimeout = 50000; //定义运动超时时间
|
||
|
||
//定义启用的轴，后面运动时会使用
|
||
public static Axis[] UseAxis =
|
||
{
|
||
Axis.ACSC_AXIS_1,
|
||
Axis.ACSC_AXIS_0,
|
||
Axis.ACSC_AXIS_8,
|
||
Axis.ACSC_NONE,
|
||
};
|
||
|
||
//定义 XYZ三个轴的左右行程范围
|
||
public string StrConfigFile = Application.StartupPath + "\\File\\config.ini";
|
||
|
||
public static double MotionSpeedxy = 60;
|
||
public static double MotionSpeedz = 30;
|
||
public static double XMaxstrokesw = 730; //正限位
|
||
public static double YMaxstrokesw = 1000;
|
||
public static double ZMaxstrokesw = 5;
|
||
|
||
public static double XMinstrokesw = -30; //负限位
|
||
public static double YMinstrokesw = -10;
|
||
public static double ZMinstrokesw = -280;
|
||
public static int port = 1234; //默认监听端口
|
||
|
||
//定义一个3D点,存储当前平台实时位置
|
||
public Point3D _mPoint3D;
|
||
|
||
#endregion 运动平台变量区
|
||
|
||
#region hexcal软件交互
|
||
|
||
private void StartServer()
|
||
{
|
||
// 对_mTcpIpServer增加判断是否已经启动且存在设备连接
|
||
if (_mTcpIpServer != null && _mTcpIpServer.ConnectStatus)
|
||
{
|
||
//弹窗提醒已经启动
|
||
MyBase.TraceWriteLine("TCP服务端已经启动，请勿重复启动");
|
||
MessageBox.Show(
|
||
"TCP服务端已经启动，请勿重复启动",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
return;
|
||
}
|
||
|
||
//启动服务器，并获取数据，解析
|
||
_mTcpIpServer = new TcpIpServer(IPAddress.Any.ToString(), Convert.ToString(port));
|
||
_mTcpIpServer.UseMode = 1; //设置通讯返回数据流格式
|
||
try
|
||
{
|
||
//启动监听
|
||
if (_mTcpIpServer.StartListen())
|
||
{
|
||
//绑定两个事件 OnRaisedStatus 和OnRaisedMessage
|
||
_mTcpIpServer.OnRaisedMessage += ReceiveMessage; //接收消息回调
|
||
_mTcpIpServer.OnRaisedStatus += ReceiveStatus; //连接状态
|
||
_mTcpIpServer.DataReceived += ReceiveByte;
|
||
}
|
||
else
|
||
{
|
||
MessageBox.Show(
|
||
"TCP服务端启动失败，请检查网络连接，重新打开软件",
|
||
"异常",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Error
|
||
);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DebugDfn.AddLogText("启动TCP服务端异常" + ex);
|
||
}
|
||
}
|
||
|
||
private void ReceiveByte(object sender, byte[] e)
|
||
{
|
||
DebugDfn.AddLogText("接收到" + BitConverter.ToString(e));
|
||
}
|
||
|
||
private void ReceiveMessage(string clientIp, string msg) //接收的内容
|
||
{
|
||
//打印ClientIP 和 Msg
|
||
DebugDfn.AddLogText("接收到" + clientIp + ": " + msg);
|
||
|
||
//根据源地址的不同，执行不同处理
|
||
string sourceIp = clientIp.Split(':')[0];
|
||
switch (sourceIp)
|
||
{
|
||
case "100.0.0.1":
|
||
ParseHexcalMsg(msg);
|
||
break;
|
||
|
||
case "100.0.0.2":
|
||
ParseHexcalMsg(msg);
|
||
break;
|
||
|
||
default:
|
||
DebugDfn.AddLogText("未知来源，没有应答");
|
||
break;
|
||
}
|
||
}
|
||
|
||
public static string ConstructString(string variableName, double[] values)
|
||
{
|
||
string result = variableName + " ";
|
||
for (int i = 0; i < values.Length; i++)
|
||
{
|
||
result += values[i].ToString("F6");
|
||
if (i < values.Length - 1)
|
||
{
|
||
result += ", ";
|
||
}
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
public static string ConstructPosString(Point3D point)
|
||
{
|
||
double[] values = { point.X, point.Y, point.Z, 0.0, 0.0, 0.0, 0.0 };
|
||
return ConstructString("POS", values);
|
||
}
|
||
|
||
public static Point3D ParsePoint3DFromCommand(string input)
|
||
{
|
||
string[] parts = input.Split(' ')[1].Split(',');
|
||
if (parts.Length >= 3)
|
||
{
|
||
double x = double.Parse(parts[0]);
|
||
double y = double.Parse(parts[1]);
|
||
double z = double.Parse(parts[2]);
|
||
return new Point3D(x, y, z);
|
||
}
|
||
|
||
throw new ArgumentException("输入字符串格式不正确。");
|
||
}
|
||
|
||
private void CheckPlatformStatus()
|
||
{
|
||
//检查平台状态，如果运动中，返回BUSY，否则返回READY
|
||
if (
|
||
_currentMotionState == MotionStates.None
|
||
|| _currentMotionState == MotionStates.InPos
|
||
) //默认或到位
|
||
{
|
||
SendMsgToHexcal("READY");
|
||
}
|
||
else
|
||
{
|
||
SendMsgToHexcal("BUSY");
|
||
}
|
||
}
|
||
|
||
private void ParseHexcalMsg(string msg) //编写一个Hexcal协议解析函数
|
||
{
|
||
//DebugDfn.AddLogText("正在解析 " + msg);
|
||
|
||
//去除Msg中\r\n
|
||
msg = msg.Replace("\r\n", "");
|
||
|
||
//判断是否含有故障ERROR字样
|
||
if (msg.Contains("ERROR"))
|
||
{
|
||
//弹窗提醒
|
||
MessageBox.Show("CMM错误", msg, MessageBoxButtons.OK, MessageBoxIcon.Error);
|
||
return;
|
||
}
|
||
|
||
if (msg.Contains("\x02") || msg.Contains("\u0002"))
|
||
{
|
||
//DebugDfn.AddLogText("接收到STX，开始解析");
|
||
CheckPlatformStatus();
|
||
}
|
||
else if (msg.Contains("\x03") || msg.Contains("\u0003"))
|
||
{
|
||
CheckPlatformStatus();
|
||
}
|
||
//else if (msg.Contains("^B")) //查询状态, READY或BUSY
|
||
//{
|
||
// checkPlatformStatus();
|
||
//}
|
||
else if (msg.Contains("CMMTYP")) //测量机类型
|
||
{
|
||
SendMsgToHexcal("CMMTYP MA 19617, FDC V15.00, 10 8 3 , 0");
|
||
}
|
||
else if (msg.Contains("VERSION")) //版本号
|
||
{
|
||
SendMsgToHexcal("00-000-000-00000 FDC V51.04.0000 DATE: 12/21/22 TIME: 12:50:55");
|
||
}
|
||
else if (msg.Contains("SHOW MAXSTROKESW")) //最大行程,根据实际情况填写
|
||
{
|
||
//MAXSTROKESW 233.200000,346.500000,15.100000,0.000000,0.000000,0.000000,0.000000
|
||
|
||
double[] values = { XMaxstrokesw, YMaxstrokesw, ZMaxstrokesw, 0.0, 0.0, 0.0, 0.0 };
|
||
string resultString = ConstructString("MAXSTROKESW", values);
|
||
SendMsgToHexcal(resultString);
|
||
}
|
||
else if (msg.Contains("SHOW MINSTROKESW")) //最小行程,根据实际情况填写
|
||
{
|
||
//MINSTROKESW -68.800000,-55.500000,-286.900000,0.000000,0.000000,0.000000,0.000000
|
||
|
||
double[] values = { XMinstrokesw, YMinstrokesw, ZMinstrokesw, 0.0, 0.0, 0.0, 0.0 };
|
||
string resultString = ConstructString("MINSTROKESW", values);
|
||
SendMsgToHexcal(resultString);
|
||
}
|
||
else if (msg.Contains("SHOW MAXVEL")) //最大速度
|
||
{
|
||
SendMsgToHexcal(
|
||
"MAXVEL 300.000000,300.000000,300.000000,0.000000,0.000000,0.000000,0.000000,0.000000"
|
||
);
|
||
}
|
||
else if (msg.Contains("SHOW MAXACC")) //最大加速度
|
||
{
|
||
SendMsgToHexcal(
|
||
"MAXACC 300.000000,300.000000,300.000000,0.000000,0.000000,0.000000,0.000000,0.000000"
|
||
);
|
||
}
|
||
else if (msg.Contains("SHOW SENSWKP"))
|
||
{
|
||
SendMsgToHexcal("X_ SENSWKP 4,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
|
||
}
|
||
else if (msg.Contains("SHOW X_SENSAXIS"))
|
||
{
|
||
SendMsgToHexcal("X_SENSAXIS 6,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
|
||
}
|
||
else if (msg.Contains("SHOW Y_SENSAXIS")) //查询Y轴
|
||
{
|
||
SendMsgToHexcal("Y_SENSAXIS 2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
|
||
}
|
||
else if (msg.Contains("SHOW Z_SENSAXIS")) //查询Z轴
|
||
{
|
||
SendMsgToHexcal("Z_SENSAXIS 7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
|
||
}
|
||
else if (msg.Contains("SHOW TEMPCOMPTYPE")) //温度补偿，温度补偿 >1 表示支持温度补偿，此处不支持
|
||
{
|
||
SendMsgToHexcal("TEMPCOMPTYPE 0");
|
||
}
|
||
else if (msg.Contains("READTP"))
|
||
{
|
||
SendMsgToHexcal("READTP 0.000000");
|
||
}
|
||
else if (msg.Contains("SHOW ESTOP")) //查询急停状态，根据真是情况调整
|
||
{
|
||
SendMsgToHexcal("ESTOP FALSE");
|
||
}
|
||
else if (msg.Contains("CMHWST"))
|
||
{
|
||
SendMsgToHexcal("CMHWST 8257,0,1792,0");
|
||
}
|
||
else if (msg.Contains("SHOW MOVPAR")) //查询速度
|
||
{
|
||
SendMsgToHexcal(
|
||
"MOVPAR 300.000000,300.000000,300.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.00000 0,0.000000"
|
||
);
|
||
}
|
||
else if (msg.Contains("SHOW MAXVEL")) //查询最大速度
|
||
{
|
||
SendMsgToHexcal(
|
||
"MAXVEL 300.000000,300.000000,300.000000,0.000000,0.000000,0.000000,0.000000,0.000000"
|
||
);
|
||
}
|
||
else if (msg.Contains("SHOW ACCEL")) //查询加速度
|
||
{
|
||
SendMsgToHexcal(
|
||
"ACCEL 1000.000000,1000.000000,1000.000000,0.000000,0.000000,0.000000,0.000000,0.000000"
|
||
);
|
||
}
|
||
else if (msg.Contains("MOVPAR")) //设置速度 xyz 轴的速度
|
||
{
|
||
SendMsgToHexcal("%");
|
||
}
|
||
else if (msg.Contains("ACCEL")) //设置加速度
|
||
{
|
||
SendMsgToHexcal("%");
|
||
}
|
||
else if (msg.Contains("PRBPIN")) //设置侧头偏置
|
||
{
|
||
SendMsgToHexcal("%");
|
||
}
|
||
else if (msg.Contains("ENABLE TEMP")) //设置温度补偿
|
||
{
|
||
SendMsgToHexcal("%");
|
||
}
|
||
else if (msg.Contains("WKPPAR"))
|
||
{
|
||
SendMsgToHexcal("%");
|
||
}
|
||
else if (msg.Contains("SCLTMP"))
|
||
{
|
||
SendMsgToHexcal("%");
|
||
}
|
||
else if (msg.Contains("DISABLE GEO"))
|
||
{
|
||
SendMsgToHexcal("%");
|
||
}
|
||
else if (msg.Contains("AUTZER")) //回家指令
|
||
{
|
||
SendMsgToHexcal("%"); //收到并执行，同时状态改为忙碌
|
||
|
||
//执行回家
|
||
IsHomed();
|
||
}
|
||
else if (msg.Contains("MOVABS")) //移动指令，解析移动位置
|
||
{
|
||
//收到指令 ，形如 MOVABS 0.015000,127.172997,-114.897003,0.000000\r\n
|
||
SendMsgToHexcal("%");
|
||
|
||
Point3D point = ParsePoint3DFromCommand(msg);
|
||
SetPositionXyz(point); //开始移动
|
||
|
||
_pointCloud.Add(point); //添加到点集合
|
||
}
|
||
else if (msg.Contains("GETPOS")) //获取位置
|
||
{
|
||
//POS 167.553898,-55.400421,-208.548678,0.000000,0.000000,0.000000,0.000000
|
||
Point3D point3D = GetPositionXyz(); //获取当前位置
|
||
string resultString = ConstructPosString(point3D);
|
||
SendMsgToHexcal(resultString);
|
||
}
|
||
else
|
||
{
|
||
DebugDfn.AddLogText("未知命令，没有应答");
|
||
}
|
||
}
|
||
|
||
private void ReceiveStatus(TcpIpServer.EnumTcpIpServer iType, string msg)
|
||
{
|
||
//记录到日志
|
||
DebugDfn.AddLogText(iType + " : " + msg);
|
||
|
||
//根据连接状态，更新界面
|
||
switch (iType)
|
||
{
|
||
case TcpIpServer.EnumTcpIpServer.ClientConnect:
|
||
_mBHexcalConnected = true;
|
||
break;
|
||
|
||
default:
|
||
_mBHexcalConnected = false;
|
||
break;
|
||
}
|
||
}
|
||
|
||
private void SendMsgToHexcal(string msg)
|
||
{
|
||
if (_mTcpIpServer == null)
|
||
return;
|
||
|
||
//发送数据
|
||
DebugDfn.AddLogText("回复 " + msg);
|
||
_mTcpIpServer.SendMessageToAllClients(msg += "\r\n"); //回复内容末尾加上\r\n，协议要求
|
||
}
|
||
|
||
private void Btn_StartServer_Click(object sender, EventArgs e)
|
||
{
|
||
Btn_StartServer.Enabled = false;
|
||
Btn_StopServer.Enabled = true;
|
||
StartServer();
|
||
DebugDfn.AddLogText("TCP服务端启动成功 ");
|
||
}
|
||
|
||
private void Btn_StopServer_Click(object sender, EventArgs e)
|
||
{
|
||
//关闭服务端
|
||
if (_mTcpIpServer != null)
|
||
{
|
||
_mTcpIpServer.StopListen();
|
||
}
|
||
|
||
Btn_StopServer.Enabled = false;
|
||
Btn_StartServer.Enabled = true;
|
||
_mBHexcalConnected = false;
|
||
DebugDfn.AddLogText("TCP服务端已关闭");
|
||
}
|
||
|
||
#endregion hexcal软件交互
|
||
|
||
#region ACS平台相关
|
||
|
||
#region 异常抓取
|
||
|
||
//实现函数
|
||
|
||
private static void Application_ThreadException(object sender, ThreadExceptionEventArgs e)
|
||
{
|
||
// 防止程序终止
|
||
MessageBox.Show(
|
||
"发生了未处理的异常：" + e.Exception.Message,
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
}
|
||
|
||
private static void CurrentDomain_UnhandledException(
|
||
object sender,
|
||
UnhandledExceptionEventArgs e
|
||
)
|
||
{
|
||
if (e.ExceptionObject is Exception ex)
|
||
{
|
||
HandleException(ex);
|
||
}
|
||
}
|
||
|
||
private static void HandleException(Exception ex)
|
||
{
|
||
MessageBox.Show(
|
||
$"发生了未处理的异常：{ex.Message}",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
}
|
||
|
||
//订阅报错
|
||
private void EnableFaultEvent()
|
||
{
|
||
_acs.EnableEvent(Interrupts.ACSC_INTR_COMM_CHANNEL_CLOSED);
|
||
_acs.EnableEvent(Interrupts.ACSC_INTR_EMERGENCY);
|
||
_acs.EnableEvent(Interrupts.ACSC_INTR_SYSTEM_ERROR);
|
||
_acs.EnableEvent(Interrupts.ACSC_INTR_ETHERCAT_ERROR);
|
||
_acs.EnableEvent(Interrupts.ACSC_INTR_MOTOR_FAILURE);
|
||
_acs.EnableEvent(Interrupts.ACSC_INTR_MOTION_FAILURE);
|
||
_acs.EnableEvent(Interrupts.ACSC_INTR_PHYSICAL_MOTION_END);
|
||
_acs.COMMCHANNELCLOSED += _acs_COMMCHANNELCLOSED;
|
||
_acs.MOTORFAILURE += _acs_MOTORFAILURE;
|
||
_acs.MOTIONFAILURE += _acs_MOTIONFAILURE;
|
||
_acs.SYSTEMERROR += _acs_SYSTEMERROR;
|
||
_acs.ETHERCATERROR += _acs_ETHERCATERROR;
|
||
_acs.EMERGENCY += _acs_EMERGENCY;
|
||
_acs.PHYSICALMOTIONEND += _acs_PHYSICAL_MOTION_END;
|
||
}
|
||
|
||
private void DisableFaultEvent()
|
||
{
|
||
_acs.COMMCHANNELCLOSED -= _acs_COMMCHANNELCLOSED;
|
||
_acs.MOTORFAILURE -= _acs_MOTORFAILURE;
|
||
_acs.MOTIONFAILURE -= _acs_MOTIONFAILURE;
|
||
_acs.SYSTEMERROR -= _acs_SYSTEMERROR;
|
||
_acs.ETHERCATERROR -= _acs_ETHERCATERROR;
|
||
_acs.EMERGENCY -= _acs_EMERGENCY;
|
||
_acs.PHYSICALMOTIONEND -= _acs_PHYSICAL_MOTION_END;
|
||
|
||
_acs.DisableEvent(Interrupts.ACSC_INTR_COMM_CHANNEL_CLOSED);
|
||
_acs.DisableEvent(Interrupts.ACSC_INTR_EMERGENCY);
|
||
_acs.DisableEvent(Interrupts.ACSC_INTR_SYSTEM_ERROR);
|
||
_acs.DisableEvent(Interrupts.ACSC_INTR_ETHERCAT_ERROR);
|
||
_acs.DisableEvent(Interrupts.ACSC_INTR_MOTOR_FAILURE);
|
||
_acs.DisableEvent(Interrupts.ACSC_INTR_MOTION_FAILURE);
|
||
_acs.DisableEvent(Interrupts.ACSC_INTR_PHYSICAL_MOTION_END);
|
||
}
|
||
|
||
//关联函数
|
||
private void _acs_EMERGENCY(ulong param)
|
||
{
|
||
DebugDfn.AddLogText($"[EStopError] Error Message:{_acs.GetErrorString((int)param)}");
|
||
}
|
||
|
||
private void _acs_ETHERCATERROR(ulong param)
|
||
{
|
||
DebugDfn.AddLogText(
|
||
$"[EtherCatError] Error Message:{_acs.GetErrorString((int)param)}"
|
||
);
|
||
}
|
||
|
||
private void _acs_SYSTEMERROR(ulong param)
|
||
{
|
||
DebugDfn.AddLogText($"[SystemError] Error Message:{_acs.GetErrorString((int)param)}");
|
||
}
|
||
|
||
private void _acs_MOTIONFAILURE(AxisMasks axis)
|
||
{
|
||
for (int i = 0; i < _acs.GetAxesCount(); i++)
|
||
{
|
||
if (((int)axis & (int)Math.Pow(2, i)) == Math.Pow(2, i))
|
||
{
|
||
if (_acs.GetMotionError((Axis)i) != 0)
|
||
{
|
||
//Motor无法自动捕获，需要在motion报错中获取
|
||
int errorcode = _acs.GetMotionError((Axis)i);
|
||
|
||
DebugDfn.AddLogText(
|
||
$"[MotionError] Axis:{i} Error Code:{errorcode} Error Message: {_acs.GetErrorString(errorcode)}"
|
||
);
|
||
|
||
int errorcodes = _acs.GetMotorError((Axis)i);
|
||
|
||
DebugDfn.AddLogText(
|
||
$"[MotorError] Axis:{i} Error Code:{errorcodes} Error Message:{_acs.GetErrorString(errorcodes)}"
|
||
);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
private void _acs_MOTORFAILURE(AxisMasks axis)
|
||
{
|
||
for (int i = 0; i < _acs.GetAxesCount(); i++)
|
||
{
|
||
if (((int)axis & (int)Math.Pow(2, i)) == Math.Pow(2, i))
|
||
{
|
||
int errorcode = _acs.GetMotorError((Axis)i);
|
||
|
||
DebugDfn.AddLogText(
|
||
$"[MotorError] Axis:{i} Error Code:{errorcode} Error Message:{_acs.GetErrorString(errorcode)}"
|
||
);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void _acs_COMMCHANNELCLOSED(ulong param)
|
||
{
|
||
DebugDfn.AddLogText($"[CommError] Error Message:{_acs.GetErrorString((int)param)}");
|
||
}
|
||
|
||
private void _acs_PHYSICAL_MOTION_END(AxisMasks axis)
|
||
{
|
||
int bit = 0x01;
|
||
int axisNo = 0;
|
||
|
||
for (int i = 0; i < 64; i++)
|
||
{
|
||
if ((int)axis == bit)
|
||
{
|
||
axisNo = i;
|
||
break;
|
||
}
|
||
|
||
bit = bit << 1;
|
||
}
|
||
|
||
//DebugDfn.AddLogText(string.Format(" - Axis {0}, Stoppped", axisNo));
|
||
}
|
||
|
||
#endregion 异常抓取
|
||
|
||
private void BtnEnable_Click(object sender, EventArgs e) //使能所有轴
|
||
{
|
||
if (_mAcsConnected)
|
||
{
|
||
//!!!! Important !! Must insert '-1' at the last
|
||
_acs.EnableM(UseAxis);
|
||
}
|
||
else
|
||
{
|
||
//弹窗提醒尚未连接
|
||
MessageBox.Show("未连接到运动平台，请先点击连接");
|
||
}
|
||
}
|
||
|
||
private void BtnDisable_Click(object sender, EventArgs e) //轴取消
|
||
{
|
||
// Disable all of axes
|
||
_acs.DisableAll();
|
||
}
|
||
|
||
private bool IsMotionInPose()
|
||
{
|
||
bool x_inpose = false,
|
||
y_inpose = false,
|
||
z_inpose = false;
|
||
|
||
_mNMotorState = _acs.GetMotorState(Axis.ACSC_AXIS_1);
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
|
||
{
|
||
x_inpose = true;
|
||
}
|
||
|
||
_mNMotorState = _acs.GetMotorState(Axis.ACSC_AXIS_0);
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
|
||
{
|
||
y_inpose = true;
|
||
}
|
||
|
||
_mNMotorState = _acs.GetMotorState(Axis.ACSC_AXIS_8);
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
|
||
{
|
||
z_inpose = true;
|
||
}
|
||
|
||
if (x_inpose && y_inpose && z_inpose)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private void TmrMonitor_Tick(object sender, EventArgs e) //用于刷新状态
|
||
{
|
||
if (_mAcsConnected)
|
||
{
|
||
try
|
||
{
|
||
_mPoint3D = GetPositionXyz(); //取平台当前位置
|
||
#region 更新限位及运动状态
|
||
|
||
//左右限位刷新
|
||
_mObjReadVar = _acs.ReadVariableAsVector(
|
||
"FAULT",
|
||
ProgramBuffer.ACSC_NONE,
|
||
0,
|
||
_mNTotalAxis - 1
|
||
);
|
||
if (_mObjReadVar != null)
|
||
{
|
||
_mArrReadVector = _mObjReadVar as Array;
|
||
if (_mArrReadVector != null)
|
||
{
|
||
UpdateLimitState(0, (int)_mArrReadVector.GetValue(1)); //获取X轴
|
||
UpdateLimitState(1, (int)_mArrReadVector.GetValue(0));
|
||
UpdateLimitState(2, (int)_mArrReadVector.GetValue(8));
|
||
}
|
||
}
|
||
|
||
UpdateSingleAxisStatus(); //刷新运动状态
|
||
#endregion 更新限位及运动状态
|
||
|
||
#region 到位判断
|
||
|
||
if (IsMotionInPose())
|
||
{
|
||
_currentMotionState = MotionStates.InPos;
|
||
//DebugDfn.AddLogText("运动到位");
|
||
}
|
||
else
|
||
{
|
||
_currentMotionState = MotionStates.Moving;
|
||
//DebugDfn.AddLogText("运动中");
|
||
isInPose = false;
|
||
}
|
||
|
||
//增加判断 运动中到 运动到位，主动发送READY
|
||
if (
|
||
_currentMotionState == MotionStates.InPos
|
||
&& _currentMotionState != _currentMotorStateLast
|
||
)
|
||
{
|
||
DebugDfn.AddLogText("运动到位");
|
||
isInPose = true;
|
||
|
||
if (isCounting)
|
||
{
|
||
m_nInPosCount++;
|
||
DebugDfn.AddLogText("到位次数" + m_nInPosCount);
|
||
}
|
||
}
|
||
|
||
_currentMotorStateLast = _currentMotionState;
|
||
|
||
#endregion 到位判断
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DebugDfn.AddLogText("ACS平台刷新异常" + ex);
|
||
MessageBox.Show(
|
||
ex.Message,
|
||
"ERROR",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Error
|
||
);
|
||
Btn_ACSStop_Click(null, null);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void Btn_ACSStart_Click(object sender, EventArgs e) //连接
|
||
{
|
||
btn_ACSStart.Enabled = false;
|
||
btn_ACSStop.Enabled = true;
|
||
|
||
try
|
||
{
|
||
if (rdoTCP.Checked)
|
||
{
|
||
// TCP/IP (Ethernet)
|
||
// TCP/IP (Ethernet)
|
||
_acs.OpenCommEthernetTCP(
|
||
txtIP.Text, // IP Address (Default : 10.0.0.100)
|
||
Convert.ToInt32(txtPort.Text.Trim())
|
||
); // TCP/IP Port nubmer (default : 701)
|
||
}
|
||
else if (rdoSimu.Checked)
|
||
{
|
||
// Simmulation mode
|
||
_acs.OpenCommSimulator();
|
||
}
|
||
|
||
_mAcsConnected = true;
|
||
|
||
//运动相关初始化操作
|
||
InitMotion();
|
||
|
||
// 启动定时器
|
||
tmrMonitor.Interval = 50;
|
||
tmrMonitor.Start();
|
||
}
|
||
catch (COMException comex)
|
||
{
|
||
MessageBox.Show(
|
||
"Connection fail",
|
||
"Error",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Error
|
||
);
|
||
Debug.WriteLine("Connection fail" + comex.Message);
|
||
|
||
_mAcsConnected = false;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DebugDfn.AddLogText("ACS平台连接异常" + ex);
|
||
MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
|
||
}
|
||
}
|
||
|
||
private void Btn_ACSStop_Click(object sender, EventArgs e) //断开连接
|
||
{
|
||
if (_mAcsConnected)
|
||
{
|
||
//DisableFaultEvent(); //取消注册事件
|
||
_acs.CloseComm();
|
||
}
|
||
|
||
tmrMonitor.Stop();
|
||
_mAcsConnected = false;
|
||
btn_ACSStart.Enabled = true;
|
||
btn_ACSStop.Enabled = false;
|
||
}
|
||
|
||
private void UpdateLimitState(int axisNo, int fault) //刷新限位
|
||
{
|
||
if (axisNo < MaxUiLimitCnt)
|
||
{
|
||
if ((fault & (int)SafetyControlMasks.ACSC_SAFETY_LL) != 0)
|
||
_mLblLeftLimit[axisNo].Image = Resources.Error;
|
||
else
|
||
_mLblLeftLimit[axisNo].Image = Resources.Off;
|
||
if ((fault & (int)SafetyControlMasks.ACSC_SAFETY_RL) != 0)
|
||
_mLblRightLimit[axisNo].Image = Resources.Error;
|
||
else
|
||
_mLblRightLimit[axisNo].Image = Resources.Off;
|
||
}
|
||
}
|
||
|
||
public int TranslateAxisNumber(Axis originalAxisNumber)
|
||
{
|
||
int newAxisNumber = -1;
|
||
|
||
switch (originalAxisNumber)
|
||
{
|
||
case Axis.ACSC_AXIS_1: //X轴
|
||
newAxisNumber = 0;
|
||
break;
|
||
|
||
case Axis.ACSC_AXIS_0:
|
||
newAxisNumber = 1; //Y轴
|
||
break;
|
||
|
||
case Axis.ACSC_AXIS_8:
|
||
newAxisNumber = 2;
|
||
break;
|
||
}
|
||
|
||
return newAxisNumber;
|
||
}
|
||
|
||
private void UpdateSingleAxisStatus()
|
||
{
|
||
Axis axis = 0;
|
||
int _axisNo = 0;
|
||
for (int i = 0; i < UseAxis.Length; i++)
|
||
{
|
||
axis = UseAxis[i];
|
||
|
||
_axisNo = TranslateAxisNumber(UseAxis[i]);
|
||
|
||
// Get Motor State ACSPL+ Variable : MST (integer)
|
||
_mNMotorState = _acs.GetMotorState(axis);
|
||
|
||
if (_axisNo == -1)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// 运动中
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_MOVE) != 0)
|
||
{
|
||
_mlblMoving[_axisNo].Image = Resources.On;
|
||
}
|
||
else
|
||
{
|
||
_mlblMoving[_axisNo].Image = Resources.Off;
|
||
}
|
||
|
||
// 就位
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
|
||
{
|
||
_mlblInPos[_axisNo].Image = Resources.On;
|
||
}
|
||
else
|
||
{
|
||
_mlblInPos[_axisNo].Image = Resources.Off;
|
||
}
|
||
|
||
// 加速
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_ACC) != 0)
|
||
{
|
||
_mlblAcc[_axisNo].Image = Resources.On;
|
||
}
|
||
else
|
||
{
|
||
_mlblAcc[_axisNo].Image = Resources.Off;
|
||
}
|
||
|
||
// 使能
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_ENABLE) != 0)
|
||
{
|
||
_mlblEnable[_axisNo].Image = Resources.On;
|
||
axisEnabled[_axisNo] = true; //轴使能
|
||
}
|
||
else
|
||
{
|
||
_mlblEnable[_axisNo].Image = Resources.Off;
|
||
axisEnabled[_axisNo] = false;
|
||
}
|
||
}
|
||
|
||
totalAxisEnabled = CalculateTotalEnabled(axisEnabled, 0, 1, 8);
|
||
DebugDfn.AddLogText($"总的使能状态为：{(totalAxisEnabled ? "使能" : "未使能")}");
|
||
}
|
||
|
||
private void IsHomed() //读取回家状态，当未回家时执行回家指令
|
||
{
|
||
// 1、连接状态检查，如果未连接，提示
|
||
if (!_mAcsConnected)
|
||
{
|
||
DebugDfn.AddLogText("[IsHomed] ACS平台未连接,请先点击连接");
|
||
MessageBox.Show(
|
||
"ACS平台未连接,请先点击连接",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
return;
|
||
}
|
||
|
||
// 2、回家状态检查是否已经回家
|
||
if (_mAcsConnected && _homeStates == HomeStates.Homed)
|
||
{
|
||
//弹窗提示
|
||
MessageBox.Show(
|
||
"轴已经回家",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
return;
|
||
}
|
||
|
||
// 3、读取回家状态
|
||
if (_acs != null && _mAcsConnected)
|
||
{
|
||
var yawHome = _acs.ReadVariable("YAW_HOME_DONE");
|
||
|
||
if (Convert.ToBoolean(yawHome))
|
||
{
|
||
//弹窗提示
|
||
MessageBox.Show(
|
||
"轴已经回家",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
_homeStates = HomeStates.Homed;
|
||
return;
|
||
}
|
||
// 4、执行回家指令,弹窗等待用户确认
|
||
|
||
DialogResult result = MessageBox.Show(
|
||
"轴未回家，即将执行回家指令",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
|
||
if (result == DialogResult.OK)
|
||
{
|
||
// 在这里执行接下来的操作,例如回家指令
|
||
|
||
_acs.RunBuffer(ProgramBuffer.ACSC_BUFFER_6, null); //执行回家指令，这里的buffer6是回家指令的buffer
|
||
_homeStates = HomeStates.Homing;
|
||
_currentMotionState = MotionStates.Moving;
|
||
DebugDfn.AddLogText("回家运动中");
|
||
|
||
//等待回家完成
|
||
for (int i = 0; i < UseAxis.Length; i++)
|
||
{
|
||
_acs.WaitMotionEnd(UseAxis[i], _motionTimeout); //等待回家完成
|
||
}
|
||
|
||
_homeStates = HomeStates.Homed;
|
||
_currentMotionState = MotionStates.InPos;
|
||
DebugDfn.AddLogText("回家完成");
|
||
}
|
||
else
|
||
{
|
||
//弹窗提醒
|
||
MessageBox.Show(
|
||
"点击了取消，未进行任何动作",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void InitMotion() //定义 运动状态初始化函数，包括内部变量，轴启用，回家判断等
|
||
{
|
||
string strTemp;
|
||
//运动相关变量初始化
|
||
_homeStates = HomeStates.None;
|
||
_currentMotionState = MotionStates.None;
|
||
_currentMotionState = MotionStates.None;
|
||
|
||
if (!_mAcsConnected)
|
||
{
|
||
MessageBox.Show("未连接运动平台，请先连接运动平台");
|
||
return;
|
||
}
|
||
|
||
//轴启用，加电
|
||
_acs.EnableM(UseAxis);
|
||
for (int i = 0; i < UseAxis.Length; i++)
|
||
{
|
||
_acs.WaitMotorEnabled(UseAxis[i], 1, _motionTimeout); //等待电机使能
|
||
}
|
||
|
||
DebugDfn.AddLogText("电机已启用");
|
||
|
||
//回家
|
||
IsHomed();
|
||
|
||
//设置定位速度
|
||
SetSpeedXyz(MotionSpeedxy, MotionSpeedz);
|
||
|
||
//获取轴数量
|
||
strTemp = _acs.Transaction("?SYSINFO(13)");
|
||
_mNTotalAxis = Convert.ToInt32(strTemp.Trim());
|
||
_mLblLeftLimit = new Label[MaxUiLimitCnt]; //左限位
|
||
_mLblLeftLimit[0] = lblLL0;
|
||
_mLblLeftLimit[1] = lblLL1;
|
||
_mLblLeftLimit[2] = lblLL2;
|
||
|
||
_mLblRightLimit = new Label[MaxUiLimitCnt]; //右限位
|
||
_mLblRightLimit[0] = lblRL0;
|
||
_mLblRightLimit[1] = lblRL1;
|
||
_mLblRightLimit[2] = lblRL2;
|
||
|
||
_mlblMoving = new Label[MaxUiLimitCnt]; //运动中
|
||
_mlblMoving[0] = lblMoving0;
|
||
_mlblMoving[1] = lblMoving1;
|
||
_mlblMoving[2] = lblMoving2;
|
||
|
||
_mlblAcc = new Label[MaxUiLimitCnt]; // 加速中
|
||
_mlblAcc[0] = lblAcc0;
|
||
_mlblAcc[1] = lblAcc1;
|
||
_mlblAcc[2] = lblAcc2;
|
||
|
||
_mlblInPos = new Label[MaxUiLimitCnt]; //就位
|
||
_mlblInPos[0] = lblInPos0;
|
||
_mlblInPos[1] = lblInPos1;
|
||
_mlblInPos[2] = lblInPos2;
|
||
|
||
_mlblEnable = new Label[MaxUiLimitCnt]; //轴使能
|
||
_mlblEnable[0] = lblEnable0;
|
||
_mlblEnable[1] = lblEnable1;
|
||
_mlblEnable[2] = lblEnable2;
|
||
|
||
//EnableFaultEvent(); //订阅错误事件
|
||
}
|
||
|
||
public static bool IsWithinLimit(Point3D point) //判断点是否在行程范围内
|
||
{
|
||
if (
|
||
point.X >= XMinstrokesw
|
||
&& point.X <= XMaxstrokesw
|
||
&& point.Y >= YMinstrokesw
|
||
&& point.Y <= YMaxstrokesw
|
||
&& point.Z >= ZMinstrokesw
|
||
&& point.Z <= ZMaxstrokesw
|
||
)
|
||
{
|
||
|
||
return true;
|
||
}
|
||
|
||
DebugDfn.AddLogText(point.X.ToString() + " " + point.Y.ToString() + " " + point.Z.ToString());
|
||
//打印 XMinstrokesw 等值
|
||
DebugDfn.AddLogText("XMinstrokesw超限: " + XMinstrokesw);
|
||
DebugDfn.AddLogText("XMaxstrokesw超限: " + XMaxstrokesw);
|
||
DebugDfn.AddLogText("YMinstrokesw超限: " + YMinstrokesw);
|
||
DebugDfn.AddLogText("YMaxstrokesw超限: " + YMaxstrokesw);
|
||
DebugDfn.AddLogText("ZMinstrokesw超限: " + ZMinstrokesw);
|
||
DebugDfn.AddLogText("ZMaxstrokesw超限: " + ZMaxstrokesw);
|
||
return false;
|
||
}
|
||
|
||
public static bool IsWithinLimit(Point point) //判断点是否在行程范围内
|
||
{
|
||
if (
|
||
point.X >= XMinstrokesw
|
||
&& point.X <= XMaxstrokesw
|
||
&& point.Y >= YMinstrokesw
|
||
&& point.Y <= YMaxstrokesw
|
||
&& point.Z >= ZMinstrokesw
|
||
&& point.Z <= ZMaxstrokesw
|
||
)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
public void SetPositionXyz(Point3D point3D) //运动到指定位置
|
||
{
|
||
if (!_mAcsConnected)
|
||
{
|
||
DebugDfn.AddLogText("ACS平台未连接,请先点击连接");
|
||
MessageBox.Show(
|
||
"ACS平台未连接,请先点击连接",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
return;
|
||
}
|
||
|
||
if (_currentMotionState != MotionStates.Moving)
|
||
{
|
||
_currentMotionState = MotionStates.Moving; //设置当前运动状态
|
||
//判断 point3D是否合法
|
||
if (point3D != null)
|
||
{
|
||
if (IsWithinLimit(point3D)) //判断点是否在行程范围内
|
||
{
|
||
double[] pointsArray = { point3D.X, point3D.Y, point3D.Z };
|
||
//判断各轴使能状态，如果未使能，则使能
|
||
|
||
if (!totalAxisEnabled)
|
||
{
|
||
_acs.EnableM(UseAxis);
|
||
for (int i = 0; i < UseAxis.Length; i++)
|
||
{
|
||
_acs.WaitMotorEnabled(UseAxis[i], 1, _motionTimeout); //等待电机使能
|
||
}
|
||
|
||
//DebugDfn.AddLogText("电机已启用");
|
||
}
|
||
|
||
//执行运动指令
|
||
_acs.ToPointM(MotionFlags.ACSC_NONE, UseAxis, pointsArray); //多轴运动到指定位置
|
||
}
|
||
else
|
||
{
|
||
DebugDfn.AddLogText("目标位置超出行程范围，请重新设置");
|
||
//MessageBox.Show(
|
||
// "目标位置超出行程范围，请重新设置",
|
||
// "异常",
|
||
// MessageBoxButtons.OK,
|
||
// MessageBoxIcon.Error
|
||
//);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
DebugDfn.AddLogText("目标位置为空，请重新设置");
|
||
MessageBox.Show(
|
||
"目标位置为空，请重新设置",
|
||
"异常",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Error
|
||
);
|
||
}
|
||
}
|
||
}
|
||
|
||
private Point3D GetPositionXyz(int positionMode = 1) //获取当前位置
|
||
{
|
||
double xPosition = 0,
|
||
yPosition = 0,
|
||
zPosition = 0;
|
||
Point3D point3D = new Point3D(xPosition, yPosition, zPosition);
|
||
if (!_mAcsConnected)
|
||
{
|
||
DebugDfn.AddLogText("ACS平台未连接,请先点击连接");
|
||
MessageBox.Show(
|
||
"ACS平台未连接,请先点击连接",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
return point3D;
|
||
}
|
||
|
||
//获取当前位置, 两种 GetRPosition，GetFPosition
|
||
if (positionMode == 1)
|
||
{
|
||
//获取反馈位置 Feedback position (Encoder value) ACSPL+ Variable : FPO (real)
|
||
xPosition = _acs.GetFPosition(UseAxis[0]);
|
||
yPosition = _acs.GetFPosition(UseAxis[1]);
|
||
zPosition = _acs.GetFPosition(UseAxis[2]);
|
||
//DebugDfn.AddLogText("反馈位置: " + xPosition + " " + yPosition + " " + zPosition);
|
||
}
|
||
else
|
||
{
|
||
//获取参考位置 ACSPL+ Variable : RPOS (real)
|
||
xPosition = _acs.GetRPosition(UseAxis[0]);
|
||
yPosition = _acs.GetRPosition(UseAxis[1]);
|
||
zPosition = _acs.GetRPosition(UseAxis[2]);
|
||
DebugDfn.AddLogText("参考位置: " + xPosition + " " + yPosition + " " + zPosition);
|
||
}
|
||
|
||
//构造point3D格式
|
||
point3D = new Point3D(xPosition, yPosition, zPosition);
|
||
|
||
return point3D;
|
||
}
|
||
|
||
private void SetSpeedXyz(double speedxy, double speedz) //获取运动参数
|
||
{
|
||
//获取实际速度
|
||
double feedbackVelocity = (double)
|
||
_acs.ReadVariable("FVEL", ProgramBuffer.ACSC_NONE, 0, 0);
|
||
DebugDfn.AddLogText("实际速度: " + feedbackVelocity);
|
||
|
||
//设置Y轴 速度参数
|
||
_acs.SetVelocity(Axis.ACSC_AXIS_0, speedxy);
|
||
_acs.SetAcceleration(Axis.ACSC_AXIS_0, speedxy * 10);
|
||
_acs.SetDeceleration(Axis.ACSC_AXIS_0, speedxy * 10);
|
||
_acs.SetKillDeceleration(Axis.ACSC_AXIS_0, speedxy * 100);
|
||
_acs.SetJerk(Axis.ACSC_AXIS_0, speedxy * 100);
|
||
|
||
//设置X轴速度参数
|
||
_acs.SetVelocity(Axis.ACSC_AXIS_1, speedxy);
|
||
_acs.SetAcceleration(Axis.ACSC_AXIS_1, speedxy * 10);
|
||
_acs.SetDeceleration(Axis.ACSC_AXIS_1, speedxy * 10);
|
||
_acs.SetKillDeceleration(Axis.ACSC_AXIS_1, speedxy * 100);
|
||
_acs.SetJerk(Axis.ACSC_AXIS_1, speedxy * 100);
|
||
|
||
//设置Z轴速度参数
|
||
|
||
_acs.SetVelocity(Axis.ACSC_AXIS_8, speedz);
|
||
_acs.SetAcceleration(Axis.ACSC_AXIS_8, speedz * 10);
|
||
_acs.SetDeceleration(Axis.ACSC_AXIS_8, speedz * 10);
|
||
_acs.SetKillDeceleration(Axis.ACSC_AXIS_8, speedz * 100);
|
||
_acs.SetJerk(Axis.ACSC_AXIS_8, speedz * 100);
|
||
|
||
DebugDfn.AddLogText($"速度设置完成 ");
|
||
}
|
||
|
||
private void rtb_quick_loc_Click(object sender, EventArgs e) //快速定位
|
||
{
|
||
// 获取文本框的值
|
||
double x = double.Parse(rtb_SetX.Text);
|
||
double y = double.Parse(rtb_Sety.Text);
|
||
double z = double.Parse(rtb_SetZ.Text);
|
||
|
||
// 构造 Point3D 对象
|
||
Point3D point = new Point3D(x, y, z);
|
||
|
||
SetPositionXyz(point);
|
||
}
|
||
|
||
private static bool CalculateTotalEnabled(bool[] axisEnabled, params int[] axisIndices) //判断轴使能状态
|
||
{
|
||
bool totalEnabled = true;
|
||
foreach (int index in axisIndices)
|
||
{
|
||
bool isEnabled = axisEnabled[index];
|
||
if (!isEnabled)
|
||
{
|
||
totalEnabled = false;
|
||
break;
|
||
}
|
||
}
|
||
return totalEnabled;
|
||
}
|
||
|
||
private void rtb_stop_Click(object sender, EventArgs e)
|
||
{
|
||
try
|
||
{
|
||
Axis[] m_arrAxisList = new Axis[]
|
||
{
|
||
Axis.ACSC_AXIS_0,
|
||
Axis.ACSC_AXIS_1,
|
||
Axis.ACSC_AXIS_8,
|
||
Axis.ACSC_NONE,
|
||
};
|
||
|
||
if (m_arrAxisList != null)
|
||
_acs.HaltM(m_arrAxisList);
|
||
|
||
DebugDfn.AddLogText("立即停止 已发送命令");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
|
||
Debug.WriteLine(ex.Message);
|
||
}
|
||
}
|
||
|
||
private void rtb_home_Click(object sender, EventArgs e)
|
||
{
|
||
//弹窗提醒用户，是否执行回家指令
|
||
DialogResult result = MessageBox.Show(
|
||
"是否执行回家指令",
|
||
"提示",
|
||
MessageBoxButtons.OKCancel,
|
||
MessageBoxIcon.Information
|
||
);
|
||
|
||
if (result == DialogResult.Cancel)
|
||
{
|
||
return;
|
||
}
|
||
|
||
DebugDfn.AddLogText("回家指令已发送");
|
||
if (!_mAcsConnected)
|
||
{
|
||
DebugDfn.AddLogText("ACS平台未连接,请先点击连接");
|
||
MessageBox.Show(
|
||
"ACS平台未连接,请先点击连接",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
return;
|
||
}
|
||
|
||
if (_currentMotionState != MotionStates.Moving)
|
||
{
|
||
_currentMotionState = MotionStates.Moving; //设置当前运动状态
|
||
|
||
_acs.RunBuffer(ProgramBuffer.ACSC_BUFFER_6, null); //执行回家指令，这里的buffer6是回家指令的buffer
|
||
_homeStates = HomeStates.Homing;
|
||
_currentMotionState = MotionStates.Moving;
|
||
DebugDfn.AddLogText("回家运动中");
|
||
|
||
//等待回家完成
|
||
for (int i = 0; i < UseAxis.Length; i++)
|
||
{
|
||
_acs.WaitMotionEnd(UseAxis[i], _motionTimeout); //等待回家完成
|
||
}
|
||
|
||
_homeStates = HomeStates.Homed;
|
||
_currentMotionState = MotionStates.InPos;
|
||
DebugDfn.AddLogText("回家完成");
|
||
}
|
||
}
|
||
|
||
private void rtb_etalon_Click(object sender, EventArgs e) //etalon校准
|
||
{
|
||
DebugDfn.AddLogText("Etalon校准");
|
||
//判断通讯对象是否存在
|
||
if (_acs == null || !_acs.IsConnected)
|
||
{
|
||
DebugDfn.AddLogText("未建立与运动平台通讯，请在主界面先建立通讯");
|
||
|
||
// 在合适的位置调用 MessageBox.Show() 方法
|
||
MessageBox.Show(
|
||
"未建立与运动平台通讯，请在主界面先建立通讯",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
EtalonForm etalonForm = new EtalonForm(this);
|
||
etalonForm.Show();
|
||
}
|
||
}
|
||
|
||
#endregion ACS平台相关
|
||
|
||
#region 菜单栏
|
||
|
||
private void btn_motion_Click(object sender, EventArgs e)
|
||
{
|
||
//判断通讯对象是否存在
|
||
if (_acs == null || !_acs.IsConnected)
|
||
{
|
||
DebugDfn.AddLogText("未建立与运动平台通讯，请在主界面先建立通讯");
|
||
|
||
// 在合适的位置调用 MessageBox.Show() 方法
|
||
MessageBox.Show(
|
||
"未建立与运动平台通讯，请在主界面先建立通讯",
|
||
"提示",
|
||
MessageBoxButtons.OK,
|
||
MessageBoxIcon.Information
|
||
);
|
||
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
Motion motion = new Motion(this);
|
||
motion.Show();
|
||
}
|
||
}
|
||
|
||
private void Rtb_about_Click(object sender, EventArgs e) //关于界面
|
||
{
|
||
AboutBox mAboutBox = new AboutBox();
|
||
mAboutBox.StartPosition = FormStartPosition.CenterScreen;
|
||
mAboutBox.Show();
|
||
}
|
||
|
||
private void rtb_demo_Click(object sender, EventArgs e)
|
||
{
|
||
DemoShow demoShow = new DemoShow(_acs);
|
||
demoShow.Show();
|
||
demoShow.BringToFront();
|
||
}
|
||
|
||
private void Timer_RefreshUI_Tick(object sender, EventArgs e) //UI刷新
|
||
{
|
||
//状态灯刷新
|
||
lamp_acs.State = _mAcsConnected ? LampColor.Green : LampColor.Silver;
|
||
lamp_hexcal.State = _mBHexcalConnected ? LampColor.Green : LampColor.Silver;
|
||
|
||
//时间栏
|
||
//获取当前时间，构造形如 精确到秒,例如 2023-10-08 16:01:23
|
||
rle_timer.Text = "当前时间: " + DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss");
|
||
|
||
Plot2D(_pointCloud); //绘图
|
||
|
||
//更新位置
|
||
if (_mPoint3D != null)
|
||
{
|
||
rtb_xPos.Text = _mPoint3D.X.ToString("F3");
|
||
rtb_yPos.Text = _mPoint3D.Y.ToString("F3");
|
||
rtb_zPos.Text = _mPoint3D.Z.ToString("F3");
|
||
}
|
||
}
|
||
|
||
#endregion 菜单栏
|
||
}
|
||
}
|