Kepler-Release/Venus/Venus_RT/Devices/LzMatch_Ethercat.cs

using Aitex.Core.Backend;
using Aitex.Core.Common.DeviceData;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.OperationCenter;
using MECF.Framework.Common.Device.Bases;
using MECF.Framework.Common.Equipment;
using System;
using System.Diagnostics;
using System.Windows;
using Venus_Core;
using Venus_RT.Devices.AdLinkEthercat;

namespace Venus_RT.Devices
{
    public class LzMatch_Ethercat : RfMatchBase
    {
        //Int32 v_card_name = 0;
        Int32 v_board_id = -1;
        //Int32 v_StartAxisID = 0;
        Int32 BUS_No = 0;
        Int32 Mod_ID = 1;
        private readonly VenusDevice _device;
        private Stopwatch _stopWatch = new Stopwatch();
        private string WorkMode { get; set; } = "";

        public LzMatch_Ethercat(ModuleName mod, VenusDevice venusDevice, Int32 board_id) : base(mod.ToString(), venusDevice.ToString())
        {
            _device = venusDevice;
            v_board_id = board_id;
            _stopWatch.Start();

        }
        public override bool Initialize()
        {
            base.Initialize();
            DATA.Subscribe($"{Module}.{Name}.C1", () => TunePosition1);
            DATA.Subscribe($"{Module}.{Name}.C2", () => TunePosition2);
            DATA.Subscribe($"{Module}.{Name}.WorkMode", () => WorkMode);

            OP.Subscribe($"{Module}.{Name}.{AITRfOperation.SetMatchPosition}", (out string reason, int time, object[] param) =>
            {
                SetMatchPosition((float)Convert.ToDouble(param[0]), (float)Convert.ToDouble(param[1]), out reason);
                return true;
            });

            OP.Subscribe($"{Module}.{Name}.{AITRfOperation.SetMatchProcessMode}", (out string reason, int time, object[] param) =>
            {
                SetMatchMode((string)param[0] == "Auto" ? EnumRfMatchTuneMode.Auto : EnumRfMatchTuneMode.Manual, out reason);
                return true;
            });
            return true;
        }
        public override void Monitor()
        {
            if (_stopWatch.ElapsedMilliseconds > 500)
            {
                TunePosition1 = GetPositionC1();
                TunePosition2 = GetPositionC2();
                WorkMode = GetWorkMode();   

                _stopWatch.Restart();
            }
        }
        public override void SetMatchPosition(float c1, float c2, out string reason)
        {
            base.SetMatchPosition(c1, c2, out reason);
            executeMatchPostion(c1, c2);
            reason = "";
        }
        private void executeMatchPostion(float c1, float c2)
        {
            SetWorkMode(EnumRfMatchTuneMode.Manual);
            SetPosition(c1, c2);
        }
        private void SetPosition(float c1val, float c2val)
        {
            SetPositionC1(c1val);
            SetPositionC2(c2val);
        }
        private void SetPositionC1(float c1val)
        {
            Int32 ret = 0;
            Byte[] Data = new Byte[32];
            UInt16 OD_index = 0x8000;
            UInt16 OD_Subindex = 3;
            UInt32 DataLength = 32;
            UInt32 TimeOut = 10000;
            UInt32 Flags = 0;
            Data = BitConverter.GetBytes(c1val);
            ret = APS168.APS_set_field_bus_sdo(v_board_id, BUS_No, Mod_ID, OD_index, OD_Subindex, Data, DataLength, TimeOut, Flags);
            if (ret != 0)
            {
                LOG.Write(eEvent.ERR_RF, Module, $"{_device} Set C1 {c1val} Fail!");
            }
            else
            {
                LOG.Write(eEvent.EV_DEVICE_INFO, Module, $"{_device} Set C1 {c1val} Success!");
            }
        }
        private void SetPositionC2(float c1va2)
        {
            Int32 ret = 0;
            Byte[] Data = new Byte[32];
            UInt16 OD_index = 0x8000;
            UInt16 OD_Subindex = 2;
            UInt32 DataLength = 32;
            UInt32 TimeOut = 10000;
            UInt32 Flags = 0;
            Data = BitConverter.GetBytes(c1va2);
            ret = APS168.APS_set_field_bus_sdo(v_board_id, BUS_No, Mod_ID, OD_index, OD_Subindex, Data, DataLength, TimeOut, Flags);
            if (ret != 0)
            {
                LOG.Write(eEvent.ERR_RF, Module, $"{_device} Set C2 {c1va2} Fail!");
            }
            else
            {
                LOG.Write(eEvent.EV_DEVICE_INFO, Module, $"{_device} Set C2 {c1va2} Success!");
            }
        }
        private float GetPositionC1()
        {
            Int32 ret;
            Int32 BUS_No = 0;
            UInt16 ByteOffset = 22;
            UInt16 Size = 4;
            uint value = 0;
            ret = APS168.APS_get_field_bus_pdo(v_board_id, BUS_No, ByteOffset, Size, ref value);
            float f;

            if (ret != 0)
            {
                f = -1;
                LOG.Write(eEvent.ERR_MATCH, Module, $"{_device} Get C1 Value Fail");
            }
            else
            {
                f = BitConverter.ToSingle(BitConverter.GetBytes(value), 0);
                //LOG.Write(eEvent.EV_DEVICE_INFO, Module, $"{_device} Get C1 Value Success");
            }
            return f;
        }
        private float GetPositionC2()
        {
            Int32 ret;
            Int32 BUS_No = 0;
            UInt16 ByteOffset = 18;
            UInt16 Size = 4;
            uint value = 0;
            ret = APS168.APS_get_field_bus_pdo(v_board_id, BUS_No, ByteOffset, Size, ref value);
            float f;

            if (ret != 0)
            {
                f = -1;
                LOG.Write(eEvent.ERR_MATCH, Module, $"{_device} Get C2 Value Fail");
            }
            else
            {
                f = BitConverter.ToSingle(BitConverter.GetBytes(value), 0);
                //LOG.Write(eEvent.EV_DEVICE_INFO, Module, $"{_device} Get C2 Value Success");
            }
            return f;
        }
        public override bool SetMatchMode(EnumRfMatchTuneMode enumRfMatchTuneMode, out string reason)
        {
            reason = string.Empty;
            SetWorkMode(enumRfMatchTuneMode);
            return true;
        }
        private void SetWorkMode(EnumRfMatchTuneMode mode)
        {
            Int32 ret = 0;
            Byte[] Data = new Byte[32];
            UInt16 OD_index = 0x8000;
            UInt16 OD_Subindex = 1;
            UInt32 DataLength = 1;
            UInt32 TimeOut = 10000;
            Int32 Datatemp;
            UInt32 Flags = 0;

            if (mode == EnumRfMatchTuneMode.Auto)
            {
                Datatemp = 0;
            }
            else
            {
                Datatemp = 1;
            }

            for (Int32 i = 0; i < 8; i++)
            {
                Data[i] = (Byte)((Datatemp >> (i * 8)) & 0xFF);
            }

            ret = APS168.APS_set_field_bus_sdo(v_board_id, BUS_No, Mod_ID, OD_index, OD_Subindex, Data, DataLength, TimeOut, Flags);
            if (ret != 0)
            {
                LOG.Write(eEvent.ERR_MATCH, Module, $"{_device} set work mode {mode} Fail!");
            }
            else
            {
                //LOG.Write(eEvent.EV_DEVICE_INFO, Module, $"{_device} set work mode {mode} Success!");
            }
        }
        private string GetWorkMode()
        {
            Int32 ret = 0;
            Byte[] Data = new Byte[32];
            UInt16 OD_index = 0x8000;
            UInt16 OD_Subindex = 1;
            UInt32 DataLength = 1;
            UInt32 TimeOut = 10000;
            UInt32 OutDataLength = 0;
            UInt32 Flags = 0;

            ret = APS168.APS_get_field_bus_sdo(v_board_id, BUS_No, Mod_ID, OD_index, OD_Subindex, Data, DataLength, ref OutDataLength, TimeOut, Flags);
            if (ret != 0)
            {
                LOG.Write(eEvent.ERR_MATCH, Module, $"{_device} Get Work Mode Fail");
            }
            else
            {
                if (Data[0] == 0)
                {
                    return "Auto";

                }
                else if (Data[0] == 1)
                {
                    return "Manual";
                }

            }
            return "";
        }
    }
}