emcrsh.cc

CNC 的开放码,EMC2 V2.2.8版

      emcStatus->task.origin.c); // No rotational offsets
    strcat(context->outBuf, buf);
    }
  return rtNoError;
}

static cmdResponseType getRelActPos(char *s, connectionRecType *context)
{
  char *pRelActPosStr = "REL_ACT_POS";
  char buf[16];
  int axis;
  
  if (s == NULL) axis = -1; // Return all axes
  else axis = atoi(s);
  strcpy(context->outBuf, pRelActPosStr);
  if (axis != -1) {
    sprintf(buf, " %d", axis);
    strcat(context->outBuf, buf);
    }
  if ((axis == -1) || (axis == 0)) {
    sprintf(buf, " %f", emcStatus->motion.traj.actualPosition.tran.x -
      emcStatus->task.origin.tran.x);
    strcat(context->outBuf, buf);
    }
  if ((axis == -1) || (axis == 1)) {
    sprintf(buf, " %f", emcStatus->motion.traj.actualPosition.tran.y -
      emcStatus->task.origin.tran.y);
    strcat(context->outBuf, buf);
    }
  if ((axis == -1) || (axis == 2)) {
    sprintf(buf, " %f", emcStatus->motion.traj.actualPosition.tran.z -
      emcStatus->task.origin.tran.z);
    strcat(context->outBuf, buf);
    }
  if ((axis == -1) || (axis == 3)) {
    sprintf(buf, " %f", emcStatus->motion.traj.actualPosition.a -
      emcStatus->task.origin.a);
    strcat(context->outBuf, buf);
    }
  if ((axis == -1) || (axis == 4)) {
    sprintf(buf, " %f", emcStatus->motion.traj.actualPosition.b -
      emcStatus->task.origin.b);
    strcat(context->outBuf, buf);
    }
  if ((axis == -1) || (axis == 5)) {
    sprintf(buf, " %f", emcStatus->motion.traj.actualPosition.c -
      emcStatus->task.origin.c);
    strcat(context->outBuf, buf);
    }
  return rtNoError;
}

static cmdResponseType getJointPos(char *s, connectionRecType *context)
{
  char *pJointPos = "JOINT_POS";
  int axis, i;
  char buf[16];
  
  if (s == NULL) axis = -1; // Return all axes
  else axis = atoi(s);
  if (axis == -1) {
    strcpy(context->outBuf, pJointPos);
    for (i=0; i<6; i++) {
      sprintf(buf, " %f", emcStatus->motion.axis[i].input);
      strcat(context->outBuf, buf);
      }
    }
  else
    sprintf(context->outBuf, "%s %d %f", pJointPos, axis, emcStatus->motion.axis[axis].input);
  
  return rtNoError;
}

static cmdResponseType getPosOffset(char *s, connectionRecType *context)
{
  char *pPosOffset = "POS_OFFSET";
  char buf[16];
  
  if (s == NULL) {
    strcpy(context->outBuf, pPosOffset);
    sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.tran.x));
    strcat(context->outBuf, buf);
    sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.tran.y));
    strcat(context->outBuf, buf);
    sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.tran.z));
    strcat(context->outBuf, buf);
    sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.a));
    strcat(context->outBuf, buf);
    sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.b));
    strcat(context->outBuf, buf);
    sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.c));
    strcat(context->outBuf, buf);
    }
  else
    {
      switch (s[0]) {
        case 'X': sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.tran.x)); break;
        case 'Y': sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.tran.y)); break;
        case 'Z': sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.tran.z)); break;
        case 'A': 
        case 'R': sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.a)); break;
        case 'B': 
        case 'P': sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.b)); break;
        case 'C': 
        case 'W': sprintf(buf, " %f", convertLinearUnits(emcStatus->task.origin.c));
      }
      sprintf(context->outBuf, "%s %c %s", pPosOffset, s[0], buf);
    }
  return rtNoError;
}

static cmdResponseType getJointLimit(char *s, connectionRecType *context)
{
  char *pJointLimit = "JOINT_LIMIT";
  char buf[16];
  int axis, i;
  
  if (s == NULL) {
    strcpy(context->outBuf, pJointLimit);
    for (i=0; i<6; i++) {
      if (emcStatus->motion.axis[i].minHardLimit)
        strcpy(buf, " MINHARD");
      else
        if (emcStatus->motion.axis[i].minSoftLimit)
	  strcpy(buf, " MINSOFT");
	else
	  if (emcStatus->motion.axis[i].maxSoftLimit)
	    strcpy(buf, " MAXSOFT");
	  else
	    if (emcStatus->motion.axis[i].maxHardLimit)
	      strcpy(buf, " MAXHARD");
	    else strcpy(buf, "OK");
      strcat(context->outBuf, buf);
      }
    }
  else
    {
      axis = atoi(s);
      if (emcStatus->motion.axis[axis].minHardLimit)
        strcpy(buf, "MINHARD");
      else
        if (emcStatus->motion.axis[axis].minSoftLimit)
	  strcpy(buf, "MINSOFT");
	else
	  if (emcStatus->motion.axis[axis].maxSoftLimit)
	    strcpy(buf, "MAXSOFT");
	  else
	    if (emcStatus->motion.axis[axis].maxHardLimit)
	      strcpy(buf, "MAXHARD");
	    else strcpy(buf, "OK");
      sprintf(context->outBuf, "%s %d %s", pJointLimit, axis, buf);
    }
  return rtNoError;
}

static cmdResponseType getJointFault(char *s, connectionRecType *context)
{
  char *pJointFault = "JOINT_LIMIT";
  char buf[16];
  int axis, i;
  
  if (s == NULL) {
    strcpy(context->outBuf, pJointFault);
    for (i=0; i<6; i++) {
      if (emcStatus->motion.axis[i].fault)
        strcat(context->outBuf, " FAULT");
      else strcat(context->outBuf, " OK");
      }
    }
  else {
      axis = atoi(s);
      if (emcStatus->motion.axis[axis].fault)
        strcpy(buf, "FAULT");
      else strcpy(buf, "OK");
      sprintf(context->outBuf, "%s %d %s", pJointFault, axis, buf);
    }
  return rtNoError;
}

static cmdResponseType getOverrideLimits(char *s, connectionRecType *context)
{
  char *pOverrideLimits = "OVERRIDE_LIMITS %s";
  
  sprintf(context->outBuf, pOverrideLimits, emcStatus->motion.axis[0].overrideLimits);
  return rtNoError;
}

static cmdResponseType getJointHomed(char *s, connectionRecType *context)
{
  char *pJointHomed = "JOINT_HOMED";
  char buf[16];
  int axis, i;
  
  if (s == NULL) {
    strcpy(context->outBuf, pJointHomed);
    for (i=0; i<6; i++) {
      if (emcStatus->motion.axis[i].homed)
        strcat(context->outBuf, " YES");
      else strcat(context->outBuf, " NO");
      }
    }
  else {
      axis = atoi(s);
      if (emcStatus->motion.axis[axis].homed)
        strcpy(buf, "YES");
      else strcpy(buf, "NO");
      sprintf(context->outBuf, "%s %d %s", pJointHomed, axis, buf);
    }
  return rtNoError;
}

static cmdResponseType getProgram(char *s, connectionRecType *context)
{
  char *pProgram = "PROGRAM %s";
  
//  sprintf(outBuf, pProgram, progName);
//  printf("Program name = %s", emcStatus->task.file[0]);
  if (emcStatus->task.file[0] != 0)
    sprintf(context->outBuf, pProgram, emcStatus->task.file);
  return rtNoError;
}

static cmdResponseType getProgramLine(char *s, connectionRecType *context)
{
  char *pProgramLine = "PROGRAM_LINE %d";
  int lineNo;
  
  if ((programStartLine< 0) || (emcStatus->task.readLine < programStartLine))
    lineNo = emcStatus->task.readLine;
  else
    if (emcStatus->task.currentLine > 0)
      if ((emcStatus->task.motionLine > 0) && 
        (emcStatus->task.motionLine < emcStatus->task.currentLine))
	  lineNo = emcStatus->task.motionLine;
      else lineNo = emcStatus->task.currentLine;
    else lineNo = 0;
  sprintf(context->outBuf, pProgramLine, lineNo);
  return rtNoError;
}

static cmdResponseType getProgramStatus(char *s, connectionRecType *context)
{
  char *pProgramStatus = "PROGRAM_STATUS %s";
  
  switch (emcStatus->task.interpState) {
      case EMC_TASK_INTERP_READING:
      case EMC_TASK_INTERP_WAITING: sprintf(context->outBuf, pProgramStatus, "RUNNING"); break;
      case EMC_TASK_INTERP_PAUSED: sprintf(context->outBuf, pProgramStatus, "PAUSED"); break;
      default: sprintf(context->outBuf, pProgramStatus, "IDLE"); break;
    }
  return rtNoError;
}

static cmdResponseType getProgramCodes(char *s, connectionRecType *context)
{
  char *pProgramCodes = "PROGRAM_CODES ";
  char buf[256];
  int code, i;
  
  buf[0] = 0;
  strcpy(context->outBuf, pProgramCodes);
  for (i=1; i<ACTIVE_G_CODES; i++) {
      code = emcStatus->task.activeGCodes[i];
      if (code == -1) continue;
      if (code % 10) sprintf(buf, "G%.1f ", (double) code / 10.0);
      else sprintf(buf, "G%d ", code / 10);
      strcat(context->outBuf, buf);
    }
  sprintf(buf, "F%.0f ", emcStatus->task.activeSettings[1]);
  strcat(context->outBuf, buf);
  sprintf(buf, "S%.0f", fabs(emcStatus->task.activeSettings[2]));
  strcat(context->outBuf, buf);
  return rtNoError;
}

static cmdResponseType getJointType(char *s, connectionRecType *context)
{
  char *pJointType = "JOINT_TYPE";
  char buf[16];
  int axis, i;
  
  if (s == NULL) {
    strcpy(context->outBuf, pJointType);
    for (i=0; i<6; i++) {
      switch (emcStatus->motion.axis[i].axisType) {
        case EMC_AXIS_LINEAR: strcat(context->outBuf, " LINEAR"); break;
	case EMC_AXIS_ANGULAR: strcat(context->outBuf, " ANGULAR"); break;
	default: strcat(context->outBuf, "CUSTOM");
	}
      }
    }
  else {
      axis = atoi(s);
      switch (emcStatus->motion.axis[axis].axisType) {
        case EMC_AXIS_LINEAR: strcpy(buf, " LINEAR"); break;
	case EMC_AXIS_ANGULAR: strcpy(buf, " ANGULAR"); break;
	default: strcpy(buf, "CUSTOM");
	}
      sprintf(context->outBuf, "%s %d %s", pJointType, axis, buf);
    }
  return rtNoError;
}

static cmdResponseType getJointUnits(char *s, connectionRecType *context)
{
  char *pJointUnits = "JOINT_UNITS";
  char buf[16];
  int axis, i;
  
  if (s == NULL) {
    strcpy(context->outBuf, pJointUnits);
    for (i=0; i<6; i++) {
      switch (emcStatus->motion.axis[i].axisType) {
        case EMC_AXIS_LINEAR: 
	  if (CLOSE(emcStatus->motion.axis[i].units, 1.0, LINEAR_CLOSENESS))
	    strcat(context->outBuf, " MM");
	  else 
	    if (CLOSE(emcStatus->motion.axis[i].units, INCH_PER_MM,
	      LINEAR_CLOSENESS)) strcat(context->outBuf, " INCH");
	    else
	      if (CLOSE(emcStatus->motion.axis[i].units, CM_PER_MM,
	        LINEAR_CLOSENESS)) strcat(context->outBuf, " CM");
	      else strcat(context->outBuf, " CUSTOM");
	  break;
	case EMC_AXIS_ANGULAR:
	  if (CLOSE(emcStatus->motion.axis[i].units, 1.0, ANGULAR_CLOSENESS))
	    strcat(context->outBuf, " DEG");
	  else
  	    if (CLOSE(emcStatus->motion.axis[i].units, RAD_PER_DEG, ANGULAR_CLOSENESS))
	      strcat(context->outBuf, " RAD");
	    else
	      if (CLOSE(emcStatus->motion.axis[i].units, GRAD_PER_DEG, ANGULAR_CLOSENESS))
	        strcat(context->outBuf, " GRAD");
	      else strcat(context->outBuf, " CUSTOM");
	  break;
	default: strcat(context->outBuf, " CUSTOM");
	}
      }
    }
  else {
      axis = atoi(s);
      switch (emcStatus->motion.axis[axis].axisType) {
        case EMC_AXIS_LINEAR: 
	  if (CLOSE(emcStatus->motion.axis[axis].units, 1.0, LINEAR_CLOSENESS))
	    strcpy(buf, "MM");
	  else 
	    if (CLOSE(emcStatus->motion.axis[axis].units, INCH_PER_MM,
	      LINEAR_CLOSENESS)) strcpy(buf, "INCH");
	    else
	      if (CLOSE(emcStatus->motion.axis[axis].units, CM_PER_MM,
	        LINEAR_CLOSENESS)) strcpy(buf, "CM");
	      else strcpy(buf, "CUSTOM");
	  break;
	case EMC_AXIS_ANGULAR:
	  if (CLOSE(emcStatus->motion.axis[axis].units, 1.0, ANGULAR_CLOSENESS))
	    strcpy(buf, "DEG");
	  else
  	    if (CLOSE(emcStatus->motion.axis[axis].units, RAD_PER_DEG, ANGULAR_CLOSENESS))
	      strcpy(buf, "RAD");
	    else
	      if (CLOSE(emcStatus->motion.axis[axis].units, GRAD_PER_DEG, ANGULAR_CLOSENESS))
	        strcpy(buf, "GRAD");
	      else strcpy(buf, "CUSTOM");
	  break;
	default: strcpy(buf, "CUSTOM");
      sprintf(context->outBuf, "%s %d %s", pJointUnits, axis, buf);
      }
    }
  return rtNoError;
}

static cmdResponseType getProgramLinearUnits(char *s, connectionRecType *context)
{
  char *programUnits = "PROGRAM_UNITS %s";
  
  switch (emcStatus->task.programUnits) {
    case CANON_UNITS_INCHES: sprintf(context->outBuf, programUnits, "INCH"); break;
    case CANON_UNITS_MM: sprintf(context->outBuf, programUnits, "MM"); break;
    case CANON_UNITS_CM: sprintf(context->outBuf, programUnits, "CM"); break;
    default: sprintf(context->outBuf, programUnits, "CUSTOM"); break;
    }
  return rtNoError;
}

static cmdResponseType getProgramAngularUnits(char *s, connectionRecType *context)
{
  char *programAngularUnits = "PROGRAM_ANGULAR_UNITS %s";
  
  sprintf(context->outBuf, programAngularUnits, "DEG");
  return rtNoError;
}

static cmdResponseType getUserLinearUnits(char *s, connectionRecType *context)
{
  char *userLinearUnits = "USER_LINEAR_UNITS %s";
  
  if (CLOSE(emcStatus->motion.traj.linearUnits, 1.0, LINEAR_CLOSENESS))
    sprintf(context->outBuf, userLinearUnits, "MM");
  else
    if (CLOSE(emcStatus->motion.traj.linearUnits, INCH_PER_MM, LINEAR_CLOSENESS))
      sprintf(context->outBuf, userLinearUnits, "INCH");
    else
      if (CLOSE(emcStatus->motion.traj.linearUnits, CM_PER_MM, LINEAR_CLOSENESS))
        sprintf(context->outBuf, userLinearUnits, "CM");
      else
        sprintf(context->outBuf, userLinearUnits, "CUSTOM");
  return rtNoError;
}

static cmdResponseType getUserAngularUnits(char *s, connectionRecType *context)
{
  char *pUserAngularUnits = "USER_ANGULAR_UNITS %s";
  
  if (CLOSE(emcStatus->motion.traj.angularUnits, 1.0, ANGULAR_CLOSENESS))
    sprintf(context->outBuf, pUserAngularUnits, "DEG");
  else
    if (CLOSE(emcStatus->motion.traj.angularUnits, RAD_PER_DEG, ANGULAR_CLOSENESS))
      sprintf