00001
00017 #include <math.h>
00018 #include <korebot/korebot.h>
00019
00020
00021 #define LRF_DEVICE "/dev/ttyACM0"
00022
00023
00024
00029 int main( int argc , char * argv[] )
00030 {
00031 int rc,i;
00032 int LRF_DeviceHandle;
00033
00034 float angle,x,y;
00035
00036
00037 kb_clrscr();
00038
00039 printf("Led Range Finder Small Example Program (C) K-Team S.A\r\n");
00040
00041
00042 kb_set_debug_level(2);
00043
00044
00045 if((rc = kb_init( argc , argv )) < 0 )
00046 {
00047 printf("\nERROR: port %s could not initialise libkorebot!\n");
00048 return -1;
00049 }
00050
00051 kb_lrf_Power_On();
00052
00053
00054 if ((LRF_DeviceHandle = kb_lrf_Init(LRF_DEVICE))<0)
00055 {
00056 printf("\nERROR: port %s could not initialise LRF!\n");
00057 return -2;
00058 }
00059
00060
00061 if (kb_lrf_GetDistances(LRF_DeviceHandle)<0)
00062 {
00063 printf("\nERROR: port %s could not initialise LRF!\n");
00064 kb_lrf_Close(LRF_DeviceHandle);
00065 return -3;
00066 }
00067
00068
00069
00070 printf("index dist[mm] angle[deg] x[mm] y[mm]\n");
00071
00072
00073
00074
00075
00076
00077 for (i=0;i<LRF_DATA_NB;i++)
00078 {
00079 angle= (i-LRF_DATA_NB/2+1024/4)*360.0/1024.0;
00080
00081
00082 x=kb_lrf_DistanceData[i]*cos(angle*M_PI/180.0);
00083 y=kb_lrf_DistanceData[i]*sin(angle*M_PI/180.0);
00084
00085 printf("%3d\t%4ld\t%+6.1f\t%+7.1f\t%+7.1f\n",i,kb_lrf_DistanceData[i],angle,x,y);
00086
00087 }
00088
00089
00090
00091 kb_lrf_Close(LRF_DeviceHandle);
00092
00093 return 0;
00094
00095 }