---

testprobe.cc

Go to the documentation of this file.

extern "C" {
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "pmac.h"
}

#define LINELEN 80

struct NML_CARTESIAN
{
  NML_CARTESIAN() {}
  NML_CARTESIAN(double _x, double _y, double _z) {x = _x; y = _y; z = _z;}
  double x;
  double y;
  double z;
};

void nml_log_error(int error, const char *fmt = NULL, ...)
{
  printf("error %d\n", error);
}

static int nml_servo_set_scale_called[3] = {1, 1, 1};

struct SCALE
{
  double a;
};

struct SERVO_WM
{
  SCALE scale[3];
  double home_offset[3];
};

static SERVO_WM servo_wm;

/* Probing */

/* returns the most recent probe value */
NML_CARTESIAN servo_wm_probe()
{
  NML_CARTESIAN probe;

  /* make sure scale is set */
  if (!nml_servo_set_scale_called[0] ||
      !nml_servo_set_scale_called[1] ||
      !nml_servo_set_scale_called[2])
  {
    nml_log_error(1124);
    return probe;
  }

  if (pmac_probe_tripped(1) &&
      pmac_probe_tripped(2) &&
      pmac_probe_tripped(3))
  {
    probe.x = pmac_probe_value(1) / servo_wm.scale[0].a -
      servo_wm.home_offset[0];
    pmac_probe_clear(1);
    probe.y = pmac_probe_value(2) / servo_wm.scale[1].a -
      servo_wm.home_offset[1];
    pmac_probe_clear(2);
    probe.z = pmac_probe_value(3) / servo_wm.scale[2].a -
      servo_wm.home_offset[2];
    pmac_probe_clear(3);
  }
  else
  {
    /* no probe data-- fill with zero */
    probe.x = probe.y = probe.z = 0.0;
  }

  return probe;
}

int main()
{
  static char buffer[LINELEN];
  static char cmd[LINELEN];
  int axis;
  NML_CARTESIAN pos;

  servo_wm.scale[0].a = servo_wm.scale[1].a = servo_wm.scale[2].a =
    1578.4;
  servo_wm.home_offset[0] = servo_wm.home_offset[1] =
    servo_wm.home_offset[2] = -5.0;

  while (!feof(stdin))
  {
    fputs("probe> ", stdout);
    fflush(stdout);

    fgets(buffer, LINELEN, stdin);

    if (!strncmp(buffer, "on", 2))
    {
      printf("%d\n", pmac_probe_on());
      continue;
    }

    if (!strncmp(buffer, "off", 2))
    {
      printf("%d\n", pmac_probe_off());
      continue;
    }

    if (!strncmp(buffer, "trip", 1))
    {
      if (2 == sscanf(buffer, "%s%d", cmd, &axis))
      {
        printf("%d\n", pmac_probe_tripped(axis));
        continue;
      }
    }

    if (!strncmp(buffer, "value", 1))
    {
      if (2 == sscanf(buffer, "%s%d", cmd, &axis))
      {
        printf("%d\n", pmac_probe_value(axis));
        continue;
      }
    }

    if (!strncmp(buffer, "probe", 1))
    {
      pos = servo_wm_probe();
      printf("%f\t%f\t%f\n", pos.x, pos.y, pos.z);
      continue;
    }

    if (!strncmp(buffer, "quit", 1))
    {
      exit(0);
    }

    printf("?\n");
  }
}

---