TargetService/Script/Model/ModelTrackControl.cs

using System.Numerics;
using EGFramework;
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; }

    public int CurrentRotation { set; get; } = 0;
    public override void Init()
    {
        // List<string> portList = this.EGSerialPort().RefreshSerialPort();
        CurrentPosition = this.GetModule<ModelActionStatus>().Status.Position;
        CurrentRotation = this.GetModule<ModelActionStatus>().Status.Rotate;
        LowSpeed = (ushort)this.GetModule<ModelParamSetting>().Setting.MoveLowSpeed;
        Speed = (ushort)this.GetModule<ModelParamSetting>().Setting.MoveSpeed;
        ResetTarget();
    }

    public void EnableTrack()
    {
        this.EGSendMessage(new RequestTrackEnabled(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
        IsInitTrack = true;
    }

    public void StartTrack()
    {
        if (!IsInitTrack)
        {
            EnableTrack();
        }
        this.EGSendMessage(new RequestTrackSpeed(Speed), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
        this.EGSendMessage(new RequestTrackMove(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
    }

    public void StartSlowTrack()
    {
        if (!IsInitTrack)
        {
            EnableTrack();
        }
        this.EGSendMessage(new RequestTrackSpeed(LowSpeed), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
        this.EGSendMessage(new RequestTrackMove(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
    }

    public void BackTrack()
    {
        if (!IsInitTrack)
        {
            EnableTrack();
        }
        this.EGSendMessage(new RequestTrackSpeed(Speed), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
        this.EGSendMessage(new RequestTrackBack(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
    }

    public void BackSlowTrack()
    {
        if (!IsInitTrack)
        {
            EnableTrack();
        }
        this.EGSendMessage(new RequestTrackSpeed(LowSpeed), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
        this.EGSendMessage(new RequestTrackBack(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
    }

    public void StopTrack()
    {
        if (!IsInitTrack)
        {
            EnableTrack();
        }
        this.EGSendMessage(new RequestTrackSpeed(0), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
        this.EGSendMessage(new RequestTrackStop(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
    }

    public void BreakTrack()
    {
        if (!IsInitTrack)
        {
            EnableTrack();
        }
        this.EGSendMessage(new RequestTrackBreak(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
        this.EGSendMessage(new RequestTrackSpeed(0), this.GetModule<ModelSerialTest>().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<ModelParamSetting>().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)
        {
            this.EGSendMessage(new RequestTargetControl(2000,500,false), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
            CurrentRotation = 90;
            this.GetModule<ModelActionStatus>().SetRotation(CurrentRotation);
        }
    }

    public void RevertTarget()
    {
        if (CurrentRotation != 0)
        {
            this.EGSendMessage(new RequestTargetControl(2000, 0, false), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
            CurrentRotation = 0;
            this.GetModule<ModelActionStatus>().SetRotation(CurrentRotation);
        }
    }
    public void ResetTarget()
    {
        if (CurrentRotation != 0)
        {
            this.EGSendMessage(new RequestTargetControl(2000, 500, true), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
            CurrentRotation = 0;
            this.GetModule<ModelActionStatus>().SetRotation(CurrentRotation);
        }
    }

    public void StartScanColor()
    {
        this.EGSendMessage(new RequestColorStartScan(), this.GetModule<ModelSerialTest>().ColorSerialPort, ProtocolType.SerialPort);
        ColorSensorEvent = this.EGRegisterMessageEvent<ResponseColorSensor>(OnColorScanned);
        IsScanning = true;
    }

    public void StopScanColor()
    {
        this.EGSendMessage(new RequestColorStopScan(), this.GetModule<ModelSerialTest>().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<ModelParamSetting>().Setting.ColorOffset)
                {
                    HueCache = currentHue;
                    return;
                }
                float targetHue = this.GetModule<ModelParamSetting>().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<ModelParamSetting>().Setting.ColorOffset)
                {
                    HueCache = currentHue;
                    return;
                }
                float targetHue = this.GetModule<ModelParamSetting>().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<ModelParamSetting>().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<ModelParamSetting>().Setting.ColorOffset &&
        hue1 < hue2 + this.GetModule<ModelParamSetting>().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<RequestControlMoveStatus>(new RequestControlMoveStatus()
                {
                    TargetID = this.GetModule<ModelParamSetting>().Setting.DeviceID,
                    FunctionCode = 301,
                    Position = this.GetModule<ModelTrackControl>().CurrentPosition,
                    Status = MoveStatus
                }, sender, ProtocolType.TCPServer);
            }
        }
    }
}

public enum StatusTrack
{
    Stop = 0x00,
    Start = 0x01,
    StartSlow = 0x02,
    Back = 0x03,
    BackSlow = 0x04,
    Reset = 0x05
}