FlyADClientAdv.cs

using FObjBase;
using Misc;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Diagnostics;
using System.Linq;
using System.Net;
using System.Text;
using System.Threading.Tasks;

namespace FlyADBase
{
    public partial class FlyAD7 : IFlyADClientAdv
    {
        NLog.Logger logger = NLog.LogManager.GetCurrentClassLogger();

        /// <summary>
        /// 通过脉冲计算速度，不使用AD盒的输出
        /// </summary>
        public bool IsCalSpeed { get; set; } = true;

        private int gridsmooth = 0;
        /// <summary>
        /// grid数据平滑
        /// </summary>
        public int GridSmooth
        {
            get { return gridsmooth; }
            set
            {
                if (value < 0)
                    value = 0;

                if (gridsmooth != value)
                {
                    gridsmooth = value;
                }
            }
        }


        /// <summary>
        /// 机架总长
        /// </summary>
        public int PosLen { get; set; } = 8900;

        /// <summary>
        /// Speed1 = Velocity * Speed1Scale
        /// </summary>
        public double Speed1Scale => (double)Ratio02 / Ratio01;

        /// <summary>
        /// 动作完成
        /// </summary>
        public bool IsFinish { get; set; }

        int pos1_last_for_speed = int.MinValue;
        int pos2_last_for_speed = int.MinValue;
        Stopwatch sw_calSpeed = new Stopwatch();
        void constructor2()
        {
            fGrid.SetSize(PosLen / PosOfGrid);

            mTimeGridAdvHelper.Init();

            this.PropertyChanged += FlyAD7_PropertyChanged1;


            sw_calSpeed.Start();
            //更新线速度
            PollModule.Current.Poll_Config(PollModule.POLL_CONFIG.ADD,
                () =>
                {
                    //速度是错了，需要自己计算
                    double scale = 1000.0 / sw_calSpeed.ElapsedMilliseconds;
                    sw_calSpeed.Restart();


                    int speed = Speed;
                    int speed2 = Speed2;
                    if (pos1_last_for_speed != int.MinValue)
                    {
                        speed = (int)((Position - pos1_last_for_speed) * scale);
                    }
                    pos1_last_for_speed = Position;


                    if (pos2_last_for_speed != int.MinValue)
                    {
                        speed2 = (int)((Position2 - pos2_last_for_speed) * scale);
                    }
                    pos2_last_for_speed = Position2;

                    if (IsCalSpeed)
                    {
                        Speed = speed;
                        Speed2 = speed2;
                    }
                }, TimeSpan.FromSeconds(1));
        }

        private void FlyAD7_PropertyChanged1(object sender, System.ComponentModel.PropertyChangedEventArgs e)
        {
            if (e.PropertyName == "DriveStatus")
            {
                switch (DriveStatus)
                {
                    case DRIVE_MAN_STATUS.STOP_MANUAL:
                    case DRIVE_MAN_STATUS.STOP:
                    case DRIVE_MAN_STATUS.LIMIT:
                        IsFinish = true;
                        break;
                    case DRIVE_MAN_STATUS.RUNNING:
                        IsFinish = false;
                        break;
                }
            }
            else if ((e.PropertyName == "PosLen") ||
                (e.PropertyName == "PosOfGrid"))
            {
                fGrid.SetSize(PosLen / PosOfGrid);
            }
            else if (e.PropertyName == "Marker")
            {
                //if (DriveOrder == DRIVE_MAN_ORDER.SYNC)
                {
                    //同步运行中
                    //检测与哪个指令一致, 
                    //该指令置为 Doing， 前面的全部置为 Finish，并且删除
                    for (int i = 0; i < SyncOrders.Count(); i++)
                    {
                        if (SyncOrders[i].Marker == Marker)
                        {
                            SyncOrders[i].State = SyncOrderState.Doing;

                            for (int j = 0; j < i; j++)
                            {
                                SyncOrders[0].State = SyncOrderState.Finish;
                                SyncOrders.RemoveAt(0);
                            }
                        }
                    }
                }
            }
        }

        void Init2()
        {
            mTimeGridAdvHelper.Clear();
            preTimeGrid_start = DateTime.MinValue;
            preTimeGrid_end = DateTime.MinValue;
        }




        /// <summary>
        /// Runto(0)， 不同于 Backward
        /// </summary>
        public void RuntoMin()
        {
            Runto(0);
        }

        /// <summary>
        /// Runto(PosLen)， 不同于 Forward
        /// </summary>
        public void RuntoMax()
        {
            Runto(PosLen);
        }

        /// <summary>
        /// 设置输出
        /// </summary>
        /// <param name="index"></param>
        /// <param name="is1"></param>
        public void SetOutputBit(int index, bool is1)
        {
            if (index > 15)//4)
                return;
            if (index < 0)
                return;
            if (is1)
            {
                SetOutput(
                    (UInt16)Misc.MyBase.BIT(index),
                    (UInt16)Misc.MyBase.BIT(index));
            }
            else
            {
                SetOutput(
                    (UInt16)Misc.MyBase.BIT(index),
                    (UInt16)~Misc.MyBase.BIT(index));
            }
        }





        /// <summary>
        /// 正反转 grid数据 缓存区
        /// </summary>
        class SGrid
        {
            public const int GRID_MAX_SIZE = 1000;
            int size;
            public int[][] data = new int[2][];//data[0]=forword, data[1]=backward
            public SGrid()
            {
                //清空所有数据
                SetSize(GRID_MAX_SIZE);
            }
            public void SetSize(int size)
            {
                this.size = size;
                data[0] = new int[size];
                data[1] = new int[size];
                Clear();
            }
            public void Clear()
            {
                //清空所有数据
                for (int i = 0; i < size; i++)
                {
                    data[0][i] = Misc.MyBase.NULL_VALUE;
                    data[1][i] = Misc.MyBase.NULL_VALUE;
                }
            }
        }
        SGrid fGrid = new SGrid();
        /// <summary>
        /// 从正反缓存区, 获取grid数据
        /// </summary>
        /// <param name="direction">方向, 只有 正,反</param>
        /// <param name="grid_start">grid 开始位置</param>
        /// <param name="grid_len">grid 长度</param>
        /// <param name="dat">grid 数据</param>
        public void GetGrid(Misc.DIRECTION direction, int grid_start, int grid_len, out int[] dat)
        {
            int index = 0;
            if (direction == Misc.DIRECTION.BACKWARD)
                index = 1;

            dat = new int[grid_len];

            for (int i = 0; i < dat.Length; i++)
            {
                int grid_num = grid_start + i;
                if (grid_num >= fGrid.data[index].Length)
                    break;

                if (GridSmooth > 0)
                {
                    int sum = 0;
                    int cnt = 0;
                    for (int j = 0; j < (GridSmooth * 2 + 1); j++)
                    {
                        int idx = grid_num - GridSmooth + j;
                        if (idx < 0)
                            continue;
                        if (idx >= fGrid.data[index].Length)
                            break;
                        if (Misc.MyBase.ISVALIDATA(fGrid.data[index][idx]))
                        {
                            sum += fGrid.data[index][idx];
                            cnt++;
                        }
                    }
                    if (cnt > 0)
                        dat[i] = sum / cnt;
                    else
                        dat[i] = Misc.MyBase.NULL_VALUE;
                }
                else
                {
                    dat[i] = fGrid.data[index][grid_num];
                }
            }

            CorrectADs?.Invoke(direction, grid_start, dat);
            return;
        }

        /// <summary>
        /// 从正反缓存区, 获取全部grid数据
        /// </summary>
        /// <param name="direction">方向, 只有 正,反</param>
        /// <param name="dat">grid 数据</param>
        public void GetGrid(Misc.DIRECTION direction, out int[] dat)
        {
            GetGrid(direction, 0, fGrid.data[0].Length, out dat);
        }

        /// <summary>
        /// 机架修正
        /// </summary>
        public CorrectADsHandler CorrectADs { get; set; }

        #region 滞后处理
        //一共有1.AD数据池（由timegrid提供） 1min
        //2.pos数据池（pos推送提供） 1min
        //4.当接收的grid事件数据。它有 （direction, grid_start，grid_len, systick )
        //systick 就是结束的时间点。 当AD数据池出现了这个时间点

        //pos数据池向前找。 pos 在 grid_start*posOfGrid 范围的数据。 
        //找到开始的systick 后，整合3个数据池的数据。
        //5.最后代替 grid 推送出去。

        /// <summary>
        /// ad滞后修正  单位ms
        /// </summary>
        public int ADLag { get; set; }
        public event TimeGridAdvEventHandler TimeGridAdvEvent;
        TimeGridAdvHelper mTimeGridAdvHelper = new TimeGridAdvHelper();
        /// <summary>
        /// 上一次接收到timegrid 的时间
        /// </summary>
        DateTime preTimeGrid_start;
        DateTime preTimeGrid_end;
        #endregion
        #region runto 推送 istatus 变化

        public void GridIStatusEventAdd(int istatus_no, GridIStatusEventHander func)
        {
            mTimeGridAdvHelper.GridIStatusEventAdd(istatus_no, func);
        }
        public void GridIStatusEventDel(int istatus_no, GridIStatusEventHander func)
        {
            mTimeGridAdvHelper.GridIStatusEventDel(istatus_no, func);
        }


        #endregion


        void advGetPos1AD1(int ad)
        {
            mTimeGridAdvHelper.AddPos_Default(Position, Now);

            if (CorrectADs != null)
            {
                ad = CorrectAD(Position / PosOfGrid, ad);
            }
            AD = ad;
        }
        void advGetIo()
        {
            mTimeGridAdvHelper.AddIStatus_Default(IStatus, Position, Now);
        }
        void advPushPos1()
        {
            if (Version == 1)
            {
                mTimeGridAdvHelper.AddPos(Position, DateTime.Now);
            }
            else
            {
                mTimeGridAdvHelper.AddPos(Position, Now);
            }
        }
        void advPushAd(int ad)
        {
            if (CorrectADs != null)
                ad = CorrectAD(Position / PosOfGrid, ad);

            AD = ad;
        }
        void advPushIo(int pos1)
        {
            mTimeGridAdvHelper.AddIStatus(IStatus, pos1, Now);
        }
        void advPushTimeGrid(DateTime dt, int[] data)
        {
            double ts_ms = 1.28;
            long ticks = (long)((ts_ms * data.Length) * TimeSpan.TicksPerMillisecond);

            if (TimeGridEvent != null)
            {
                if (CorrectADs != null)
                {
                    if (PosOfGrid > 0)
                    {
                        int grid = Position / PosOfGrid;
                        int[] d = new int[1];
                        for (int i = 0; i < data.Count(); i++)
                        {
                            d[0] = data[i];
                            CorrectADs(Misc.DIRECTION.FIX, grid, d);
                            data[i] = d[0];
                        }
                    }
                }
            }


            #region 高级 timegrid
            //if (preTimeGrid_start != DateTime.MinValue)
            //{
            //    if (TimeGridAdvEvent != null)
            //    {
            //        var gridAdvUnits = mTimeGridAdvHelper.GetGridAdv(preTimeGrid_start, preTimeGrid_end);

            //        if (CorrectADs != null)
            //        {
            //            foreach (var gridAdvUnit in gridAdvUnits)
            //            {
            //                gridAdvUnit.ad = CorrectAD(gridAdvUnit.pos / PosOfGrid, gridAdvUnit.ad);
            //            }
            //        }
            //        TimeGridAdvEvent(this, new TimeGridAdvEventArgs() { Data = gridAdvUnits });
            //    }
            //}

            mTimeGridAdvHelper.AddAD(data, dt - TimeSpan.FromMilliseconds(ADLag), ts_ms);

            if (pushGridInfo != null)
            {
                if (TimeGridAdvEvent != null)
                {
                    var gridAdvUnits = mTimeGridAdvHelper.GetTimeGridAdv(pushGridInfo.dt, pushGridInfo.direction, pushGridInfo.grid_start, pushGridInfo.grid_len, PosOfGrid);

                    if (CorrectADs != null)
                    {
                        foreach (var gridAdvUnit in gridAdvUnits)
                        {
                            gridAdvUnit.ad = CorrectAD(gridAdvUnit.pos / PosOfGrid, gridAdvUnit.ad);
                        }
                    }
                    TimeGridAdvEvent(this, 
                        new TimeGridAdvEventArgs() { 
                            Data = gridAdvUnits, 
                            Marker = pushGridInfo.marker,
                            Direction = pushGridInfo.direction
                        });
                    
                }
                pushGridInfo = null;
            }

            //preTimeGrid_start = dt;
            //preTimeGrid_end = dt + TimeSpan.FromTicks(ticks);
            #endregion
        }


        /// <summary>
        /// 转为 grid 数据
        /// </summary>
        /// <param name="gridAdvUnits"></param>
        /// <param name="dat"></param>
        /// <param name="adLag">测试功能，让ad值滞后</param>
        public void ToGrid(IEnumerable<GridAdvUnit> gridAdvUnits, out int[] dat, int adLag=0) 
        {
            dat = mTimeGridAdvHelper.ToGrid(gridAdvUnits, PosOfGrid, SGrid.GRID_MAX_SIZE, adLag);
        }
        class PushGridInfo 
        {
            public DateTime dt;
            public int marker;
            public DIRECTION direction;
            public int grid_start;
            public int grid_len;
        }

        PushGridInfo pushGridInfo=null;
        void advPushGrid(DateTime dt, int marker, DIRECTION direction, int grid_start, int[] data)
        {
            mTimeGridAdvHelper.NotifyGridIStatusEvent(
                            direction, grid_start * PosOfGrid, data.Length * PosOfGrid, marker, dt, this);

            int index = (direction == Misc.DIRECTION.BACKWARD) ? 1 : 0;

            if (grid_start >= fGrid.data[index].Length)
                return;

            int grid_end = data.Length + grid_start - 1;

            if (grid_end >= fGrid.data[index].Length)
                grid_end = fGrid.data[index].Length - 1;

            int len = grid_end - grid_start + 1;


            Array.Copy(data, 0, fGrid.data[index], grid_start, len);

            //清空其它数据
            for (int i = 0; i < grid_start; i++)
            {
                fGrid.data[index][i] = Misc.MyBase.NULL_VALUE;
            }
            for (int i = (grid_end + 1); i < fGrid.data[index].Length; i++)
            {
                fGrid.data[index][i] = Misc.MyBase.NULL_VALUE;
            }
            if (GridEvent != null)
            {
                CorrectADs?.Invoke(direction, grid_start, data);
            }

            if (data.Length <= 1)
            {
                logger.Error("GRID 数据太少, 不触发 TimeGridAdv");
                return;
            }
            pushGridInfo = new PushGridInfo()
            {
                dt = dt,
                marker = marker,
                direction = direction,
                grid_start = grid_start,
                grid_len = data.Length
            };
        }

        void advPushMiniGrid(DIRECTION direction, int grid_start, int[] data) 
        {
            int index = (direction == Misc.DIRECTION.BACKWARD) ? 1 : 0;

            if (grid_start >= fGrid.data[index].Length)
                return;

            int grid_end = data.Length + grid_start - 1;



            if (grid_end >= fGrid.data[index].Length)
                grid_end = fGrid.data[index].Length - 1;

            int len = grid_end - grid_start + 1;

            Array.Copy(data, 0, fGrid.data[index], grid_start, len);

            //清空后面的数据
            if (direction == Misc.DIRECTION.BACKWARD)
            {
                for (int i = 0; i < grid_start; i++)
                {
                    fGrid.data[index][i] = Misc.MyBase.NULL_VALUE;
                }
            }
            else
            {
                for (int i = (grid_end + 1); i < fGrid.data[index].Length; i++)
                {
                    fGrid.data[index][i] = Misc.MyBase.NULL_VALUE;
                }
            }
            CorrectADs?.Invoke(direction, grid_start, data);

        }

        int CorrectAD(int grid, int ad)
        {
            int[] d = new int[1];
            d[0] = ad;
            CorrectADs(Misc.DIRECTION.FIX, grid, d);
            return d[0];
        }


    }
}