/// <summary>
/// Checks any errors reported by the motion board. If there is an error, the motion board is immediately stopped
/// and the application is signaled to cleanup and exit.
/// </summary>
/// <param name="errorCode">The error code returned by the board during the last motion call</param>
private void CheckNonmodalErrors(int errorCode)
{
string errorMessage;
if (errorCode == 0)
{
return;
}
errorMessage = GetErrorMessage(errorCode, 0, 0);
if (errorMessage == null)
{
return;
}
logger.Error("[MotionControllerHulk] " + errorMessage);
if (Hulk.SystemStatus == Hulk.Status.Initializing)
{
// Program was still being initialized when this error was found. Signal the main program to terminate.
throw new Exception();
}
// Program was already initialized. Halt the motors and exit gracefully.
Hulk.Halt();
MainForm.ExitApplication(errorMessage);
}
/// <summary>
/// Instructs the Hulk to move to the MAYDAY position.
/// </summary>
/// <param name="sender">Not used</param>
/// <param name="e">Not used</param>
private static void maydayButton_Click(object sender, EventArgs e)
{
logger.Debug("Enter: maydayButton_Click(object, EventArgs)");
if (Hulk.CheckCommandAllowed())
{
ThreadPool.QueueUserWorkItem(new WaitCallback(Hulk.Mayday));
}
}
/// <summary>
/// Instructs the Hulk to begin the homing procedure.
/// </summary>
/// <param name="sender">Not used</param>
/// <param name="e">Not used</param>
private static void homeButton_Click(object sender, EventArgs e)
{
logger.Debug("Enter: homeButton_Click(object, EventArgs)");
if (Hulk.CheckCommandAllowed())
{
Hulk.Home();
}
}
/// <summary>
/// Checks whether the motion board has reported any modal (runtime) errors. If there is an error,
/// the motion board is immediately stopped and the application is signaled to cleanup and exit.
/// </summary>
internal override void CheckModalErrors()
{
int errorCode;
string newModalMessage;
string allModalMessages;
ushort communicationStatus;
ushort commandID;
ushort resourceID;
allModalMessages = "";
Motion.flex_read_csr_rtn(BoardId, out communicationStatus);
while ((communicationStatus & Motion.NIMC_MODAL_ERROR_MSG) != 0)
{
// There is at least one modal error. Loop through them.
// Get the modal error from the motion board's error stack.
Motion.flex_read_error_msg_rtn(BoardId, out commandID, out resourceID, out errorCode);
// Display the error.
newModalMessage = GetErrorMessage(errorCode, commandID, resourceID);
if (newModalMessage != null)
{
allModalMessages += "[MotionControllerHulk] " + newModalMessage;
}
// Look for the next error.
Motion.flex_read_csr_rtn(BoardId, out communicationStatus);
}
if (allModalMessages == "")
{
return;
}
logger.Error(allModalMessages);
if (Hulk.SystemStatus == Hulk.Status.Initializing)
{
// Program was still being initialized when this error was found. Signal the main program to terminate.
throw new Exception();
}
// Program was already initialized. Halt the motors and exit gracefully.
Hulk.Halt();
MainForm.ExitApplication(allModalMessages);
}
/// <summary>
/// Instructs the Hulk to take the motors offline.
/// </summary>
/// <param name="sender">Not used</param>
/// <param name="e">Not used</param>
private static void killButton_Click(object sender, EventArgs e)
{
logger.Debug("Enter: killButton_Click(object, EventArgs)");
Hulk.Kill();
}
/// <summary>
/// The main control loop.
/// </summary>
internal static void ControlHulk()
{
bool joystickMessageGiven;
bool lightMessageGiven;
logger.Debug("Enter: ControlHulk()");
logger.Info("Control loop starting.");
// Check frames-per-second once every second
frameTime = DateTime.Now.AddSeconds(1.0);
updateCounter = 0;
updateRate = 0;
joystickMessageGiven = false;
lightMessageGiven = false;
// Do the control loop until a signal is sent to stop
keepRunning = true;
do
{
// Record the frames-per-second
if (DateTime.Now.CompareTo(frameTime) >= 0)
{
updateRate = updateCounter;
frameTime = DateTime.Now.AddSeconds(1.0);
updateCounter = 1;
}
else
{
updateCounter++;
}
// Make sure that joystick input is possible
if (InputController.IsJoystickConnected)
{
joystickMessageGiven = false;
}
else
{
InputController.AcquireJoystick();
if (!InputController.IsJoystickConnected)
{
if (!joystickMessageGiven)
{
// if no joystick connection is detected, the following code needs to be executed just once. Piggyback on the joystickMessageGiven functionality to do this,
// since that message is logged once as well
// if a task is currently executing, halt execution
switch (status)
{
case Status.Task:
case Status.TaskExecuting:
case Status.Executing:
Hulk.Halt();
// Stop the currently executing task to prevent the task from continuing where it left off once the joystick is plugged back in
if (AppMain.CurrentTask != null)
{
AppMain.CurrentTask.StopTask();
}
status = Status.Idling;
break;
default:
break;
}
// put the HULK in the loading position
Hulk.Loading(null);
logger.Warn("Joystick must be plugged in to continue.");
joystickMessageGiven = true;
}
continue;
}
}
// See if safety lights show readiness for commands
if (DataController.Instance.IsLightGreen)
{
Hulk.Halt();
if (!lightMessageGiven)
{
logger.Warn("Light must be red to continue.");
lightMessageGiven = true;
}
Thread.Sleep(250);
continue;
}
else
{
if (lightMessageGiven)
{
logger.Info("Red light detected.");
lightMessageGiven = false;
}
}
// Is this program ready to send commands?
if (!DataController.Instance.PCReady)
{
Thread.Sleep(250);
continue;
}
// See if PLC is still ready to receive commands
if (!DataController.Instance.PLCReady)
{
Thread.Sleep(250);
continue;
}
// If needed, initiate mayday
if (DataController.Instance.IsMaydayPressed)
{
//Mayday(null);
ThreadPool.QueueUserWorkItem(new WaitCallback(Mayday));
}
// Check if the motion control board is reporting an error
MotionController.Instance.CheckModalErrors();
// Notify programs of current HULK motion
MotionController.Instance.ReadCurrentMotion(updateRate);
if (AppMain.CurrentTask != null)
{
AppMain.CurrentTask.LogBasicData();
AppMain.CurrentTask.UpdateNetworkClients();
}
switch (status)
{
case Status.Task:
AppMain.CurrentTask.ContinueTask();
break;
case Status.TaskExecuting:
if (MotionController.Instance.IsMoveComplete())
{
status = Status.Task;
}
break;
case Status.Stopping:
AppMain.CurrentTask.StopTask();
status = Status.Idling;
break;
case Status.Homing:
ContinueHoming();
break;
case Status.Executing:
if (MotionController.Instance.IsMoveComplete())
{
status = Status.Idling;
}
break;
}
} while (keepRunning);
// Program is exiting. Perform any necessary cleanup.
DataLogger.ReleaseDataLog();
InputController.ReleaseInputDevices();
Halt();
DataController.Instance.PCReady = false;
DataController.Instance.StopTasks();
logger.Info("Control loop has exited.");
}
