|
@@ -3,9 +3,11 @@ using System.Collections.Generic;
|
|
|
using System.Linq;
|
|
using System.Linq;
|
|
|
using System.Text;
|
|
using System.Text;
|
|
|
using System.Threading.Tasks;
|
|
using System.Threading.Tasks;
|
|
|
|
|
+using GSG.NET.LINQ;
|
|
|
using OHV.Common.Shareds;
|
|
using OHV.Common.Shareds;
|
|
|
using OHV.SqliteDAL;
|
|
using OHV.SqliteDAL;
|
|
|
using VehicleControlSystem.ControlLayer.Axis;
|
|
using VehicleControlSystem.ControlLayer.Axis;
|
|
|
|
|
+using VehicleControlSystem.ControlLayer.Lib.EziPlusE;
|
|
|
|
|
|
|
|
namespace VehicleControlSystem.ControlLayer
|
|
namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
{
|
|
@@ -19,6 +21,11 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.sql = sqliteManager;
|
|
this.sql = sqliteManager;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
+ public void Init()
|
|
|
|
|
+ {
|
|
|
|
|
+ this.CreateAxis();
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
void CreateAxis()
|
|
void CreateAxis()
|
|
|
{
|
|
{
|
|
|
sql.AxisConfigDAL.All.ForEach(config =>
|
|
sql.AxisConfigDAL.All.ForEach(config =>
|
|
@@ -53,32 +60,171 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
});
|
|
});
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- bool CLAMP_OriginReturn()
|
|
|
|
|
|
|
+ public int LeftOriginReturn()
|
|
|
{
|
|
{
|
|
|
- this.axes.ForEach(a =>
|
|
|
|
|
- {
|
|
|
|
|
- a.OriginReturn(true);
|
|
|
|
|
- });
|
|
|
|
|
|
|
+ var servo = this.axes.Where(s => s.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT)).Single();
|
|
|
|
|
+ return servo.OriginReturn(true);
|
|
|
|
|
+ }
|
|
|
|
|
|
|
|
- return true;
|
|
|
|
|
|
|
+ public int RightOriginReturn()
|
|
|
|
|
+ {
|
|
|
|
|
+ var servo = this.axes.Where(s => s.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_RIGHT)).Single();
|
|
|
|
|
+ return servo.OriginReturn(true);
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
- bool CLAMP_Lock()
|
|
|
|
|
|
|
+ public int AllOriginReturn()
|
|
|
{
|
|
{
|
|
|
- this.axes.ForEach(a =>
|
|
|
|
|
- {
|
|
|
|
|
- var pData = sql.AxisPositionDataDAL.GetK(a.Config.AxisName, ConstString.TEACH_POSITION_LOCK);
|
|
|
|
|
- a.StartMove(pData.Value, 0);
|
|
|
|
|
- });
|
|
|
|
|
|
|
+ int result = 0;
|
|
|
|
|
+ result = LeftOriginReturn();
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return 7;
|
|
|
|
|
|
|
|
- this.axes.ForEach(a =>
|
|
|
|
|
|
|
+ result = RightOriginReturn();
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return 8;
|
|
|
|
|
+
|
|
|
|
|
+ return result;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ public int IsAllAxisOriginReturn()
|
|
|
|
|
+ {
|
|
|
|
|
+ int result = 0;
|
|
|
|
|
+ this.axes.ForEach(x =>
|
|
|
{
|
|
{
|
|
|
- a.Wait4Done();
|
|
|
|
|
|
|
+ if (!x.IsOriginReturn())
|
|
|
|
|
+ {
|
|
|
|
|
+ if (x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT))
|
|
|
|
|
+ result = 5;
|
|
|
|
|
+ else
|
|
|
|
|
+ result = 6;
|
|
|
|
|
+ return;
|
|
|
|
|
+ }
|
|
|
});
|
|
});
|
|
|
|
|
|
|
|
- return true;
|
|
|
|
|
|
|
+ return result;
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
+ #region Data
|
|
|
|
|
+ public void SavePositionData(string pos, double left, double right)
|
|
|
|
|
+ {
|
|
|
|
|
+ var dataList = sql.AxisPositionDataDAL.GetKFromPostion(pos);
|
|
|
|
|
+ var sqlLeft = dataList.Where(x => x.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT)).Single();
|
|
|
|
|
+ var sqlRight = dataList.Where(x => x.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_RIGHT)).Single();
|
|
|
|
|
+
|
|
|
|
|
+ sqlLeft.Value = left;
|
|
|
|
|
+ sql.AxisPositionDataDAL.Update(sqlLeft);
|
|
|
|
|
+
|
|
|
|
|
+ sqlRight.Value = right;
|
|
|
|
|
+ sql.AxisPositionDataDAL.Update(sqlRight);
|
|
|
|
|
+ }
|
|
|
|
|
+ #endregion
|
|
|
|
|
+
|
|
|
|
|
+ public int JogMove(string axisName, E_JogMoveDir dir, int velocity)
|
|
|
|
|
+ {
|
|
|
|
|
+ var axis = this.axes.Where(x => x.Config.AxisName.Equals(axisName)).Single();
|
|
|
|
|
+ return axis.JogMove(dir, velocity);
|
|
|
|
|
+ }
|
|
|
|
|
|
|
|
|
|
+ public int MoveToLockPosition(string axisName)
|
|
|
|
|
+ {
|
|
|
|
|
+ var servo = this.axes.Where(x => x.Config.AxisName.Equals(axisName)).Single();
|
|
|
|
|
+ var axisPositionDatas = sql.AxisPositionDataDAL.GetKFromName(axisName).ToList();
|
|
|
|
|
+ var data = axisPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_LOCK)).Single();
|
|
|
|
|
+
|
|
|
|
|
+ int result = servo.Move(data.Value);
|
|
|
|
|
+
|
|
|
|
|
+ return result;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ public int MoveToUnlockPosition(string axisName)
|
|
|
|
|
+ {
|
|
|
|
|
+ var servo = this.axes.Where(x => x.Config.AxisName.Equals(axisName)).Single();
|
|
|
|
|
+ var axisPositionDatas = sql.AxisPositionDataDAL.GetKFromName(axisName).ToList();
|
|
|
|
|
+ var data = axisPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_UNLOCK)).Single();
|
|
|
|
|
+
|
|
|
|
|
+ int result = servo.Move(data.Value);
|
|
|
|
|
+
|
|
|
|
|
+ return result;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ /// <summary>
|
|
|
|
|
+ /// 두개를 동시에 Lock
|
|
|
|
|
+ /// </summary>
|
|
|
|
|
+ /// <returns></returns>
|
|
|
|
|
+ public int Lock_Sync()
|
|
|
|
|
+ {
|
|
|
|
|
+ int result = 0;
|
|
|
|
|
+
|
|
|
|
|
+ result = this.AllOriginReturn();
|
|
|
|
|
+ if (result != 0)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+ var leftPositionDatas = sql.AxisPositionDataDAL.GetKFromName(ConstString.AXIS_CARRIER_LOCK_LEFT).ToList();
|
|
|
|
|
+ var leftData = leftPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_LOCK)).Single();
|
|
|
|
|
+ var left = this.axes.Where(x => x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT)).Single();
|
|
|
|
|
+
|
|
|
|
|
+ var rightPositionDatas = sql.AxisPositionDataDAL.GetKFromName(ConstString.AXIS_CARRIER_LOCK_RIGHT).ToList();
|
|
|
|
|
+ var rightData = leftPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_LOCK)).Single();
|
|
|
|
|
+ var right = this.axes.Where(x => x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_RIGHT)).Single();
|
|
|
|
|
+
|
|
|
|
|
+ result = left.StartMove(leftData.Value, 0);
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+ result = right.StartMove(rightData.Value, 0);
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+ result = left.Wait4Done();
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+ result = right.Wait4Done();
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+
|
|
|
|
|
+ return EziMOTIONPlusELib.FMM_OK;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ /// <summary>
|
|
|
|
|
+ /// 두개의 Axis 를 동시에 Unlock
|
|
|
|
|
+ /// </summary>
|
|
|
|
|
+ /// <returns></returns>
|
|
|
|
|
+ public int Unlock_Sync()
|
|
|
|
|
+ {
|
|
|
|
|
+ int result = 0;
|
|
|
|
|
+
|
|
|
|
|
+ result = this.AllOriginReturn();
|
|
|
|
|
+ if (result != 0)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+ var leftPositionDatas = sql.AxisPositionDataDAL.GetKFromName(ConstString.AXIS_CARRIER_LOCK_LEFT).ToList();
|
|
|
|
|
+ var leftData = leftPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_UNLOCK)).Single();
|
|
|
|
|
+ var left = this.axes.Where(x => x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_LEFT)).Single();
|
|
|
|
|
+
|
|
|
|
|
+ var rightPositionDatas = sql.AxisPositionDataDAL.GetKFromName(ConstString.AXIS_CARRIER_LOCK_RIGHT).ToList();
|
|
|
|
|
+ var rightData = leftPositionDatas.Where(x => x.Name.Equals(ConstString.TEACH_POSITION_UNLOCK)).Single();
|
|
|
|
|
+ var right = this.axes.Where(x => x.Config.AxisName.Equals(ConstString.AXIS_CARRIER_LOCK_RIGHT)).Single();
|
|
|
|
|
+
|
|
|
|
|
+ result = left.StartMove(leftData.Value, 0);
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+ result = right.StartMove(rightData.Value, 0);
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+ result = left.Wait4Done();
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+ result = right.Wait4Done();
|
|
|
|
|
+ if (result != EziMOTIONPlusELib.FMM_OK)
|
|
|
|
|
+ return result;
|
|
|
|
|
+
|
|
|
|
|
+
|
|
|
|
|
+ return EziMOTIONPlusELib.FMM_OK;
|
|
|
|
|
+ }
|
|
|
}
|
|
}
|
|
|
}
|
|
}
|