K-Team Khepera IV Stargazer, Руководство пользователя

Страница: 26 / 30

22
4.4.4 Software example
A small example of software controlling the KSG is enclosed below.
You can find its source code file in libkhepera-
VERSION/src/tests/kgazer_small_ex.c .
This sample example does not use calibration of the sensor due to
parallelism error of the ceiling and sensor plan. This results in x/y error
when
the robot rotates. See libkhepera-
VERSION/src/tests/kgazer_test.c for a full example with calibration.
#include <khepera/khepera.h>
int main(int argc, char *argv[])
{
char version[128];
int ret=0,c=0,i,idnum;
float angle,x,y,z,xc,yc;
// stargazer returned variables and corrected position
char cmode; // current mode of the Stargazer
/* reset the screen */
kb_clrscr();
printf("\nKhepera III Stargazer small example program\n");
/* Set the libkhepera debug level - Highly recommended for development. */
kb_set_debug_level(2);
// initialise the libkhepera
if((ret = kb_init( argc , argv )) < 0 )
return -1;
printf("\nInitialising Stargazer module; please wait!\n");
// initialise the Stargazer module
if ((ret=kb_stargazer_Init())!=0)
{
printf("\nError initialising the Stargazer (error = %d)!\n",ret);
kb_stargazer_Close();
return -2;
}
// get Stargazer firmware version
kb_gazer_get_version(version);
printf("\nStargazer version is: %s\n",version);
// read parameters values
printf("\nRead current Stargazer parameters:\n");
kb_gazer_get_landmark_number(&c);
printf(" landmark number: %d\n",c);
kb_gazer_get_ref_id(&c);
printf(" reference id : %d\n",c);
kb_gazer_get_landmark_type(&c);
printf(" landmark type : %s\n",kb_gazer_landmark_types[c]);
kb_gazer_get_landmark_mode(&c);
printf(" landmark mode : %s\n",kb_gazer_landmark_modes[c]);
kb_gazer_get_height_fix_mode(&c);
printf(" height fix mode: %s\n",kb_gazer_height_fix_modes[c]);
kb_gazer_start_computation(); // start computation of position
// continues on next page