SimDrive_Servo.cs

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

namespace FLY.Simulation.Flyad7
{
    /// <summary>
    /// 伺服模拟器，有内部脉冲，和外部脉冲比例区别！！
    /// </summary>
    public interface ISimDrive_Servo:ISimDrive
    {
        /// <summary>
        /// 是正向！！！！
        /// </summary>
        bool IsForw{get;set;}

        /// <summary>
        /// 以额定速度，输出总脉冲， 此脉冲没有方向
        /// </summary>
        /// <param name="velocity">单位 pps</param>
        /// <param name="total_pos"></param>
        void Drive(int velocity, int total_pos);

        /// <summary>
        /// 停止输出
        /// </summary>
        void Stop();

        /// <summary>
        /// 剩余脉冲， 此脉冲没有方向
        /// </summary>
        int Surplus{get;}
    }
    
    /// <summary>
    /// 伺服模拟
    /// </summary>
    public class SimDrive_Servo:ISimDrive_Servo
    {
        const double mmpp=0.2;//0.2mm/pulse
        const int m_maxpos = 9200;/*2000*/ //3米
        const int m_minpos = -500;/*-100*/
        const double inpout=4;//输入脉冲/输出脉冲 
        //当前速度，单位 pos/s
        double m_speed = 0;
        
        double m_currpos = 853;//4500;/*当前脉冲*/

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

        /// <summary>
        /// 是正向！！！！
        /// </summary>
        public bool IsForw { get; set; }

        private double surplus = 0;
        /// <summary>
        /// 剩余脉冲， 此脉冲没有方向
        /// </summary>
        public int Surplus 
        {
            get { return (int)surplus; }
        }
        int m_inpos_dest;//目标位置


        public SimDrive_Servo()
        {
        }


        void OnPoll_Order(TimeSpan ts)
        {
            if (m_speed > 0)
            {
                double currpos;
                double s = m_speed * ts.TotalMinutes;
                if (surplus < s)
                    s = surplus;
                
                surplus -= s;

                if(IsForw)
                    currpos = m_currpos + s / inpout;
                else
                    currpos = m_currpos - s / inpout;

                if ((IsForw) && (currpos >= m_maxpos))
                {
                    //限位了
                    m_currpos = m_maxpos;
                    m_speed = 0;
                }
                else if ((!IsForw) && (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;
        }

        /// <summary>
        /// 以额定速度，输出总脉冲， 此脉冲没有方向
        /// </summary>
        /// <param name="velocity">单位 pps</param>
        /// <param name="total_pos"></param>
        public void Drive(int velocity, int total_pos) 
        {
            m_speed = velocity;
            surplus = total_pos;
        }

        /// <summary>
        /// 停止输出
        /// </summary>
        public void Stop() 
        {
            m_speed = 0;
        }



    }
}