SimDrive_VF0.cs

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

namespace FLY.Simulation.Flyad7
{
    public interface ISimDrive 
    {
        /// <summary>
        /// 输入，归0信号
        /// </summary>
        bool PIN_IN_ORG { get; }
        /// <summary>
        /// 输入，前进限位
        /// </summary>
        bool PIN_IN_LIMIT_FORW { get; }

        /// <summary>
        /// 输入，后退限位
        /// </summary>
        bool PIN_IN_LIMIT_BACKW { get; }
        /// <summary>
        /// 当前位置
        /// </summary>
        int Pos { get; }

        /// <summary>
        /// 复位位置
        /// </summary>
        void ResetPos();

        /// <summary>
        /// 
        /// </summary>
        /// <param name="now"></param>
        void OnPoll(DateTime now);
    }
    public interface ISimDrive_VF0 : ISimDrive
    {
        /// <summary>
        /// 输出， 前进
        /// </summary>
        bool PIN_OUT_FORW { get; set; }
        /// <summary>
        /// 输出，后退
        /// </summary>
        bool PIN_OUT_BACKW { get; set; }
    }
    /// <summary>
    /// VF0 模拟！！！！！
    /// </summary>
    public class SimDrive_VF0 : ISimDrive_VF0
    {
        const double mmpp=0.2;//0.2mm/pulse
        const int m_maxpos = 9200;//
        const int m_minpos = -500;
        //目标速度 单位 pos/s
        const double m_targetv = (6000 / mmpp) / 60; //6m/min

        //当前速度，单位 pos/s
        double m_speed = 0;

        
        double m_acc = m_targetv / 1;//pos/s^2
        double m_dec = m_targetv /1;// pos/s^2
        
        double m_currpos = 853;//内部脉冲，最后输出位 m_currpos+m_offset;
        
        int m_offset = 0;//ResetPos() 时， m_offset= - m_currpos;

        public int Pos
        {
            get
            {
                return (int)(m_currpos) + m_offset;
            }
        }
        public void ResetPos() 
        {
            m_offset =  - (int)(m_currpos);
        }

        private bool pin_forw = true;
        public bool PIN_OUT_FORW 
        {
            get { return pin_forw; }
            set {
                pin_forw = value;
            }
        }
        private bool pin_backw = true;
        public bool PIN_OUT_BACKW 
        {
            get { return pin_forw; }
            set
            {
                pin_backw = value;
            }
        }
        private bool pin_org = true;
        public bool PIN_IN_ORG
        {
            get { return pin_org; }
            set
            {
                pin_org = value;
            }
        }
        private bool pin_in_limit_forw = true;
        public bool PIN_IN_LIMIT_FORW
        {
            get { return pin_in_limit_forw; }
            set
            {
                pin_in_limit_forw = value;
            }
        }
        private bool pin_in_limit_backw = true;
        public bool PIN_IN_LIMIT_BACKW
        {
            get { return pin_in_limit_backw; }
            set
            {
                pin_in_limit_backw = value;
            }
        }        



        public SimDrive_VF0()
        {
            PIN_OUT_FORW = true;
            PIN_OUT_BACKW = true;
            PIN_IN_ORG = true;
            PIN_IN_LIMIT_FORW = true;
            PIN_IN_LIMIT_BACKW = true;


        }

        enum ORDER
        {
            STOP,
            FORW,
            BACKW
        }
        void OnPoll_Order(TimeSpan ts)
        {
            ORDER order = ORDER.STOP;
            if (PIN_OUT_FORW == false)
            { 
                //前进
                order = ORDER.FORW;
            }
            else if (PIN_OUT_BACKW == false)
            {
                //后退
                order = ORDER.BACKW;
            }

            switch (order)
            {
                case ORDER.STOP:
                   if (m_speed > 0)
                    {
                        m_speed -= m_dec * ts.TotalSeconds;
                        if (m_speed < 0)
                            m_speed = 0;
                    }
                    else
                    {
                        m_speed += m_dec * ts.TotalSeconds;
                        if (m_speed > 0)
                            m_speed = 0;
                    }
                    break;
                case ORDER.FORW:
                    m_speed += m_acc * ts.TotalMinutes;
                    if (m_speed > m_targetv)
                        m_speed = m_targetv;
                    break;
                case ORDER.BACKW:
                    m_speed -= m_acc * ts.TotalMinutes;
                    if (m_speed < -m_targetv)
                        m_speed = -m_targetv;
                    break;
            }
            if (m_speed != 0)
            {
                double currpos = m_currpos + m_speed * ts.TotalMinutes;

                if ((m_speed > 0) && (currpos >= m_maxpos))
                {
                    //限位了
                    m_currpos = m_maxpos;
                    m_speed = 0;
                }
                else if ((m_speed < 0) && (currpos <= m_minpos))
                {
                    m_speed = 0;
                    m_currpos = m_minpos;
                }
                else
                {
                    m_currpos = currpos;
                }


                if (m_currpos < 0)
                    PIN_IN_ORG = false;
                else
                    PIN_IN_ORG = true;

                if (m_currpos >= m_maxpos)
                    PIN_IN_LIMIT_FORW = false;
                else
                    PIN_IN_LIMIT_FORW = true;

                if (m_currpos<= m_minpos)
                    PIN_IN_LIMIT_BACKW = false;
                else
                    PIN_IN_LIMIT_BACKW = true;
            }
        }

        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;
        }

    }
}