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; } /// <summary> /// 当前正向限位 /// </summary> public bool IsMaxLimit { get { if (m_currpos >= MaxPos) { return true; } return false; } } /// <summary> /// 当前反向限位 /// </summary> public bool IsMinLimit { get { if (m_currpos <= MinPos) { return true; } return false; } } /// <summary> /// 归0信号灯亮 /// </summary> public bool IsOrg { get { if (m_currpos <= 0) return true; else return false; } } const double mmpp = 0.1133;//mm/脉冲 public double Mmpp { get { return mmpp; } } const double maxmm = 1970;//机架最大位置 const double minmm = -50;//机架最小位置 /// <summary> /// 最大脉冲 /// </summary> public int MaxPos { get{ return (int)(maxmm / mmpp); } } /// <summary> /// 最小脉冲 /// </summary> public int MinPos { get { return (int)(minmm / mmpp); } } /// <summary> /// 目标速度 12m/min , 单位 脉冲/s /// </summary> const double m_targetspeed = (12 * 1000) / mmpp / 60; /// <summary> /// 加速度,启动的加速度, 单位 脉冲/s² 0->12m/min 只需要0.5s /// </summary> double m_acc; UInt32 acctime; /// <summary> /// 加速时间 ms /// </summary> public UInt32 AccTime { get { return acctime; } set { if (acctime != value) { acctime = value; m_acc = m_targetspeed / (acctime / 1000); } } } /// <summary> /// 减速度 刹车时的加速度 单位 脉冲/s² /// </summary> double m_dec; UInt32 dectime; /// <summary> /// 减速时间 ms /// </summary> public UInt32 DecTime { get { return dectime; } set { if (dectime != value) { dectime = value; m_dec = m_targetspeed / (dectime / 1000); } } } double velocity; /// <summary> /// 速度 脉冲/s /// </summary> public double Velocity { get { return velocity; } set { if (velocity != value) { velocity = value; } } } /// <summary> /// 最大速度 25m/min , 单位 脉冲/s /// </summary> const double m_maxspeed = (int)((25 * 1000) / mmpp / 60); /// <summary> /// 最大加速度,启动的加速度, 单位 脉冲/s² 0->25m/min 只需要0.1s /// </summary> const double m_maxacc = m_maxspeed / 0.1; /// <summary> /// 当前脉冲 /// </summary> double m_currpos = 990; /// <summary> /// 归0信号偏移 /// </summary> int m_offset = 0; public int Pos { get { return (int)(m_currpos) + m_offset; } set { m_offset = value - (int)(m_currpos); } } /// <summary> /// 脉冲/s /// </summary> double m_speed = 0; public int Speed { get { return (int)m_speed; } } 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() { MotorType = MOTORTYPE.VF0; Velocity = (12 * 1000) / mmpp / 60;//12m/min AccTime = 500;//0.5s DecTime = 500;//0.5s } 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 > Pos) { runto_order_sub = ORDER.FORW; } else if (runto_target_pos < Pos) { 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 OnPoll_Order(TimeSpan ts) { switch (order) { case ORDER.QUICK_RUNTO: { //不允许走过了 double target_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds; double acc = (target_speed - m_speed) / ts.TotalSeconds; if (Math.Abs(acc) > m_maxacc) { if (acc > 0) acc = m_maxacc; else acc = -m_maxacc; target_speed = acc * ts.TotalSeconds + m_speed; } if (Math.Abs(target_speed) > m_maxspeed) { if (target_speed > 0) target_speed = m_maxspeed; else target_speed = -m_maxspeed; } double currpos = m_currpos + target_speed * ts.TotalSeconds; if (runto_target_pos > m_currpos) { if (currpos > runto_target_pos)//走过了 { target_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds; } } else if (runto_target_pos < m_currpos) { if (currpos < runto_target_pos)//走过了 { target_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds; } } m_speed = target_speed; }break; case ORDER.RUNTO: { switch (runto_order_sub) { case ORDER.FORW: { m_speed += m_acc * ts.TotalSeconds; if (m_speed > Velocity) m_speed = Velocity; double currpos = m_currpos + m_speed * ts.TotalSeconds; if (currpos >= runto_target_pos) { runto_order_sub = ORDER.STOP; if (MotorType == MOTORTYPE.SERVO) m_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds; } } break; case ORDER.BACKW: { m_speed -= m_acc * ts.TotalSeconds; if (m_speed < -Velocity) m_speed = -Velocity; double currpos = m_currpos + m_speed * ts.TotalSeconds; if (currpos <= runto_target_pos) { runto_order_sub = ORDER.STOP; if (MotorType == MOTORTYPE.SERVO) m_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds; } } break; case ORDER.STOP: if (MotorType == MOTORTYPE.SERVO) { m_speed = 0; } else { 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; } }break; case ORDER.STOP: if (MotorType == MOTORTYPE.SERVO) { m_speed = 0; } else { 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.TotalSeconds; if (m_speed > Velocity) m_speed = Velocity; break; case ORDER.BACKW: m_speed -= m_acc * ts.TotalSeconds; if (m_speed < -Velocity) m_speed = -Velocity; break; } if(m_speed!=0) { state = STATE.RUNNING; double currpos = m_currpos + m_speed * ts.TotalSeconds; if ((m_speed > 0) && (currpos >= MaxPos)) { //限位了 m_currpos = MaxPos; m_speed = 0; state = STATE.STOP_BY_LIMIT; order = ORDER.IDLE; } else if ((m_speed < 0) && (currpos <= MinPos)) { m_speed = 0; m_currpos = MinPos; state = STATE.STOP_BY_LIMIT; order = ORDER.IDLE; } else { m_currpos = currpos; } } 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 int 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 int 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 int Velocity1; public int 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.Pos = 0; //归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.Pos = 0; //归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(); } } }