SynqNetOk.cpp

Demonstrates verifying the SynqNet network.

/*  SynqNetOk.cpp

 Copyright(c) 1998-2009 by Robotic Systems Integration, Inc. All rights reserved.
 This software contains proprietary and confidential information of Robotic 
 Systems Integration, Inc. (RSI) and its suppliers. Except as may be set forth 
 in the license agreement under which this software is supplied, disclosure, 
 reproduction, or use with controls other than those provided by RSI or suppliers
 for RSI is strictly prohibited without the prior express written consent of 
 Robotic Systems Integration.

 Warning!  This is a sample program to assist in the integration of your motion 
 controller with your application.  It may not contain all of the logic and safety
 features that your application requires.

 For any questions regarding this sample code please visit www.roboticsys.com.
 ==================================================================================
 */

#include <string>
#include <iostream>
#include <sstream>
#include "rsi.h"

using namespace RSI::RapidCode::SynqNet;
using namespace std;

// functions for the sample application (main function)
bool IsSynqNetOkAtStartup(void);  // call this at startup
bool IsSynqNetOk(void);       // call this anytime



// global variables -- user should change to match his/her SynqNet system
MotionController* motionController;
const int EXPECTED_NODE_COUNT = (5);   // this will/should come from a configuration file
RSINodeType expectedNodeTypes[EXPECTED_NODE_COUNT] =   // these must be in correct order!
{
  RSINodeTypeMEI_SLICE,
  RSINodeTypeMEI_RMB,
  RSINodeTypeYASKAWA_SGDS,
  RSINodeTypeTRUST_TA805,
  RSINodeTypeKOLLMORGEN_S200,
};



// macros/class for logging/printing
#ifndef RELEASE
#define RSILOG(msg)   RSILog log(msg, __FUNCTION__, __LINE__)
#define RSIERR(msg)   RSILog log(msg, __FUNCTION__, __LINE__, true)
#endif

class RSILog
{
public:
  RSILog(char* message, char* function, long lineNumber)
  {
    printf("%s: %s Line %d\n\n", function, message, lineNumber);
  }
  RSILog(char* message, char* function, long lineNumber, bool error)
  {
    printf("Error: ");
    RSILog(message, function, lineNumber);
  }
};



bool CheckSynqNetState()
{
  bool result = true;

  RSISynqNetState state = motionController->SynqNetStateGet();

  if(state != ::RSISynqNetStateSYNQ)
  {
    RSILOG("SynqNet Network not in the required SYNQ state.");
    result = false;
  }
  return result;
}

bool CheckToFindFailedSqNode()
{
  bool result = true;
  long failedNodeMask = motionController->SynqNetFailedNodeMaskGet();

  if(failedNodeMask != 0)
  {      
    // Iterate through 32 nodes
    for(int i = 0; i < MEIXmpMAX_Axes; i++)
    {
      // if node failed log the failed node
      if(  (1<<i) & failedNodeMask)
      {
        stringstream temp;
        temp << "SynqNet Node " << i << " is not functioning";
        RSILOG((char*)temp.str().c_str());

        result = false;
      }
    }
  }

  return result;
}


void CheckToFindBadCableNumber()
{
  stringstream temp;
  temp << "Cable " << motionController->SynqNetIdleCableNumberGet() << " has failed check Link LEDs";
  RSILOG((char*)temp.str().c_str());
}


bool CheckIdleCableStatus()
{
  bool result = true;

  if(motionController->SynqNetIdleCableStatusGet() != ::RSISynqNetCableStatusGOOD)
  {
    result = false;
  }

  if(result == false)
  {
    CheckToFindBadCableNumber();
  }

  return result;
}


bool CheckSynqNetNetworkType()
{
  bool result = true;
  
  RSINetworkType type = motionController->SynqNetNetworkTypeGet();

  if(type != ::RSINetworkTypeRING)
  {
    // Should have been a ring log the problem
    RSILOG("SynqNet Network not in the required Ring topology.");
    result = false;
  }
  return result;
}


bool CheckSynqNetRecoveryEvent()
{
  bool result = true;

  // we want to return true if ok,  false if recovery event has happened (reverse of RapidCode method)
  result = !(motionController->SynqNetRecoveryEventGet()) ;

  if(result == false)
  {
    RSILOG("SynqNet Network had to recover from a network/cable error.");
  
    CheckToFindBadCableNumber();
    CheckToFindFailedSqNode();
  }

  return result; 
}

bool CheckSynqNetNodeCount()
{
  bool result = true;

  int nodeCount = motionController->SynqNetNodeCountGet();

  if(EXPECTED_NODE_COUNT != nodeCount)
  {
    stringstream temp;
    temp << "ExpectedCount: " << EXPECTED_NODE_COUNT << " does not match actual Node count: " << nodeCount;
    RSILOG((char*)temp.str().c_str());
    result = false;
  }
  return result;
}

bool CheckSynqNetNodeTypes()
{
  bool result = true;

  for(int i = 0; i < EXPECTED_NODE_COUNT; i++)
  {
    if(expectedNodeTypes[i] != motionController->IOGet(i)->SqNode->TypeGet())
    {
      stringstream temp;
      temp << "ExpectedType on Node " << i << " does not match actual Node type";
      RSILOG((char*)temp.str().c_str());
      result = false;
    }
  }

  return result;
}


void LogMotionControllerErrors(RapidCodeObject *rsiObject)
{
  RsiError *err;

  while(rsiObject->ErrorLogCountGet() > 0)
  {
    err = rsiObject->ErrorLogGet();
    RSIERR(err->text);
  }
}

// here's a function to call after initializing the MotionController
bool IsSynqNetOkAtStartup()
{
  bool synqNetOk = IsSynqNetOk();
  bool nodeCountResult = CheckSynqNetNodeCount();
  bool nodeTypesResult = CheckSynqNetNodeTypes();

  return synqNetOk && nodeCountResult && nodeTypesResult;
}

// here's a function you can call periodically in the application (if problems, user should be notified, then re-start)
bool IsSynqNetOk()
{
  bool stateResult = CheckSynqNetState();

  bool typeResult = CheckSynqNetNetworkType();

  bool recoveryResult = CheckSynqNetRecoveryEvent();

  bool idleCableResult = CheckIdleCableStatus();

  return stateResult && typeResult && recoveryResult && idleCableResult;
}


int SynqNetOkMain()
{
  motionController = MotionController::CreateFromBoard(0);

  // any errors when trying to create a MotionController are logged, not thrown as exceptions
  LogMotionControllerErrors(motionController);

  try
  {
    // if this fails, don't initialize I/O, Axes, etc
    bool synqNetOK = IsSynqNetOkAtStartup();

    if(synqNetOK == false)
    {
      RSILOG("SynqNet Network initialization had problems, going to reset and retry now.");

      // Attempt to fix
      motionController->Reset();

      // try again
      synqNetOK = IsSynqNetOkAtStartup();
    }

    if(synqNetOK)
    {
      // great, run application forever, or until SynqNet is not OK
      while(1)
      {
        // check to see if SynqNet is OK every second or so...
        motionController->OS->Sleep(1000);

        if(IsSynqNetOk())
        {
          // continue on
          RSILOG("SynqNet is ok...(Ctrl-C to exit)");
        }
        else
        {
          break;
        }
      }
    }

  }
  catch (RsiError *err)
  {
    RSIERR(err->text);
    return -1;
  }
  return 0;
}