emcrsh.cc

来自「CNC 的开放码,EMC2 V2.2.8版」· CC 代码 · 共 2,098 行 · 第 1/5 页

  scPlat, scIni, scDebug, scSetWait, scWait, scSetTimeout, scUpdate, scError,
  scOperatorDisplay, scOperatorText, scTime, scEStop, scMachine, scMode,
  scMist, scFlood, scLube, scLubeLevel, scSpindle, scBrake, scTool, scToolOffset,
  scLoadToolTable, scHome, scJogStop, scJog, scJogIncr, scFeedOverride,
  scAbsCmdPos, scAbsActPos, scRelCmdPos, scRelActPos, scJointPos, scPosOffset,
  scJointLimit, scJointFault, scJointHomed, scMDI, scTskPlanInit, scOpen, scRun,
  scPause, scResume, scStep, scAbort, scProgram, scProgramLine, scProgramStatus,
  scProgramCodes, scJointType, scJointUnits, scProgramUnits, scProgramLinearUnits,
  scProgramAngularUnits, scUserLinearUnits, scUserAngularUnits, scDisplayLinearUnits,
  scDisplayAngularUnits, scLinearUnitConversion,  scAngularUnitConversion, scProbeClear, 
  scProbeTripped, scProbeValue, scProbe, scTeleopEnable, scKinematicsType, scOverrideLimits, 
  scSpindleOverride, scOptionalStop, scUnknown
  } setCommandType;
  
typedef enum {
  rtNoError, rtHandledNoError, rtStandardError, rtCustomError, rtCustomHandledError
  } cmdResponseType;
  
typedef struct {  
  int cliSock;
  char hostName[80];
  char version[8];
  bool linked;
  bool echo;
  bool verbose;
  bool enabled;
  int commMode;
  int commProt;
  char inBuf[256];
  char outBuf[4096];
  char progName[256];} connectionRecType;

int server_sockfd, client_sockfd;
socklen_t server_len, client_len;
struct sockaddr_in server_address;
struct sockaddr_in client_address;
bool useSockets = true;
int tokenIdx;
char *delims = " \n\r\0";
int enabledConn = -1;

char *setCommands[] = {
  "ECHO", "VERBOSE", "ENABLE", "CONFIG", "COMM_MODE", "COMM_PROT", "INIFILE", "PLAT", "INI", "DEBUG",
  "SET_WAIT", "WAIT", "TIMEOUT", "UPDATE", "ERROR", "OPERATOR_DISPLAY", "OPERATOR_TEXT",
  "TIME", "ESTOP", "MACHINE", "MODE", "MIST", "FLOOD", "LUBE", "LUBE_LEVEL",
  "SPINDLE", "BRAKE", "TOOL", "TOOL_OFFSET", "LOAD_TOOL_TABLE", "HOME",
  "JOG_STOP", "JOG", "JOG_INCR", "FEED_OVERRIDE", "ABS_CMD_POS", "ABS_ACT_POS",
  "REL_CMD_POS", "REL_ACT_POS", "JOINT_POS", "POS_OFFSET", "JOINT_LIMIT",
  "JOINT_FAULT", "JOINT_HOMED", "MDI", "TASK_PLAN_INIT", "OPEN", "RUN", "PAUSE",
  "RESUME", "STEP", "ABORT", "PROGRAM", "PROGRAM_LINE", "PROGRAM_STATUS", "PROGRAM_CODES",
  "JOINT_TYPE", "JOINT_UNITS", "PROGRAM_UNITS", "PROGRAM_LINEAR_UNITS", "PROGRAM_ANGULAR_UNITS", 
  "USER_LINEAR_UNITS", "USER_ANGULAR_UNITS", "DISPLAY_LINEAR_UNITS", "DISPLAY_ANGULAR_UNITS", 
  "LINEAR_UNIT_CONVERSION", "ANGULAR_UNIT_CONVERSION", "PROBE_CLEAR", "PROBE_TRIPPED", 
  "PROBE_VALUE", "PROBE", "TELEOP_ENABLE", "KINEMATICS_TYPE", "OVERRIDE_LIMITS", 
  "SPINDLE_OVERRIDE", "OPTIONAL_STOP", ""};

char *commands[] = {"HELLO", "SET", "GET", "QUIT", "SHUTDOWN", "HELP", ""};

/* static char *skipWhite(char *s)
{
    while (isspace(*s)) {
	s++;
    }
    return s;
} */

static void thisQuit()
{
    EMC_NULL emc_null_msg;

    if (emcStatusBuffer != 0) {
	// wait until current message has been received
	emcCommandWaitReceived(emcCommandSerialNumber);
    }

    if (emcCommandBuffer != 0) {
	// send null message to reset serial number to original
	emc_null_msg.serial_number = saveEmcCommandSerialNumber;
	emcCommandBuffer->write(emc_null_msg);
    }
    // clean up NML buffers

    if (emcErrorBuffer != 0) {
	delete emcErrorBuffer;
	emcErrorBuffer = 0;
    }

    if (emcStatusBuffer != 0) {
	delete emcStatusBuffer;
	emcStatusBuffer = 0;
	emcStatus = 0;
    }

    if (emcCommandBuffer != 0) {
	delete emcCommandBuffer;
	emcCommandBuffer = 0;
    }

//    Tcl_Exit(0);
    exit(0);
}

static int initSockets()
{
  server_sockfd = socket(AF_INET, SOCK_STREAM, 0);
  server_address.sin_family = AF_INET;
  server_address.sin_addr.s_addr = htonl(INADDR_ANY);
  server_address.sin_port = htons(5007);
  server_len = sizeof(server_address);
  bind(server_sockfd, (struct sockaddr *)&server_address, server_len);
  listen(server_sockfd, 5);
  signal(SIGCHLD, SIG_IGN);
  return 0;
}

static void sigQuit(int sig)
{
    thisQuit();
}

static int sockWrite(connectionRecType *context)
{
   strcat(context->outBuf, "\r\n");
   write(context->cliSock, context->outBuf, strlen(context->outBuf));
   return 0;
}

static setCommandType lookupSetCommand(char *s)
{
  setCommandType i = scEcho;
  int temp;
  
  while (i < scUnknown) {
    if (strcmp(setCommands[i], s) == 0) return i;
//    (int)i += 1;
      temp = i;
      temp++;
      i = (setCommandType) temp;
    }
  return i;
}

static int commandHello(connectionRecType *context)
{
  char *pch;
  char *password = "EMC";
  
  pch = strtok(NULL, delims);
  if (pch == NULL) return -1;
  if (strcmp(pch, password) != 0) return -1;
  pch = strtok(NULL, delims);
  if (pch == NULL) return -1;
  strcpy(context->hostName, pch);  
  pch = strtok(NULL, delims);
  if (pch == NULL) return -1;
  context->linked = true;    
  strcpy(context->version, pch);
  printf("Connected to %s\n", context->hostName);
  return 0;
}

static int checkOnOff(char *s)
{
  static char *onStr = "ON";
  static char *offStr = "OFF";
  
  if (s == NULL) return -1;
  strupr(s);
  if (strcmp(s, onStr) == 0) return 0;
  if (strcmp(s, offStr) == 0) return 1;
  return -1;
}

static int checkBinaryASCII(char *s)
{
  static char *binaryStr = "BINARY";
  static char *ASCIIStr = "ASCII";
  
  if (s == NULL) return -1;
  strupr(s);
  if (strcmp(s, ASCIIStr) == 0) return 0;
  if (strcmp(s, binaryStr) == 0) return 1;
  return -1;
}

static int checkReceivedDoneNone(char *s)
{
  static char *receivedStr = "RECEIVED";
  static char *doneStr = "DONE";
  static char *noneStr = "NONE";
  
  if (s == NULL) return -1;
  strupr(s);
  if (strcmp(s, receivedStr) == 0) return 0;
  if (strcmp(s, doneStr) == 0) return 1;
  if (strcmp(s, noneStr) == 0) return 2;
  return -1;
}

static int checkNoneAuto(char *s)
{
  static char *noneStr = "NONE";
  static char *autoStr = "AUTO";
  
  if (s == NULL) return -1;
  strupr(s);
  if (strcmp(s, noneStr) == 0) return 0;
  if (strcmp(s, autoStr) == 0) return 1;
  return -1;
}

static int checkManualAutoMDI(char *s)
{
  static char *manualStr = "MANUAL";
  static char *autoStr = "AUTO";
  static char *mDIStr = "MDI";
  
  if (s == NULL) return -1;
  strupr(s);
  if (strcmp(s, manualStr) == 0) return 0;
  if (strcmp(s, autoStr) == 0) return 1;
  if (strcmp(s, mDIStr) == 0) return 2;
  return -1;
}

static int checkSpindleStr(char *s)
{
  static char *forwardStr = "FORWARD";
  static char *reverseStr = "REVERSE";
  static char *increaseStr = "INCREASE";
  static char *decreaseStr = "DECREASE";
  static char *constantStr = "CONSTANT";
  static char *offStr = "OFF";
  
  if (s == NULL) return -1;
  strupr(s);
  if (strcmp(s, forwardStr) == 0) return 0;
  if (strcmp(s, reverseStr) == 0) return 1;
  if (strcmp(s, increaseStr) == 0) return 2;
  if (strcmp(s, decreaseStr) == 0) return 3;
  if (strcmp(s, constantStr) == 0) return 4;
  if (strcmp(s, offStr) == 0) return 5;
  return -1;
}

static int checkConversionStr(char *s)
{
  static char *inchStr = "INCH";
  static char *mmStr = "MM";
  static char *cmStr = "CM";
  static char *autoStr = "AUTO";
  static char *customStr = "CUSTOM";
  
  if (s == NULL) return -1;
  strupr(s);
  if (strcmp(s, inchStr) == 0) return 0;
  if (strcmp(s, mmStr) == 0) return 1;
  if (strcmp(s, cmStr) == 0) return 2;
  if (strcmp(s, autoStr) == 0) return 3;
  if (strcmp(s, customStr) == 0) return 4;
  return -1;
}

static int checkAngularConversionStr(char *s)
{
  static char *degStr = "DEG";
  static char *radStr = "RAD";
  static char *gradStr = "GRAD";
  static char *autoStr = "AUTO";
  static char *customStr = "CUSTOM";
  
  if (s == NULL) return -1;
  strupr(s);
  if (strcmp(s, degStr) == 0) return 0;
  if (strcmp(s, radStr) == 0) return 1;
  if (strcmp(s, gradStr) == 0) return 2;
  if (strcmp(s, autoStr) == 0) return 3;
  if (strcmp(s, customStr) == 0) return 4;
  return -1;
}

static cmdResponseType setEcho(char *s, connectionRecType *context)
{
   
   switch (checkOnOff(s)) {
     case -1: return rtStandardError;
     case 0: context->echo = true; break;
     case 1: context->echo = false;
     }
   return rtNoError;
}

static cmdResponseType setVerbose(char *s, connectionRecType *context)
{
   
   switch (checkOnOff(s)) {
     case -1: return rtStandardError;
     case 0: context->verbose = true; break;
     case 1: context->verbose = false;
     }
   return rtNoError;
}

static cmdResponseType setEnable(char *s, connectionRecType *context)
{
   char *enablePWD = "EMCTOO";
  
   switch (checkOnOff(s)) {
     case -1: 
       if (strcmp(s, enablePWD) == 0) {
//         enable = true;
	 enabledConn = context->cliSock;
	 context->enabled = true;
//	 printf("Enabled Context = %d This context = %d\n", enabledConn, connId);
         return rtNoError;
	 }
       else return rtStandardError;
     case 1: 
       context->enabled = false;
       enabledConn = -1;
     }
   return rtNoError;
}

static cmdResponseType setConfig(char *s, connectionRecType *context)
{
  return rtNoError;
}

static cmdResponseType setCommMode(char *s, connectionRecType *context)
{
  int ret;
  
  ret = checkBinaryASCII(s);
  if (ret == -1) return rtStandardError;
  context->commMode = ret;
  return rtNoError;
}

static cmdResponseType setCommProt(char *s, connectionRecType *context)
{
  char *pVersion;
  
  pVersion = strtok(NULL, delims);
  if (pVersion == NULL) return rtStandardError;
  strcpy(context->version, pVersion);
  return rtNoError;
}

static cmdResponseType setDebug(char *s, connectionRecType *context)
{
  char *pLevel;
  int level;
  
  pLevel = strtok(NULL, delims);
  if (pLevel == NULL) return rtStandardError;
  if (sscanf(pLevel, "%i", &level) == -1) return rtStandardError;
  else sendDebug(level);
  return rtNoError;
}

static cmdResponseType setSetWait(char *s, connectionRecType *context)
{
   switch (checkReceivedDoneNone(s)) {
     case -1: return rtStandardError;
     case 0: emcWaitType = EMC_WAIT_RECEIVED; break;
     case 1: emcWaitType = EMC_WAIT_DONE; break;
     case 2: emcWaitType = EMC_WAIT_NONE; break;
     }
   return rtNoError;
}

static cmdResponseType setMachine(char *s, connectionRecType *context)
{
   switch (checkOnOff(s)) {
     case -1: return rtStandardError;
     case 0: sendMachineOn(); break;
     case 1: sendMachineOff();
     }
   return rtNoError;
}

static cmdResponseType setEStop(char *s, connectionRecType *context)
{
   switch (checkOnOff(s)) {
     case -1: return rtStandardError;
     case 0: sendEstop(); break;
     case 1: sendEstopReset();
     }
   return rtNoError;
}

static cmdResponseType setWait(char *s, connectionRecType *context)
{
  switch (checkReceivedDoneNone(s)) {
    case -1: return rtStandardError;
    case 0: 
      if (emcCommandWaitReceived(emcCommandSerialNumber) != 0) return rtStandardError;
      break;
    case 1: 
      if (emcCommandWaitDone(emcCommandSerialNumber) != 0) return rtStandardError;
      break;
    case 2: ;
    default: return rtStandardError;
    }
  return rtNoError;
}

static cmdResponseType setTimeout(char *s, connectionRecType *context)
{
  float Timeout;
  
  if (s == NULL) return rtStandardError;
  if (sscanf(s, "%f", &Timeout) < 1) return rtStandardError;
  emcTimeout = Timeout;
  return rtNoError;
}

static cmdResponseType setUpdate(char *s, connectionRecType *context)
{