infinity_scan.c

Author:

Satofumi KAMIMURA

Id:

infinity_scan.c 1950 2011-05-07 08:18:39Z satofumi

#include "urg_ctrl.h"
#include <stdio.h>
#include <stdlib.h>


static void urg_exit(urg_t *urg, const char *message)
{
  printf("%s: %s\n", message, urg_error(urg));
  urg_disconnect(urg);

#ifdef MSC
  getchar();
#endif
  exit(1);
}


int main(int argc, char *argv[])
{
  int ret;
  int data_max;
  long* data;
  urg_parameter_t parameter;
  //int scan_msec;

#ifdef WINDOWS_OS
  const char device[] = "COM4"; /* For Windows */
#else
  const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif

  /* Connection */
  urg_t urg;
  ret = urg_connect(&urg, device, 115200);
  if (ret < 0) {
    urg_exit(&urg, "urg_connect()");
    exit(1);
  }

  /* Reserve for receive buffer */
  data_max = urg_dataMax(&urg);
  data = (long*)malloc(sizeof(long) * data_max);
  if (data == NULL) {
    fprintf(stderr, "data_max: %d\n", data_max);
    perror("data buffer");
    exit(1);
  }
  urg_parameters(&urg, &parameter);
  //scan_msec = urg_scanMsec(&urg);

  /* Request data */
  while (1) {
    int n;
    int timestamp;

    ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
    if (ret < 0) {
      urg_exit(&urg, "urg_requestData()");
    }

    n = urg_receiveData(&urg, data, data_max);
    timestamp = urg_recentTimestamp(&urg);

    printf("%d: %d", n, timestamp);
    if (n < 0) {
      printf(" %s", urg_error(&urg));
    }
    printf("\n");
  }
}