src/hokuyo/gd_scan.c

   1 /*!
   2   \example gd_scan.c
   3
   4   \brief Sample of GD scan
   5
   6   Get distance data continuously using GD command
   7
   8   \author Satofumi KAMIMURA
   9
  10   $Id: gd_scan.c 1683 2010-02-10 10:28:05Z satofumi $
  11 */
  12
  13 #include "urg_ctrl.h"
  14 #include <stdio.h>
  15 #include <stdlib.h>
  16
  17
  18 static void urg_exit(urg_t *urg, const char *message)
  19 {
  20   printf("%s: %s\n", message, urg_error(urg));
  21   urg_disconnect(urg);
  22
  23 #ifdef MSC
  24   getchar();
  25 #endif
  26   exit(1);
  27 }
  28
  29
  30 int main(int argc, char *argv[])
  31 {
  32 #ifdef WINDOWS_OS
  33   const char device[] = "COM3"; /* For Windows */
  34 #else
  35   const char device[] = "/dev/ttyACM0"; /* For Linux */
  36 #endif
  37
  38   int data_max;
  39   long *data;
  40   int timestamp;
  41   int ret;
  42   int n;
  43   int i;
  44
  45   /* Connection */
  46   urg_t urg;
  47   ret = urg_connect(&urg, device, 115200);
  48   if (ret < 0) {
  49     urg_exit(&urg, "urg_connect()");
  50   }
  51
  52   /* Reserve for reception data */
  53   data_max = urg_dataMax(&urg);
  54   data = (long*)malloc(sizeof(long) * data_max);
  55   if (data == NULL) {
  56     perror("malloc");
  57     exit(1);
  58   }
  59
  60   /* Request for GD data */
  61   ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
  62   if (ret < 0) {
  63     urg_exit(&urg, "urg_requestData()");
  64   }
  65
  66   /* Reception */
  67   n = urg_receiveData(&urg, data, data_max);
  68   printf("# n = %d\n", n);
  69   if (n < 0) {
  70     urg_exit(&urg, "urg_receiveData()");
  71   }
  72
  73   /* Display */
  74   timestamp = urg_recentTimestamp(&urg);
  75   printf("# timestamp: %d\n", timestamp);
  76   for (i = 0; i < n; ++i) {
  77     /*Neglect the distance less than  urg_minDistance()  */
  78     printf("%d %ld, ", i, data[i]);
  79   }
  80   printf("\n");
  81
  82   urg_disconnect(&urg);
  83   free(data);
  84
  85 #ifdef MSC
  86   getchar();
  87 #endif
  88
  89   return 0;
  90 }