SimDrive.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using FlyADBase;

namespace FLY.Simulation.Flyad7
{
    //public interface ISimDrive 
    //{
    //    MOTORTYPE MotorType{get;set;}
    //    bool IsMaxLimit{get;}
    //    bool IsMinLimit { get; }
    //    bool IsOrg { get; }
    //    int MaxPos { get; }
    //    int MinPos { get; }
    //    double Mmpp { get; }
    //    UInt32 AccTime { get; set; }
    //    UInt32 DecTime { get; set; }
    //    double Velocity { get; set; }
    //    int Pos { get; set; }
    //    int Speed { get; }
    //    SimDrive_STATE State { get; }

    //    void QuickRunto(int pos);
    //    void Forw();
    //    void Backw();
    //    void Stop();
    //    void OnPoll(DateTime now);
    //}


    public enum SimDrive_STATE 
    {
        STOPPING,
        STOP_BY_LIMIT,
        RUNNING
    }

    public class SimDrive 
    {
        public MOTORTYPE MotorType { get; set; } = MOTORTYPE.VF0;
        /// <summary>
        /// 当前正向限位
        /// </summary>
        public bool IsMaxLimit
        {
            get
            {
                if (PhysicalPos >= MaxPos)
                {
                    return true;
                }
                return false;
            }

        }
        /// <summary>
        /// 当前反向限位
        /// </summary>
        public bool IsMinLimit
        {
            get
            {
                if (PhysicalPos <= MinPos)
                {
                    return true;
                }
                return false;
            }
        }
        /// <summary>
        /// 归0信号灯亮
        /// </summary>
        public bool IsOrg
        {
            get
            {
                if (PhysicalPos <= OrgPos)
                    return true;
                else
                    return false;
            }
        }

        /// <summary>
        /// 最大脉冲，这个是内部位置
        /// </summary>
        public int MaxPos { get; set; } = 8000;

        /// <summary>
        /// 最小脉冲, 这个是内部位置
        /// </summary>
        public int MinPos { get; set; } = -100;

        public int OrgPos { get; set; } = 50;
        /// <summary>
        /// 目标速度 脉冲/s
        /// </summary>
        public double Velocity { get; set; } = 1000;
        /// <summary>
        /// 启动速度
        /// </summary>
        public double SVelocity { get; set; } = 50;
        /// <summary>
        /// 加速时间 ms
        /// </summary>
        public UInt32 AccTime { get; set; }

        /// <summary> 
        /// 减速时间 ms
        /// </summary>
        public UInt32 DecTime { get; set; }

        /// <summary>
        /// 当前脉冲
        /// </summary>
        double m_currpos => m_physicalPos + m_offset + PosOffset;
        double m_physicalPos = 990;
        /// <summary>
        /// 归0信号偏移
        /// </summary>
        int m_offset;
        public int CurrPos=> (int)m_currpos;

        public void Reset() {
            m_offset = -PhysicalPos;
        }
        public int PhysicalPos => (int)m_physicalPos;

        /// <summary>
        /// 
        /// </summary>
        public int PosOffset { get; set; }

        /// <summary>
        /// 当前 脉冲/s 
        /// </summary>
        public double CurrSpeed { get; private set; }


        public enum STATE
        {
            STOPPING,
            STOP_BY_LIMIT,
            RUNNING
        }
        public STATE state = STATE.STOPPING;

        public enum ORDER
        {
            IDLE,
            STOP,
            FORW,
            BACKW,
            /// <summary>
            /// 超快速,无视加速时间
            /// </summary>
            QUICK_RUNTO,
            /// <summary>
            /// 运行到
            /// </summary>
            RUNTO,
        }
        public ORDER order = ORDER.IDLE;
        ORDER runto_order_sub;
        public SimDrive() 
        {

        }


        int runto_target_pos;

        public void QuickRunto(int pos) 
        {
            order = ORDER.QUICK_RUNTO;
            state = STATE.RUNNING;
            runto_target_pos = pos;
        }
        public void Runto(int pos)
        {
            order = ORDER.RUNTO;
            state = STATE.RUNNING;
            runto_target_pos = pos;

            if (runto_target_pos > CurrPos)
            {
                runto_order_sub = ORDER.FORW;
            }
            else if (runto_target_pos < CurrPos)
            {
                runto_order_sub = ORDER.BACKW;
            }
            else
            {
                runto_order_sub = ORDER.STOP;
            }
        }
        public void Forw()
        {
            order = ORDER.FORW;
            state = STATE.RUNNING;
        }
        public void Backw() 
        {
            order = ORDER.BACKW;
            state = STATE.RUNNING;
        }
        public void Stop() 
        {
            order = ORDER.STOP;
            state = STATE.RUNNING;
        }

        void calSpeed_forw(TimeSpan ts) {
            if (CurrSpeed < Velocity)
            {
                double acc = (Velocity - SVelocity) / (AccTime / 1000.0);
                CurrSpeed += acc * ts.TotalSeconds;
                if (CurrSpeed < SVelocity)
                    CurrSpeed = SVelocity;
                else if (CurrSpeed > Velocity)
                    CurrSpeed = Velocity;
            }
        }
        void calSpeed_backw(TimeSpan ts)
        {
            if (CurrSpeed > -Velocity)
            {
                double acc = (Velocity - SVelocity) / (AccTime / 1000.0);

                CurrSpeed -= acc * ts.TotalSeconds;
                if (CurrSpeed > -SVelocity)
                    CurrSpeed = -SVelocity;
                else if (CurrSpeed < -Velocity)
                    CurrSpeed = -Velocity;
            }
        }
        void calSpeed_stop(TimeSpan ts) {
            double dec = (Velocity - SVelocity) / (DecTime / 1000.0);

            if (CurrSpeed > 0)
            {
                CurrSpeed -= dec * ts.TotalSeconds;
                if (CurrSpeed < SVelocity)
                    CurrSpeed = 0;
            }
            else
            {
                CurrSpeed += dec * ts.TotalSeconds;
                if (CurrSpeed > -SVelocity)
                    CurrSpeed = 0;
            }
        }
        void OnPoll_Order(TimeSpan ts)
        {
            switch (order)
            {
                case ORDER.QUICK_RUNTO:
                    {
                        //不允许走过了
                        double target_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds;
                        CurrSpeed = target_speed;
                    }break;
                case ORDER.RUNTO:
                    {
                        switch (runto_order_sub) 
                        {
                            case ORDER.FORW:
                                {
                                    calSpeed_forw(ts);

                                    double currpos = m_currpos + CurrSpeed * ts.TotalSeconds;
                                    if (currpos >= runto_target_pos)
                                    {
                                        runto_order_sub = ORDER.STOP;
                                        if (MotorType == MOTORTYPE.SERVO) {
                                            CurrSpeed = (runto_target_pos - m_currpos)/ ts.TotalSeconds;
                                        }
                                        
                                    }
                                } break;
                            case ORDER.BACKW:
                                {
                                    calSpeed_backw(ts);

                                    double currpos = m_currpos + CurrSpeed * ts.TotalSeconds;
                                    if (currpos <= runto_target_pos)
                                    {
                                        runto_order_sub = ORDER.STOP;
                                        if (MotorType == MOTORTYPE.SERVO)
                                        {
                                            CurrSpeed = (runto_target_pos - m_currpos) / ts.TotalSeconds;
                                        }
                                    }
                                } break;
                            case ORDER.STOP:

                                if (MotorType == MOTORTYPE.SERVO)
                                {
                                    CurrSpeed = 0;
                                }
                                else
                                {
                                    calSpeed_stop(ts);
                                }
                                break;
                        }
                    }break;

                case ORDER.STOP:

                    if (MotorType == MOTORTYPE.SERVO)
                    {
                        CurrSpeed = 0;
                    }
                    else
                    {
                        calSpeed_stop(ts);
                    }
                    break;
                case ORDER.FORW:
                    {
                        calSpeed_forw(ts);
                    }
                    break;
                case ORDER.BACKW:
                    {
                        calSpeed_backw(ts);
                    }
                    break;
            }

            if(CurrSpeed!=0)
            {
                state = STATE.RUNNING;
                double physicalPos = m_physicalPos + CurrSpeed * ts.TotalSeconds;
                
                if ((CurrSpeed > 0) && (physicalPos >= MaxPos))
                {
                    //限位了
                    m_physicalPos = MaxPos;
                    CurrSpeed = 0;
                    state = STATE.STOP_BY_LIMIT;
                    order = ORDER.IDLE;
                }
                else if ((CurrSpeed < 0) && (physicalPos <= MinPos))
                {
                    CurrSpeed = 0;
                    m_physicalPos = MinPos;
                    state = STATE.STOP_BY_LIMIT;
                    order = ORDER.IDLE;
                }
                else
                {
                    m_physicalPos = physicalPos;
                }
            }
            else
            {
                state = STATE.STOPPING;
                order = ORDER.IDLE;
            }
        }

        DateTime dtLast = DateTime.MinValue;
        public void OnPoll(DateTime now)
        {
            DateTime dt = now;
            if (dtLast != DateTime.MinValue)
            {
                TimeSpan ts = dt - dtLast;
                OnPoll_Order(ts);
            }
            dtLast = dt;
        }

    }
    public class OrderMan 
    {
        public delegate void FinishEventHandler(bool isNormalStop);
        public event FinishEventHandler FinishEvent;

        public Order_Runto order_runto = new Order_Runto();
        public Order_Stop order_stop = new Order_Stop();
        public Order_Org order_org = new Order_Org();
        public Order_Forward order_forw = new Order_Forward();
        public Order_Backward order_backw = new Order_Backward();
        public Order_Sync order_sync = new Order_Sync();
        public Order order;
        public void Init(FLYAD7 flyad, SimDrive simDrive) 
        {
            order_runto.Init(flyad, simDrive);
            order_stop.Init(flyad, simDrive);
            order_org.Init(flyad, simDrive);
            order_forw.Init(flyad, simDrive);
            order_backw.Init(flyad, simDrive);
            order_sync.Init(flyad, simDrive);
        }
        public void Stop()
        {
            order = order_stop;
            order.Start();
        }
        public void Org()
        {
            order = order_org;
            order.Start();
        }
        public void Forw()
        {
            order = order_forw;
            order.Start();
        }
        public void Backw()
        {
            order = order_backw;
            order.Start();
        }
        public void Runto(int dest)
        {
            order_runto.destpos = dest;
            order = order_runto;
            order.Start();
        }
        public void Sync() 
        {
            order = order_sync;
            order.Start();
        }
        public void OnPoll() 
        {
            if (order != null) 
            {
                Order.STATE state = order.OnPoll();
                if (state != Order.STATE.RUNNING) 
                {
                    if (FinishEvent != null)
                        FinishEvent((state == Order.STATE.STOP));
                    order = null;
                }
            }
        }
    }
    public abstract class Order 
    {
        public enum STATE
        {
            INIT,
            RUNNING,
            STOP,
            STOP_BY_LIMIT
        }
        public STATE State = STATE.INIT;
        public virtual void Start()
        {
            State = STATE.RUNNING;
        }
        public virtual STATE OnPoll()
        {
            State = STATE.STOP;
            return State;
        }

        public virtual void Init(FLYAD7 flyad, SimDrive simDrive) 
        {
            mflyad7 = flyad;
            mSimDrive = simDrive;
        }
        protected FLYAD7 mflyad7;
        protected SimDrive mSimDrive;
    }
    public class Order_Runto : Order 
    {
        public int destpos=0;
        public double Velocity;
        public override void Start() 
        {
            mflyad7.mPosGrid.Start();
            base.Start();
            mSimDrive.Velocity = Velocity;
            mSimDrive.Runto(destpos);
        }
        public override STATE OnPoll() 
        {
            switch (mSimDrive.state)
            {
                case SimDrive.STATE.STOPPING:
                    {
                        mflyad7.mPosGrid.End();
                        State = STATE.STOP;
                    } break;
                case SimDrive.STATE.STOP_BY_LIMIT:
                    {
                        State = STATE.STOP_BY_LIMIT;
                    } break;
            }
            return State;
        }
    }
    public class Order_Forward : Order 
    {
        public double Velocity;
        public override void Start()
        {
            State = STATE.RUNNING;
            mSimDrive.Velocity = Velocity;
            mSimDrive.Forw();
        }
        public override STATE OnPoll()
        {
            switch (mSimDrive.state)
            { 
                case SimDrive.STATE.STOPPING:
                    State = STATE.STOP;
                    break;
                case SimDrive.STATE.STOP_BY_LIMIT:
                    State = STATE.STOP_BY_LIMIT;
                    break;
            }
            
            return State;
        }
    }
    public class Order_Backward : Order_Forward 
    {

        public override void Start()
        {
            State = STATE.RUNNING;
            mSimDrive.Velocity = Velocity;
            mSimDrive.Backw();
        }
    }
    public class Order_Org : Order
    {
        private int order_adv_state = 0;
        public double Velocity1;
        public double Velocity2;
        public override void Start()
        {
            State = STATE.RUNNING;
            order_adv_state = 0;
        }
        public override STATE OnPoll()
        {
            switch (order_adv_state)
            {
                case 0:
                    if (!mSimDrive.IsOrg)
                    {
                        mSimDrive.Velocity = Velocity1;
                        mSimDrive.Backw();
                    }
                        order_adv_state = 1;
                    break;
                case 1:
                    if (mSimDrive.IsOrg)
                    {
                        mSimDrive.Stop(); 
                        order_adv_state = 2;
                    } break;
                case 2:
                    if (mSimDrive.state != SimDrive.STATE.RUNNING)
                    {
                        mSimDrive.Velocity = Velocity2;
                        mSimDrive.Forw();
                        order_adv_state = 3;
                    } break;
                case 3:
                    if (!mSimDrive.IsOrg)
                    {
                        mSimDrive.Stop();
                        order_adv_state = 4;
                    } break;
                case 4:
                    if (mSimDrive.state != SimDrive.STATE.RUNNING)
                    {
                        mSimDrive.Reset();
                        //归0完成
                        State = STATE.STOP;
                    } break;
            }
            return State;
        }
    }
    public class Order_Stop : Order_Forward
    {
        public override void Start()
        {
            State = STATE.RUNNING;
            mSimDrive.Stop();
        }
    }
    public class Order_Sync : Order
    {
        public List<ASyncOrder> mSyncOrderList;

        public override STATE OnPoll()
        {
            if (mSyncOrderList.Count() > 0)
            {
                ASyncOrder  o = mSyncOrderList.First();
                if (o.State == STATE.INIT)
                {

                    o.Init(mflyad7, mSimDrive);
                    o.Start();

                    if (CurrSyncOrderChangedEvent != null)
                    {
                        CurrSyncOrderChangedEvent(this);
                    }
                }
                o.OnPoll();
                if (mflyad7.Position <= -500)
                { 
                
                }
                if (o.State != STATE.RUNNING) 
                {
                    //完成一个
                    mSyncOrderList.RemoveAt(0);

                }
            }
            return STATE.RUNNING;
        }
        public override string ToString()
        {
            if (mSyncOrderList.Count > 0)
                return "Sync (" + mSyncOrderList.Count.ToString()+") " + mSyncOrderList.First().ToString();
            else
                return "Sync (0)";
        }
        public event Action<Order_Sync> CurrSyncOrderChangedEvent;
    }

    public abstract class ASyncOrder : Order
    {
        public Int32 marker;
        public override void Start()
        {
            base.Start();
            mflyad7.Marker = marker;
        }

    }
    public class SyncOrder_RunAtLC : ASyncOrder
    {
        public int pos2_begin;
        public int pos2_end;
        public int pos1lc_end;
        public bool hasDataGrid;


        private int pos1lc_begin;
        private double p;
        public override void Start()
        {

            base.Start();
            pos1lc_begin = mflyad7.Position + mflyad7.Pos1LCShift;
            if (hasDataGrid)
                mflyad7.mPosGrid.Start();
            p = ((double)(pos1lc_end - pos1lc_begin)) / (pos2_end - pos2_begin);
        }
        public override STATE OnPoll()
        {
            if (mflyad7.Position2 < pos2_begin)
            { 
                
            }
            else if(mflyad7.Position2 <= pos2_end)
            {
                int pos = (int)((mflyad7.Position2 - pos2_begin) * p)+ (pos1lc_begin - mflyad7.Pos1LCShift);

                mSimDrive.QuickRunto(pos);
            }
            else 
            {
                State = STATE.STOP;
                if (hasDataGrid)
                    mflyad7.mPosGrid.End();
            }
            return State;
        }
        public override string ToString()
        {
            return "RunAtLC (" + pos2_begin.ToString() + "," + pos2_end.ToString() + ") pos1lc=" + pos1lc_end.ToString() + "|" + marker.ToString();
        }
    }
    public class SyncOrder_Origin : ASyncOrder
    {
        private int order_adv_state = 0;
        public override void Start()
        {
            base.Start();
            order_adv_state = 0;
        }
        public override STATE OnPoll()
        {
            switch (order_adv_state)
            {
                case 0:
                    if (!mSimDrive.IsOrg)
                        mSimDrive.Backw();
                    order_adv_state = 1;
                    break;
                case 1:
                    if (mSimDrive.IsOrg)
                    {
                        mSimDrive.Stop();
                        order_adv_state = 2;
                    } break;
                case 2:
                    if (mSimDrive.state != SimDrive.STATE.RUNNING)
                    {
                        mSimDrive.Forw();
                        order_adv_state = 3;
                    } break;
                case 3:
                    if (!mSimDrive.IsOrg)
                    {
                        mSimDrive.Stop();
                        order_adv_state = 4;
                    } break;
                case 4:
                    if (mSimDrive.state != SimDrive.STATE.RUNNING)
                    {
                        mSimDrive.Reset();
                        //归0完成
                        State = STATE.STOP;
                    } break;
            }
            return State;
        }

        public override string ToString()
        {
            return "Origin |" + marker.ToString();
        }
    }
    public class SyncOrder_RunTo : ASyncOrder
    {
        public int pos1;
        public UInt32 velocity;
        public bool hasDataGrid;

        public override void Start()
        {
            mSimDrive.Velocity = velocity;
            mSimDrive.Runto(pos1);
            base.Start();
            if (hasDataGrid)
                mflyad7.mPosGrid.Start();
        }
        public override STATE OnPoll()
        {
            if (mSimDrive.state != SimDrive.STATE.RUNNING)
            {
                if (hasDataGrid)
                    mflyad7.mPosGrid.End();

                State = STATE.STOP;
            }
            return State;
        }
        public override string ToString()
        {
            return "RunTo pos1=" + pos1.ToString() + "|" + marker.ToString();
        }
    }
    public class SyncOrder_RunToLC : ASyncOrder
    {
        public int pos1lc;
        public UInt32 velocity;
        public bool hasDataGrid;

        public override void Start()
        {
            mSimDrive.Velocity = velocity;
            mSimDrive.Runto(pos1lc - mflyad7.Pos1LCShift);
            base.Start();
            if (hasDataGrid)
                mflyad7.mPosGrid.Start();
        }
        public override STATE OnPoll()
        {
            if (mSimDrive.state != SimDrive.STATE.RUNNING)
            {
                if (hasDataGrid)
                    mflyad7.mPosGrid.End();

                State = STATE.STOP;
            }
            return State;
        }
        public override string ToString()
        {
            return "RunToLC pos1lc=" + pos1lc.ToString() + " pos1lcshift="+ mflyad7.Pos1LCShift.ToString()+" |" + marker.ToString();
        }
    }
    public class SyncOrder_Wait : ASyncOrder
    {
        public int ms;
        DateTime dtstart;
        public override void Start()
        {
            dtstart = mflyad7.Now;
            base.Start();
        }
        public override STATE OnPoll()
        {
            if ((mflyad7.Now - dtstart).TotalMilliseconds >= ms)
            {
                State = STATE.STOP;
            }
            return State;
        }
        public override string ToString()
        {
            return "Wait ms=" + ms.ToString() + "|" + marker.ToString();
        }
    }
    

}