ln.ethercat/ln.ethercat/controller/Controller.cs

using System;
using System.Collections.Generic;
using System.Threading;
using ln.ethercat.controller.drives;
using ln.logging;

namespace ln.ethercat.controller
{

    public delegate void ControllerLogicDelegate(Controller controller);
    public delegate void ControllerStateChangeDelegate(Controller controller, ControllerStates newState);

    public enum ControllerStates {
        NONE,
        NOTREADY,
        FAULT,
        READY,
        DISABLING,
        ENABLING,
        OPERATIONAL
    }

    public class Controller
    {
        public event ControllerLogicDelegate ControllerLogic;
        public event ControllerStateChangeDelegate OnStateChanging;
        public event ControllerStateChangeDelegate OnStateChanged;

        public ECMaster ECMaster { get; }

        public bool IgnoreRemoteInterface { get; set; }

        List<ControlLoop> controlLoops = new List<ControlLoop>();
        public ControlLoop[] ControlLoops => controlLoops.ToArray();

        public ControllerStates ControllerState { get; private set; }

        public double ControllerLoopInterval { get; set; } = 0.1;
        public double ControllerLoopFrequency {
            get => 1.0 / ControllerLoopInterval;
            set => ControllerLoopInterval = 1.0 / value;
        }
        public long CycleCounter { get; private set; }

        Thread threadWatchdog;
        int wdogCounter;
        public int WatchdogReset { get; set; } = 5;

        public Controller(ECMaster ecMaster)
        {
            ECMaster = ecMaster;
        }

        public void Add(ControlLoop controlLoop) => controlLoops.Add(controlLoop);
        public void Remove(ControlLoop controlLoop) => controlLoops.Remove(controlLoop);

        public void Initialize()
        {
            CycleCounter = 0;

            if (!(threadWatchdog?.IsAlive ?? false))
            {
                threadWatchdog = new Thread(Watchdog);
                threadWatchdog.Start();
            }

            foreach (DriveController driveController in ECMaster.DriveControllers)
                driveController.Initialize();

        }

        void UpdateControllerState()
        {
            ControllerStates nextState = ControllerStates.OPERATIONAL;

            foreach (DriveController driveController in ECMaster.DriveControllers)
            {
                if (!driveController.IgnoredByController)
                {
                    ControllerStates driveState;
                    switch (driveController.DriveState)
                    {
                        case DriveStates.OPERATIONAL:
                            driveState = ControllerStates.OPERATIONAL;
                            break;
                        case DriveStates.ERROR:
                            driveState = ControllerStates.FAULT;
                            break;
                        case DriveStates.INIT:
                        case DriveStates.BOOT:
                            driveState = ControllerStates.NOTREADY;
                            break;
                        default:
                            driveState = ControllerStates.READY;
                            break;
                    }
                    if (driveState < nextState)
                        nextState = driveState;
                }
            }
            ChangeState(nextState);
        }

        void ChangeState(ControllerStates newState)
        {
            if (newState == ControllerState)
                return;

            OnStateChanging?.Invoke(this, newState);

            switch (newState)
            {
                case ControllerStates.FAULT:
                    DisableDrives();
                    break;
                case ControllerStates.DISABLING:
                    foreach (DriveController driveController in ECMaster.DriveControllers)
                        driveController.DisableDrive();
                    break;
                case ControllerStates.ENABLING:
                    foreach (DriveController driveController in ECMaster.DriveControllers)
                        if (!driveController.IgnoredByController)
                            driveController.EnableDrive();
                    break;
            }
            
            ControllerState = newState;
            OnStateChanged?.Invoke(this, newState);
        }

        public void DisableDrives()
        {
            ChangeState(ControllerStates.DISABLING);
        }

        public void EnableDrives()
        {
            if (ControllerState == ControllerStates.READY)
            {
                ChangeState(ControllerStates.ENABLING);
            } else {
                Logging.Log(LogLevel.INFO, "Controller: EnableDrives(): Current ControllerState={0}. Refusing to enable drives", ControllerState.ToString());
            }
        }

        public void ClearFaults()
        {
            foreach (DriveController driveController in ECMaster.DriveControllers)
            {
                if (driveController.DriveState == DriveStates.ERROR)
                    driveController.ClearFault();
            }
        }

        public void RemoteAction(CRActions action)
        {
            if (!IgnoreRemoteInterface)
            {
                switch (action)
                {
                    case CRActions.CLEARFAULT:
                        ClearFaults();
                        break;
                    case CRActions.DISABLE:
                        DisableDrives();
                        break;
                    case CRActions.ENABLE:
                        EnableDrives();
                        break;
                }
            }
        }

        public void RemoteUpdateTarget(int drive, double targetValue)
        {
            if (!IgnoreRemoteInterface)
            {
                targetValue = Math.Clamp(targetValue, 0, 1);

                if (drive < 0)
                    throw new ArgumentOutOfRangeException(nameof(targetValue));
                if (drive > ECMaster.DriveControllers.Length)
                {
                    Logging.Log(LogLevel.WARNING, "Controller: RemoteUpdateTarget(): trying to update non-existent drive {0}", drive);
                    return;
                }

                ECMaster.DriveControllers[drive].TargetValue = targetValue;
            }
        }


        public void Cycle()
        {
            wdogCounter = WatchdogReset;
            CycleCounter++;

            foreach (DriveController driveController in ECMaster.DriveControllers)
                driveController.UpdateStates();

            UpdateControllerState();

            foreach (ControlLoop controlLoop in controlLoops)
                controlLoop.Loop();

            ControllerLogic?.Invoke(this);

            foreach (DriveController driveController in ECMaster.DriveControllers)
                driveController.UpdateDrive();
        }

        void Watchdog()
        {
            while (true)
            {
                try
                {
                    Thread.Sleep(TimeSpan.FromSeconds(ControllerLoopInterval));
                    if ((wdogCounter--) < 0)
                    {
                        ChangeState(ControllerStates.FAULT);
                    }
                } catch (Exception e)
                {
                    Logging.Log(e);
                }
            }

        }

    }
}