00001
00015 #include <korebot/korebot.h>
00016
00017
00018 int main(int argc, char *argv[])
00019 {
00020 char version[128];
00021 int ret=0,c=0,i,idnum;
00022 double angle,x,y,z,xc,yc;
00023 char cmode;
00024
00025
00026 kb_clrscr();
00027
00028 printf("\nKhepera III Stargazer small example program\n");
00029
00030
00031 kb_set_debug_level(2);
00032
00033
00034 if((ret = kb_init( argc , argv )) < 0 )
00035 return -1;
00036
00037 printf("\nInitialising Stargazer module; please wait!\n");
00038
00039
00040 if ((ret=kb_stargazer_Init())!=0)
00041 {
00042 printf("\nError initialising the Stargazer (error = %d)!\n",ret);
00043 kb_stargazer_Close();
00044 return -2;
00045 }
00046
00047
00048 kb_gazer_get_version(version);
00049 printf("\nStargazer version is: %s\n",version);
00050
00051
00052 printf("\nRead current Stargazer parameters:\n");
00053
00054 kb_gazer_get_landmark_number(&c);
00055 printf(" landmark number: %d\n",c);
00056
00057 kb_gazer_get_ref_id(&c);
00058 printf(" reference id : %d\n",c);
00059
00060 kb_gazer_get_landmark_type(&c);
00061 printf(" landmark type : %s\n",kb_gazer_landmark_types[c]);
00062
00063 kb_gazer_get_landmark_mode(&c);
00064 printf(" landmark mode : %s\n",kb_gazer_landmark_modes[c]);
00065
00066 kb_gazer_get_height_fix_mode(&c);
00067 printf(" height fix mode: %s\n",kb_gazer_height_fix_modes[c]);
00068
00069 kb_gazer_start_computation();
00070
00071 for (i=0;i<10; i++)
00072 {
00073
00074 ret=kb_stargazer_read_data(&x,&y,&z,&angle,&idnum,&cmode,0);
00075
00076 switch(ret)
00077 {
00078 case 0:
00079 printf("data %d: [cm,deg] x= %+6.1f y= %+6.1f angle= %+6.1f height= %+6.1f idnum= %4d mode: %c\n",i,x,y,angle,z,idnum,cmode);
00080 break;
00081 case -3:
00082 fprintf(stderr,"\nERROR: read error: buffer too short, leaving!\n");
00083 break;
00084 case -6:
00085 printf("The sensor does not see any landmark!\n");
00086 break;
00087 case -8:
00088 fprintf(stderr,"\nERROR: no data received!\n");
00089 break;
00090 default:
00091 fprintf(stderr,"\nERROR: read error number %d!\n",ret);
00092 }
00093
00094 usleep(100000);
00095 }
00096
00097 kb_stargazer_Close();
00098 return 0;
00099 }
00100