247 lines
7.7 KiB
C#
247 lines
7.7 KiB
C#
|
|
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 bool DisableRemoteWatchdog { get; set; }
|
|
int remoteWatchdogCounter;
|
|
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 RemoteTriggerWatchdog() => remoteWatchdogCounter = 5;
|
|
|
|
public void RemoteUpdateTarget(int drive, double targetValue)
|
|
{
|
|
if (!IgnoreRemoteInterface)
|
|
{
|
|
targetValue = Math.Clamp(targetValue, -1, 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)
|
|
{
|
|
wdogCounter--;
|
|
} else
|
|
{
|
|
ChangeState(ControllerStates.FAULT);
|
|
}
|
|
|
|
if (remoteWatchdogCounter > 0)
|
|
{
|
|
remoteWatchdogCounter--;
|
|
} else if (!DisableRemoteWatchdog)
|
|
{
|
|
ChangeState(ControllerStates.FAULT);
|
|
}
|
|
} catch (Exception e)
|
|
{
|
|
Logging.Log(e);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
} |