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 = 3200;
/// <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();
}
}
}