zhengxuan.zhang
1011 lines
27 KiB
C#
1011 lines
27 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.IO;
|
||
using System.Windows.Forms;
|
||
using ACS.SPiiPlusNET;
|
||
using HexcalMC.Base;
|
||
using HexcalMC.Form;
|
||
using HexcalMC.Hexcal;
|
||
using HexcalMC.Properties;
|
||
using SharpGL;
|
||
using Telerik.WinControls.UI;
|
||
|
||
namespace HexcalMC
|
||
{
|
||
//定一个 回家状态枚举，包括 从未回家，正在回家，已经回家
|
||
public enum HomeStates
|
||
{
|
||
None, //默认状态
|
||
NotHome,
|
||
Homing,
|
||
Homed
|
||
}
|
||
|
||
//定义 运动状态枚举，包括 正在运动，运动到位，Jog运动
|
||
public enum MotionStates
|
||
{
|
||
None, //默认状态
|
||
Moving,
|
||
InPos,
|
||
Jogging
|
||
}
|
||
|
||
public partial class MainFrom : RadRibbonForm
|
||
{
|
||
private readonly List<Point3D> _pointCloud = new List<Point3D>(); //运动中点集合
|
||
|
||
|
||
public MainFrom()
|
||
{
|
||
InitializeComponent();
|
||
|
||
|
||
#region 3D相关绘制，暂时取消
|
||
|
||
//var sharpGlViewportControl = new SharpGLViewportControl
|
||
//{
|
||
// Dock = DockStyle.Fill
|
||
//};
|
||
|
||
//var tabPage = new TabPage("3D 窗口");
|
||
//tabPage.Controls.Add(sharpGlViewportControl);
|
||
//tabControl1.TabPages.Add(tabPage);
|
||
|
||
//// 设置容器控件的 DoubleBuffered 属性为 true
|
||
//DoubleBuffered = true;
|
||
//// 创建一个包含点云数据的列表
|
||
//var points = new List<Point3D>
|
||
//{
|
||
// new Point3D(0, 0, 0),
|
||
// new Point3D(1, 1, 1),
|
||
// new Point3D(-1, -1, -1)
|
||
// // 可以添加更多的点云数据
|
||
//};
|
||
|
||
//// 将点云数据传递给 SharpGLViewportControl
|
||
//sharpGlViewportControl.SetPointCloud(points);
|
||
|
||
tabControl1.TabPages.RemoveAt(1);
|
||
|
||
#endregion
|
||
}
|
||
|
||
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();
|
||
this.TopMost = true;
|
||
}
|
||
|
||
private void MainFrom_Shown(object sender, EventArgs e) //窗体显示准备好接受用户输入时发生
|
||
{
|
||
//启动服务端,用于接收hexcal传来的指令
|
||
StartServer();
|
||
|
||
Btn_ACSStart_Click(null, null);
|
||
|
||
//Plot3D();
|
||
}
|
||
|
||
private void MainFrom_FormClosed(object sender, FormClosedEventArgs e)
|
||
{
|
||
MyBase.TraceWriteLine("关闭程序");
|
||
DebugDfn._strEndTime = DateTime.Now.ToString("yyyy.MM.dd HH-mm-ss");
|
||
timer_RefreshUI.Stop();
|
||
|
||
|
||
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;
|
||
}
|
||
|
||
MOTION_SPEED = FileIni.ReadDouble(StrConfigFile, "MOTOR", "MOTION_SPEED"); //运动定位速度
|
||
//正限位
|
||
X_MAXSTROKESW = FileIni.ReadDouble(StrConfigFile, "MOTOR", "X_MAXSTROKESW");
|
||
Y_MAXSTROKESW = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Y_MAXSTROKESW");
|
||
Z_MAXSTROKESW = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Z_MAXSTROKESW");
|
||
|
||
//负限位
|
||
X_MINSTROKESW = FileIni.ReadDouble(StrConfigFile, "MOTOR", "X_MINSTROKESW");
|
||
Y_MINSTROKESW = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Y_MINSTROKESW");
|
||
Z_MINSTROKESW = FileIni.ReadDouble(StrConfigFile, "MOTOR", "Z_MINSTROKESW");
|
||
}
|
||
|
||
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();
|
||
}
|
||
|
||
private void Plot3D() //绘制3D图
|
||
{
|
||
List<Point3D> points = new List<Point3D>();
|
||
|
||
// 假设已经有了一些点的数据，将其添加到 List<Point3D> 对象中
|
||
points.Add(new Point3D(1, 2, 3));
|
||
points.Add(new Point3D(4, 5, 6));
|
||
|
||
LoadPoints(points);
|
||
}
|
||
|
||
|
||
#region 运动平台变量区
|
||
|
||
private Api _acs;
|
||
|
||
private const int MaxUiLimitCnt = 8;
|
||
private const int MaxUiIoCnt = 8;
|
||
|
||
private readonly int _mNTotalAxis = 0;
|
||
private int _mNTotalBuffer = 0;
|
||
private Axis[] _mArrAxisList = null;
|
||
|
||
private 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 HomeStates _homeStates; //回家状态
|
||
private MotionStates _currentMotionState; //当前运动状态
|
||
private readonly int _motionTimeout = 50000; //定义运动超时时间
|
||
|
||
|
||
public static Axis[] USE_AXIS =
|
||
{ 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 MOTION_SPEED = 60;
|
||
public static double X_MAXSTROKESW = 730; //正限位
|
||
public static double Y_MAXSTROKESW = 1000;
|
||
public static double Z_MAXSTROKESW = 5;
|
||
|
||
public static double X_MINSTROKESW = -30; //负限位
|
||
public static double Y_MINSTROKESW = -10;
|
||
public static double Z_MINSTROKESW = -280;
|
||
|
||
#endregion
|
||
|
||
#region hexcal变量区
|
||
|
||
private TcpIpServer _mTcpIpServer; //创建tcpserver，用于接收hexcal传来的指令，并解析传递平台
|
||
private bool _mBHexcalConnected;
|
||
|
||
#endregion
|
||
|
||
|
||
#region hexcal软件交互
|
||
|
||
private void StartServer()
|
||
{
|
||
// 对_mTcpIpServer增加判断是否已经启动且存在设备连接
|
||
if (_mTcpIpServer != null)
|
||
{
|
||
//弹窗提醒已经启动
|
||
MessageBox.Show("TCP服务端已经启动，请勿重复启动", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
|
||
return;
|
||
}
|
||
|
||
//启动服务器，并获取数据，解析
|
||
_mTcpIpServer = new TcpIpServer("100.0.0.1", Convert.ToString(1234));
|
||
_mTcpIpServer.UseMode = 1; //设置通讯返回数据流格式
|
||
try
|
||
{
|
||
//启动监听
|
||
if (_mTcpIpServer.StartListen())
|
||
{
|
||
//绑定两个事件 OnRaisedStatus 和OnRaisedMessage
|
||
_mTcpIpServer.OnRaisedMessage += ReceiveMessage; //接收消息回调
|
||
_mTcpIpServer.OnRaisedStatus += ReceiveStatus; //连接状态
|
||
_mTcpIpServer.DataReceived += ReceiveByte;
|
||
DebugDfn.AddLogText("TCP服务端启动成功 ");
|
||
}
|
||
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": //L2系统
|
||
|
||
//解析处理数据
|
||
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()
|
||
{
|
||
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")) return;
|
||
|
||
if (msg.Contains("\x02") || msg.Contains("\u0002"))
|
||
{
|
||
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, 6 6 2 , 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")) //最大行程,根据实际情况填写
|
||
{
|
||
//SendMsgToHexcal("MAXSTROKESW 233.200000,346.500000,15.100000,0.000000,0.000000,0.000000,0.000000");
|
||
|
||
double[] values = { X_MAXSTROKESW, Y_MAXSTROKESW, Z_MAXSTROKESW, 0.0, 0.0, 0.0, 0.0 };
|
||
string resultString = ConstructString("MAXSTROKESW", values);
|
||
SendMsgToHexcal(resultString);
|
||
}
|
||
else if (msg.Contains("SHOW MINSTROKESW")) //最小行程,根据实际情况填写
|
||
{
|
||
//SendMsgToHexcal("MINSTROKESW -68.800000,-55.500000,-286.900000,0.000000,0.000000,0.000000,0.000000");
|
||
|
||
double[] values = { X_MINSTROKESW, Y_MINSTROKESW, Z_MINSTROKESW, 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 1");
|
||
}
|
||
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("AUTZER")) //回家指令
|
||
{
|
||
SendMsgToHexcal("%"); //收到并执行，同时状态改为忙碌
|
||
|
||
//执行回家
|
||
Ishome();
|
||
}
|
||
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")) //获取位置
|
||
{
|
||
//SendMsgToHexcal("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)
|
||
{
|
||
//发送数据
|
||
DebugDfn.AddLogText("回复 " + msg);
|
||
_mTcpIpServer.SendMessageToAllClients(msg += "\r\n"); //回复内容末尾加上\r\n，协议要求
|
||
}
|
||
|
||
private void Btn_StartServer_Click(object sender, EventArgs e)
|
||
{
|
||
StartServer();
|
||
}
|
||
|
||
private void Btn_StopServer_Click(object sender, EventArgs e)
|
||
{
|
||
//关闭服务端
|
||
if (_mTcpIpServer != null)
|
||
{
|
||
_mTcpIpServer.StopListen();
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region ACS平台相关
|
||
|
||
private void BtnEnable_Click(object sender, EventArgs e) //使能所有轴
|
||
{
|
||
if (_mAcsConnected)
|
||
{
|
||
//!!!! Important !! Must insert '-1' at the last
|
||
_acs.EnableM(USE_AXIS);
|
||
}
|
||
else
|
||
{
|
||
//弹窗提醒尚未连接
|
||
MessageBox.Show("未连接到运动平台，请先点击连接");
|
||
}
|
||
}
|
||
|
||
private void BtnDisable_Click(object sender, EventArgs e) //轴取消
|
||
{
|
||
// Disable all of axes
|
||
_acs.DisableAll();
|
||
}
|
||
|
||
private void TmrMonitor_Tick(object sender, EventArgs e) //用于刷新状态
|
||
{
|
||
int iAxisNo = cboAxisNo.SelectedIndex;
|
||
if (_mAcsConnected)
|
||
{
|
||
try
|
||
{
|
||
// Get Motor State ACSPL+ Variable : MST (integer)
|
||
_mNMotorState = _acs.GetMotorState((Axis)iAxisNo);
|
||
|
||
// Returned value is integer, you need to use bitmaks
|
||
|
||
// 运动中
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_MOVE) != 0)
|
||
{
|
||
lblMoving.Image = Resources.On;
|
||
_currentMotionState = MotionStates.Moving;
|
||
}
|
||
else
|
||
{
|
||
lblMoving.Image = Resources.Off;
|
||
}
|
||
|
||
// 就位
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_INPOS) != 0)
|
||
{
|
||
lblInPos.Image = Resources.On;
|
||
_currentMotionState = MotionStates.InPos;
|
||
}
|
||
else
|
||
{
|
||
lblInPos.Image = Resources.Off;
|
||
}
|
||
|
||
// 加速
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_ACC) != 0)
|
||
{
|
||
lblAcc.Image = Resources.On;
|
||
}
|
||
else
|
||
{
|
||
lblAcc.Image = Resources.Off;
|
||
}
|
||
|
||
// 使能
|
||
if ((_mNMotorState & MotorStates.ACSC_MST_ENABLE) != 0)
|
||
{
|
||
lblEnable.Image = Resources.On;
|
||
}
|
||
else
|
||
{
|
||
lblEnable.Image = Resources.Off;
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DebugDfn.AddLogText("ACS平台刷新异常" + ex);
|
||
MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void Btn_ACSStart_Click(object sender, EventArgs e) //连接
|
||
{
|
||
btn_ACSStart.Enabled = false;
|
||
btn_ACSStop.Enabled = true;
|
||
|
||
try
|
||
{
|
||
_acs.OpenCommEthernetTCP(
|
||
txtIP.Text, // IP Address (Default : 10.0.0.100)
|
||
Convert.ToInt32(txtPort.Text.Trim())); // TCP/IP Port nubmer (default : 701)
|
||
|
||
_mAcsConnected = true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DebugDfn.AddLogText("ACS平台连接异常" + ex);
|
||
MessageBox.Show(ex.Message, "ERROR", MessageBoxButtons.OK, MessageBoxIcon.Error);
|
||
}
|
||
|
||
|
||
// 启动定时器
|
||
tmrMonitor.Interval = 50;
|
||
tmrMonitor.Start();
|
||
|
||
|
||
//运动相关初始化操作
|
||
InitMotion();
|
||
}
|
||
|
||
private void Btn_ACSStop_Click(object sender, EventArgs e) //断开连接
|
||
{
|
||
if (_mAcsConnected) _acs.CloseComm();
|
||
|
||
tmrMonitor.Stop();
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
private void Ishome() //读取回家状态，当未回家时执行回家指令
|
||
{
|
||
// 1、连接状态检查，如果未连接，提示
|
||
if (!_mAcsConnected)
|
||
{
|
||
DebugDfn.AddLogText("[Ishome] 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 < USE_AXIS.Length; i++)
|
||
{
|
||
_acs.WaitMotionEnd(USE_AXIS[i], _motionTimeout); //等待回家完成
|
||
}
|
||
|
||
_homeStates = HomeStates.Homed;
|
||
_currentMotionState = MotionStates.InPos;
|
||
DebugDfn.AddLogText("回家完成");
|
||
}
|
||
else
|
||
{
|
||
//弹窗提醒
|
||
MessageBox.Show("点击了取消，未进行任何动作", "提示", MessageBoxButtons.OK, MessageBoxIcon.Information);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void InitMotion() //定义 运动状态初始化函数，包括内部变量，轴启用，回家判断等
|
||
{
|
||
//运动相关变量初始化
|
||
_homeStates = HomeStates.None;
|
||
_currentMotionState = MotionStates.None;
|
||
_currentMotionState = MotionStates.None;
|
||
|
||
if (!_mAcsConnected)
|
||
{
|
||
MessageBox.Show("未连接运动平台，请先连接运动平台");
|
||
return;
|
||
}
|
||
|
||
if (_acs == null)
|
||
{
|
||
DebugDfn.AddLogText("运动控制对象不存在");
|
||
return;
|
||
}
|
||
|
||
//轴启用，加电
|
||
_acs.EnableM(USE_AXIS);
|
||
for (int i = 0; i < USE_AXIS.Length; i++)
|
||
{
|
||
_acs.WaitMotorEnabled(USE_AXIS[i], 1, _motionTimeout); //等待电机使能
|
||
}
|
||
|
||
DebugDfn.AddLogText("电机已启用");
|
||
|
||
//回家
|
||
Ishome();
|
||
|
||
SetSpeedXyz();
|
||
|
||
//注册到位事件
|
||
_acs.PHYSICALMOTIONEND += ACS_PHYSICALMOTIONEND;
|
||
_acs.EnableEvent(Interrupts.ACSC_INTR_PHYSICAL_MOTION_END);
|
||
}
|
||
|
||
private void ACS_PHYSICALMOTIONEND(AxisMasks axis)
|
||
{
|
||
int bit = 0x01;
|
||
int axisNo = 0;
|
||
// Param value is bit number
|
||
// Bit Number = Axis Number
|
||
for (int i = 0; i < 64; i++)
|
||
{
|
||
if ((int)axis == bit)
|
||
{
|
||
axisNo = i;
|
||
break;
|
||
}
|
||
|
||
bit = bit << 1;
|
||
}
|
||
|
||
// Add log to ListBox
|
||
Invoke((MethodInvoker)delegate
|
||
{
|
||
DebugDfn.AddLogText("运动到位");
|
||
_currentMotionState = MotionStates.InPos;
|
||
DebugDfn.AddLogText("运动到位");
|
||
|
||
});
|
||
}
|
||
|
||
private bool IsWithinStrokes(Point3D point) //判断点是否在行程范围内
|
||
{
|
||
if (point.X >= X_MINSTROKESW && point.X <= X_MAXSTROKESW &&
|
||
point.Y >= Y_MINSTROKESW && point.Y <= Y_MAXSTROKESW &&
|
||
point.Z >= Z_MINSTROKESW && point.Z <= Z_MAXSTROKESW)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private 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 (IsWithinStrokes(point3D)) //判断点是否在行程范围内
|
||
{
|
||
double[] pointsArray =
|
||
{
|
||
point3D.X,
|
||
point3D.Y,
|
||
point3D.Z
|
||
};
|
||
|
||
//执行运动指令
|
||
_acs.ToPointM(MotionFlags.ACSC_NONE, USE_AXIS, pointsArray); //多轴运动到指定位置
|
||
|
||
////等待运动完成
|
||
//for (int i = 0; i < USE_AXIS.Length; i++)
|
||
//{
|
||
// _acs.WaitMotionEnd(USE_AXIS[i], _motionTimeout); //等待回家完成
|
||
//}
|
||
|
||
//_currentMotionState = MotionStates.InPos;
|
||
//DebugDfn.AddLogText("运动到位");
|
||
}
|
||
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(USE_AXIS[0]);
|
||
yPosition = _acs.GetFPosition(USE_AXIS[1]);
|
||
zPosition = _acs.GetFPosition(USE_AXIS[2]);
|
||
DebugDfn.AddLogText("反馈位置: " + xPosition + " " + yPosition + " " + zPosition);
|
||
}
|
||
else
|
||
{
|
||
//获取参考位置 ACSPL+ Variable : RPOS (real)
|
||
xPosition = _acs.GetRPosition(USE_AXIS[0]);
|
||
yPosition = _acs.GetRPosition(USE_AXIS[1]);
|
||
zPosition = _acs.GetRPosition(USE_AXIS[2]);
|
||
DebugDfn.AddLogText("参考位置: " + xPosition + " " + yPosition + " " + zPosition);
|
||
}
|
||
|
||
|
||
//构造point3D格式
|
||
point3D = new Point3D(xPosition, yPosition, zPosition);
|
||
|
||
return point3D;
|
||
}
|
||
|
||
private void SetSpeedXyz() //获取运动参数
|
||
{
|
||
//获取实际速度
|
||
double feedbackVelocity = (double)_acs.ReadVariable("FVEL", ProgramBuffer.ACSC_NONE, 0, 0);
|
||
DebugDfn.AddLogText("实际速度: " + feedbackVelocity);
|
||
|
||
//设置Y轴 速度参数
|
||
_acs.SetVelocity(Axis.ACSC_AXIS_0, MOTION_SPEED);
|
||
_acs.SetAcceleration(Axis.ACSC_AXIS_0,MOTION_SPEED*10);
|
||
_acs.SetDeceleration(Axis.ACSC_AXIS_0,MOTION_SPEED*10);
|
||
|
||
//设置X轴速度参数
|
||
_acs.SetVelocity(Axis.ACSC_AXIS_1, MOTION_SPEED);
|
||
_acs.SetAcceleration(Axis.ACSC_AXIS_1, MOTION_SPEED * 10);
|
||
_acs.SetDeceleration(Axis.ACSC_AXIS_1, MOTION_SPEED * 10);
|
||
|
||
//设置Z轴速度参数
|
||
_acs.SetVelocity(Axis.ACSC_AXIS_8, MOTION_SPEED);
|
||
_acs.SetAcceleration(Axis.ACSC_AXIS_8, MOTION_SPEED * 10);
|
||
_acs.SetDeceleration(Axis.ACSC_AXIS_8, MOTION_SPEED * 10);
|
||
|
||
}
|
||
|
||
private void openGLControl1_OpenGLDraw(object sender, RenderEventArgs args)
|
||
{
|
||
OpenGL gl = openGLControl1.OpenGL;
|
||
|
||
gl.ClearColor(0, 0, 0, 0);
|
||
gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);
|
||
|
||
gl.LoadIdentity();
|
||
//gl.Translate(0.0f, 0.0f, -5.0f); // 平移场景
|
||
}
|
||
|
||
private void LoadPoints(List<Point3D> points)
|
||
{
|
||
OpenGL gl = openGLControl1.OpenGL;
|
||
|
||
gl.ClearColor(0, 0, 0, 0);
|
||
gl.Clear(OpenGL.GL_COLOR_BUFFER_BIT | OpenGL.GL_DEPTH_BUFFER_BIT);
|
||
|
||
gl.LoadIdentity();
|
||
//gl.Translate(0.0f, 0.0f, -5.0f); // 平移场景
|
||
|
||
gl.Begin(OpenGL.GL_POINTS);
|
||
foreach (Point3D point in points)
|
||
{
|
||
// 设置点的颜色
|
||
gl.Color(1.0f, 0.0f, 0.0f); // 红色
|
||
|
||
// 设置点的大小
|
||
gl.PointSize(5.0f); // 大小为5个单位
|
||
gl.Vertex(point.X, point.Y, point.Z);
|
||
}
|
||
|
||
gl.End();
|
||
|
||
gl.Flush();
|
||
}
|
||
|
||
private void openGLControl1_OpenGLInitialized(object sender, EventArgs e)
|
||
{
|
||
}
|
||
|
||
private void openGLControl1_Resized(object sender, EventArgs e)
|
||
{
|
||
}
|
||
|
||
private void openGLControl1_GDIDraw(object sender, RenderEventArgs args)
|
||
{
|
||
}
|
||
|
||
|
||
|
||
#endregion ACS平台相关
|
||
|
||
#region 菜单栏
|
||
|
||
private void Rtb_motion_Click(object sender, EventArgs e) //ACS调试页面
|
||
{
|
||
Motion motion = new Motion();
|
||
motion.Show();
|
||
}
|
||
|
||
private void Rtb_about_Click(object sender, EventArgs e) //关于界面
|
||
{
|
||
AboutBox mAboutBox = new AboutBox();
|
||
mAboutBox.StartPosition = FormStartPosition.CenterScreen;
|
||
mAboutBox.Show();
|
||
}
|
||
|
||
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); //绘图
|
||
}
|
||
|
||
#endregion 菜单栏
|
||
}
|
||
} |