diff --git a/Script/Model/ModelTrackControl.cs b/Script/Model/ModelTrackControl.cs index bb3942c..63f3b1c 100644 --- a/Script/Model/ModelTrackControl.cs +++ b/Script/Model/ModelTrackControl.cs @@ -4,23 +4,8 @@ using Tmds.Linux; public class ModelTrackControl : EGModule, IEGFramework { - public const int SlaveID = 3; - public bool IsInitTrack { set; get; } = false; - - public bool IsScanning { set; get; } = false; - public string PortName { set; get; } = ""; - - public Vector3 CurrentColorRGB { set; get; } = new Vector3(); - - public Vector3 CurrentColorHSV { set; get; } = new Vector3(); - - private IUnRegister ColorSensorEvent { set; get; } public int CurrentPosition { set; get; } = 0; - public int TargetPosition { set; get; } = 0; - - public StatusTrack MoveStatus { set; get; } = StatusTrack.Stop; - private ushort LowSpeed { set; get; } private ushort Speed { set; get; } @@ -35,108 +20,6 @@ public class ModelTrackControl : EGModule, IEGFramework ResetTarget(); } - public void EnableTrack() - { - this.EGSendMessage(new RequestTrackEnabled(), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - IsInitTrack = true; - } - - public void StartTrack() - { - if (!IsInitTrack) - { - EnableTrack(); - } - this.EGSendMessage(new RequestTrackSpeed(Speed), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - this.EGSendMessage(new RequestTrackMove(), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - } - - public void StartSlowTrack() - { - if (!IsInitTrack) - { - EnableTrack(); - } - this.EGSendMessage(new RequestTrackSpeed(LowSpeed), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - this.EGSendMessage(new RequestTrackMove(), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - } - - public void BackTrack() - { - if (!IsInitTrack) - { - EnableTrack(); - } - this.EGSendMessage(new RequestTrackSpeed(Speed), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - this.EGSendMessage(new RequestTrackBack(), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - } - - public void BackSlowTrack() - { - if (!IsInitTrack) - { - EnableTrack(); - } - this.EGSendMessage(new RequestTrackSpeed(LowSpeed), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - this.EGSendMessage(new RequestTrackBack(), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - } - - public void StopTrack() - { - if (!IsInitTrack) - { - EnableTrack(); - } - this.EGSendMessage(new RequestTrackSpeed(0), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - this.EGSendMessage(new RequestTrackStop(), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - } - - public void BreakTrack() - { - if (!IsInitTrack) - { - EnableTrack(); - } - this.EGSendMessage(new RequestTrackBreak(), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - this.EGSendMessage(new RequestTrackSpeed(0), this.GetModule().EngineSerialPort, ProtocolType.SerialPort); - } - - public void MoveToPosition(int position) - { - if (this.CurrentPosition == position) - { - EG.Print("Position is same.Nothing need to move!"); - return; - } - if (this.MoveStatus != StatusTrack.Stop) - { - EG.Print("Machine should be stop first!"); - return; - } - - if (!this.GetModule().Setting.ColorMapping.ContainsKey(position)) - { - EG.Print("Position invalid!Please add color first!"); - return; - } - - TargetPosition = position; - - if (this.CurrentPosition > position) - { - this.StartScanColor(); - this.StartTrack(); - this.MoveStatus = StatusTrack.Start; - } - - if (this.CurrentPosition < position) - { - this.StartScanColor(); - this.BackTrack(); - this.MoveStatus = StatusTrack.Back; - } - } - public void StartTarget() { if (CurrentRotation != 90) @@ -166,144 +49,6 @@ public class ModelTrackControl : EGModule, IEGFramework } } - public void StartScanColor() - { - this.EGSendMessage(new RequestColorStartScan(), this.GetModule().ColorSerialPort, ProtocolType.SerialPort); - ColorSensorEvent = this.EGRegisterMessageEvent(OnColorScanned); - IsScanning = true; - } - - public void StopScanColor() - { - this.EGSendMessage(new RequestColorStopScan(), this.GetModule().ColorSerialPort, ProtocolType.SerialPort); - //null exception - ColorSensorEvent.UnRegister(); - IsScanning = false; - } - - public void ResetTrack() - { - this.CurrentPosition = 0; - this.StartScanColor(); - this.StartTrack(); - this.MoveStatus = StatusTrack.Reset; - } - - private float HueCache { set; get; } - public void OnColorScanned(ResponseColorSensor color, string sender, ProtocolType type) - { - if (type == ProtocolType.SerialPort) - { - this.CurrentColorHSV = color.ColorHSV; - this.CurrentColorRGB = color.ColorRGB; - if (this.MoveStatus == StatusTrack.Start) - { - float currentHue = CurrentColorHSV.X; - if (MathF.Abs(HueCache - currentHue) > this.GetModule().Setting.ColorOffset) - { - HueCache = currentHue; - return; - } - float targetHue = this.GetModule().Setting.ColorMapping[TargetPosition].X; - bool compareResult = this.CompareHue(currentHue, targetHue); - if (compareResult) - { - this.StartSlowTrack(); - this.MoveStatus = StatusTrack.StartSlow; - BroadCastMoveStatus(); - } - } - - if (this.MoveStatus == StatusTrack.Back) - { - float currentHue = CurrentColorHSV.X; - if (MathF.Abs(HueCache - currentHue) > this.GetModule().Setting.ColorOffset) - { - HueCache = currentHue; - return; - } - float targetHue = this.GetModule().Setting.ColorMapping[TargetPosition].X; - bool compareResult = this.CompareHue(currentHue, targetHue); - if (compareResult) - { - this.BackSlowTrack(); - this.MoveStatus = StatusTrack.BackSlow; - BroadCastMoveStatus(); - } - } - - if (this.MoveStatus == StatusTrack.BackSlow || this.MoveStatus == StatusTrack.StartSlow) - { - float currentHue = CurrentColorHSV.X; - float targetHue = 4.0f; - bool compareResult = this.CompareHue(currentHue, targetHue); - if (compareResult) - { - this.BreakTrack(); - this.MoveStatus = StatusTrack.Stop; - this.StopScanColor(); - this.CurrentPosition = this.TargetPosition; - BroadCastMoveStatus(); - } - } - - if(this.MoveStatus == StatusTrack.Reset) - { - float currentHue = CurrentColorHSV.X; - float targetHue = this.GetModule().Setting.ColorMapping[0].X; - bool compareResult = this.CompareHue(currentHue, targetHue); - if (compareResult) - { - this.BreakTrack(); - this.MoveStatus = StatusTrack.Stop; - this.StopScanColor(); - this.CurrentPosition = 0; - BroadCastMoveStatus(); - } - } - - HueCache = CurrentColorHSV.X; - } - } - - public bool CompareHue(float hue1,float hue2) - { - if (hue1 > hue2 - this.GetModule().Setting.ColorOffset && - hue1 < hue2 + this.GetModule().Setting.ColorOffset) - { - return true; - } - else - { - return false; - } - } - - public Vector3 GetColor() - { - return CurrentColorRGB; - } - public Vector3 GetColorHSV() - { - return CurrentColorHSV; - } - - public void BroadCastMoveStatus() - { - if (this.EGTCPServer().LinkedIPs.ContainsKey(6557)) - { - foreach (string sender in this.EGTCPServer().LinkedIPs[6557]) - { - this.EGSendMessage(new RequestControlMoveStatus() - { - TargetID = this.GetModule().Setting.DeviceID, - FunctionCode = 301, - Position = this.GetModule().CurrentPosition, - Status = MoveStatus - }, sender, ProtocolType.TCPServer); - } - } - } } public enum StatusTrack