Commit d9839bea authored by Nic Arnold's avatar Nic Arnold
Browse files

Updated most recent get_distance.c <should be the current version being used on the beagle bone>

parent 193420d0
// to run use ./get_disctance -e
#include <stdlib.h>
#include <sys/select.h>
#include <math.h>
#include <stdio.h>
#include <sys/time.h>
#include "urg_sensor.h"
#include "urg_utils.h"
#include "open_urg_sensor.h"
// Exits the program on enter
int kbhit()
{
struct timeval tv = { 0L, 0L };
fd_set fds;
FD_ZERO(&fds);
FD_SET(0, &fds);
return select(1, &fds, NULL, NULL, &tv);
}
static void print_data(urg_t *urg, long data[], long time_stamp,
unsigned short intensity[], int data_n) {
(void)urg;
(void)time_stamp;
int i;
long min_distance;
long max_distance;
long time_stamp_offset;
long t;
// realtime timer
char buffer[26];
struct timeval tp;
gettimeofday(&tp, 0);
time_t timer = tp.tv_sec;
struct tm *tm_info = localtime(&timer);
// Prints the X-Y coordinates for all the measurement points
urg_distance_min_max(urg, &min_distance, &max_distance);
for (i = 0; i < data_n; ++i) {
long l = data[i];
double radian;
long x;
long y;
float k; // count of rotations
if (radian == 0) {
k++;
}
if ((l <= min_distance) || (l >= max_distance)) {
continue;
}
radian = urg_index2rad(urg, i);
// once GPS coordinates are availible, need to switch to new function
x = (long)(l * cos(radian));
y = (long)(l * sin(radian));
printf("%ld %ld %ld %d %ld %f %f %s ", x, y, time_stamp, intensity[i], l, radian, k, buffer);
printf("%02d.%02d.%02d.%03d\n", tm_info->tm_hour, tm_info->tm_min, tm_info->tm_sec, tp.tv_usec/1000);
}
printf("\n");
//#endif
}
int main(int argc, char *argv[]) {
urg_t urg;
unsigned short *intensity = NULL;
long time_stamp;
long *data = NULL;
long set_angle = 90;
int i;
int n;
if (open_urg_sensor(&urg, argc, argv) < 0) {
return 1;
}
data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
if (!data) {
perror("urg_max_index()");
return 1;
}
intensity = malloc(urg_max_data_size(&urg) * sizeof(intensity[0]));
if (!intensity) {
perror("urg_max_index()");
return 1;
}
// Gets measurement data
// Case where the measurement range (start/end steps) is defined
//set_angle = strtol(argv[2], NULL, 3);
//if (argc < 2) printf("Set print angle. ex: 90 = +90deg,-90deg view \n");
/*
urg_set_scanning_parameter(&urg,
urg_deg2step(&urg, -1 * set_angle),
urg_deg2step(&urg, +1 * set_angle), 0);
*/
urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, 0);
printf("x,y,urg_time,i,real_time,pc,sensor\n");
while (!kbhit()) {
n = urg_get_distance_intensity(&urg, data, intensity, &time_stamp);
if (n <= 0) {
printf("urg_get_distance: %s\n", urg_error(&urg));
free(data);
urg_close(&urg);
return 1;
}
print_data(&urg, data, time_stamp, intensity, n);
}
//printf("Process Stopped.\n");
// Disconnects
free(data);
free(intensity);
urg_close(&urg);
#if defined(URG_MSC)
getchar();
#endif
return 0;
}
// to run use ./get_disctance -e #include <stdlib.h> #include <sys/select.h> #include <math.h> #include <stdio.h> #include <sys/time.h> #include <time.h> #include "urg_sensor.h" #include "urg_utils.h" #include "open_urg_sensor.h" // Exits the program on enter int kbhit() { struct timeval tv = { 0L, 0L }; fd_set fds; FD_ZERO(&fds); FD_SET(0, &fds); return select(1, &fds, NULL, NULL, &tv); } static void print_data(urg_t *urg, long data[], long time_stamp, unsigned short intensity[], int data_n) { (void)urg; (void)time_stamp; int i; long min_distance; long max_distance; long time_stamp_offset; long t; // realtime timer char buffer[26]; struct timeval tp; gettimeofday(&tp, 0); time_t timer = tp.tv_sec; struct tm *tm_info = localtime(&timer); // Prints the X-Y coordinates for all the measurement points urg_distance_min_max(urg, &min_distance, &max_distance); for (i = 0; i < data_n; ++i) { long l = data[i]; double radian; long x; long y; float k; // count of rotations if (radian == 0) { k++; } if ((l <= min_distance) || (l >= max_distance)) { continue; } radian = urg_index2rad(urg, i); z = (long)(l * cos(radian)); y = (long)(l * sin(radian)); printf("%ld,%ld,%ld,%d,%ld,%f,%f,%s", x, y, time_stamp, intensity[i], l, radian, k, buffer); printf("%02d.%02d.%02d.%03d\n", tm_info->tm_hour, tm_info->tm_min, tm_info->tm_sec, tp.tv_usec/1000); } printf("\n"); //#endif } int main(int argc, char *argv[]) { urg_t urg; unsigned short *intensity = NULL; long time_stamp; long *data = NULL; long set_angle = 90; int i; int n; fprintf(stderr, "Waiting for Ethernet port to become available (30s)..."); sleep(30); if (open_urg_sensor(&urg, argc, argv) < 0) { return 1; } data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0])); if (!data) { perror("urg_max_index()"); return 1; } intensity = malloc(urg_max_data_size(&urg) * sizeof(intensity[0])); if (!intensity) { perror("urg_max_index()"); return 1; } // Gets measurement data // Case where the measurement range (start/end steps) is defined //set_angle = strtol(argv[2], NULL, 3); //if (argc < 2) printf("Set print angle. ex: 90 = +90deg,-90deg view \n"); /* urg_set_scanning_parameter(&urg, urg_deg2step(&urg, -1 * set_angle), urg_deg2step(&urg, +1 * set_angle), 0); */ urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, 0); printf("z,y,urg_time,i,pc,sensor,time\n"); while (!kbhit()) { n = urg_get_distance_intensity(&urg, data, intensity, &time_stamp); if (n <= 0) { printf("urg_get_distance: %s\n", urg_error(&urg)); free(data); urg_close(&urg); return 1; } print_data(&urg, data, time_stamp, intensity, n); } //printf("Process Stopped.\n"); // Disconnects free(data); free(intensity); urg_close(&urg); #if defined(URG_MSC) getchar(); #endif return 0; }
\ No newline at end of file
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment