This module provides example to interface with the Stargazer for korebot:
Go to the source code of this file.
Defines | |
#define | NB_COLUMNS_MAX 80 |
#define | NB_LINES_MAX 50 |
#define | KEYS_TEXT_EXT_LENGTH (158-NB_COLUMNS_MAX) |
#define | DEFAULT_X_FACTOR (2.0*100.0) |
#define | DEFAULT_Y_FACTOR (2.0*100.0) |
#define | MAX_X_FACTOR (2.0*400.0) |
#define | MAX_Y_FACTOR (2.0*400.0) |
#define | MIN_X_FACTOR (2.0*20.0) |
#define | MIN_Y_FACTOR (2.0*20.0) |
#define | MEDIUM_SCALE 80 |
#define | BIGGEST_SCALE 240 |
#define | ZOOM_FACTOR 1.25 |
#define | MOVE_ANGLE_DIR_THRESH 10.0 |
#define | END_ANGLE_GOAL_THRESH 5.0 |
#define | END_POS_GOAL_THRESH 4.5 |
#define | FAST_SPEED_POS_THRESH 15.0 |
#define | MOVE_SLOW_SPEED 5000 |
#define | MOVE_FAST_SPEED 15000 |
#define | ROTATE_LOW_SPEED 2500 |
#define | ROTATE_HIGH_SPEED 5500 |
#define | ROTATE_ANGLE_TRESH 30.0 |
#define | ROUND(dbl) dbl >= 0.0 ? (int)(dbl + 0.5) : ((dbl - (double)(int)dbl) <= -0.5 ? (int)dbl : (int)(dbl - 0.5)) |
#define | DEFAULT_SPEED 10000 |
#define | MAX_SPEED 43000 |
#define | MIN_SPEED 2000 |
#define | SPEED_FACTOR 1.25 |
#define | DATA_FILE_NAME "data_corr.csv" |
Functions | |
int | initMot (knet_dev_t *hDev) |
int | initKH3 (void) |
int | set_Stargazer_parameters (int *nb_land, int *ref_land, int *land_type, int *land_mode) |
double | diff_angles (double a, double b) |
double | sum_angles (double a, double b) |
int | goto_xya (double curr_x, double curr_y, double curr_a, double goal_x, double goal_y, double goal_a, double *dist2goal, double *angle2goal) |
long long | timeval_diff (struct timeval *difference, struct timeval *end_time, struct timeval *start_time) |
int | main (int argc, char *argv[]) |
Variables | |
static double | x_factor = DEFAULT_X_FACTOR |
static double | y_factor = DEFAULT_Y_FACTOR |
static knet_dev_t * | mot1 |
static knet_dev_t * | mot2 |
static knet_dev_t * | dsPic |
This module provides example to interface with the Stargazer for korebot:
Stargazer example for Khepera 3 robot
Definition in file kgazer_test.c.
#define BIGGEST_SCALE 240 |
Definition at line 39 of file kgazer_test.c.
Referenced by main().
#define DATA_FILE_NAME "data_corr.csv" |
Definition at line 71 of file kgazer_test.c.
Referenced by main().
#define DEFAULT_SPEED 10000 |
Definition at line 65 of file kgazer_test.c.
Referenced by main().
#define DEFAULT_X_FACTOR (2.0*100.0) |
Definition at line 30 of file kgazer_test.c.
Referenced by lrfmeasure(), and main().
#define DEFAULT_Y_FACTOR (2.0*100.0) |
Definition at line 31 of file kgazer_test.c.
#define END_ANGLE_GOAL_THRESH 5.0 |
Definition at line 51 of file kgazer_test.c.
Referenced by goto_xya().
#define END_POS_GOAL_THRESH 4.5 |
Definition at line 52 of file kgazer_test.c.
Referenced by goto_xya().
#define FAST_SPEED_POS_THRESH 15.0 |
Definition at line 53 of file kgazer_test.c.
Referenced by goto_xya().
#define KEYS_TEXT_EXT_LENGTH (158-NB_COLUMNS_MAX) |
Definition at line 27 of file kgazer_test.c.
Referenced by main().
#define MAX_SPEED 43000 |
Definition at line 66 of file kgazer_test.c.
Referenced by main().
#define MAX_X_FACTOR (2.0*400.0) |
Definition at line 32 of file kgazer_test.c.
Referenced by lrfmeasure(), and main().
#define MAX_Y_FACTOR (2.0*400.0) |
Definition at line 33 of file kgazer_test.c.
Referenced by lrfmeasure(), and main().
#define MEDIUM_SCALE 80 |
Definition at line 38 of file kgazer_test.c.
Referenced by main().
#define MIN_SPEED 2000 |
Definition at line 67 of file kgazer_test.c.
Referenced by braitenberg(), braitenbergAvoidance(), and main().
#define MIN_X_FACTOR (2.0*20.0) |
Definition at line 34 of file kgazer_test.c.
Referenced by lrfmeasure(), and main().
#define MIN_Y_FACTOR (2.0*20.0) |
Definition at line 35 of file kgazer_test.c.
Referenced by lrfmeasure(), and main().
#define MOVE_ANGLE_DIR_THRESH 10.0 |
Definition at line 50 of file kgazer_test.c.
Referenced by goto_xya().
#define MOVE_FAST_SPEED 15000 |
Definition at line 55 of file kgazer_test.c.
Referenced by goto_xya().
#define MOVE_SLOW_SPEED 5000 |
Definition at line 54 of file kgazer_test.c.
Referenced by goto_xya().
#define NB_COLUMNS_MAX 80 |
Definition at line 25 of file kgazer_test.c.
Referenced by lrfmeasure(), and main().
#define NB_LINES_MAX 50 |
Definition at line 26 of file kgazer_test.c.
Referenced by lrfmeasure(), and main().
#define ROTATE_ANGLE_TRESH 30.0 |
Definition at line 58 of file kgazer_test.c.
Referenced by goto_xya().
#define ROTATE_HIGH_SPEED 5500 |
Definition at line 57 of file kgazer_test.c.
Referenced by goto_xya().
#define ROTATE_LOW_SPEED 2500 |
Definition at line 56 of file kgazer_test.c.
Referenced by goto_xya().
#define ROUND | ( | dbl | ) | dbl >= 0.0 ? (int)(dbl + 0.5) : ((dbl - (double)(int)dbl) <= -0.5 ? (int)dbl : (int)(dbl - 0.5)) |
Definition at line 61 of file kgazer_test.c.
Referenced by main().
#define SPEED_FACTOR 1.25 |
Definition at line 68 of file kgazer_test.c.
Referenced by main().
#define ZOOM_FACTOR 1.25 |
Definition at line 46 of file kgazer_test.c.
Referenced by lrfmeasure(), and main().
double diff_angles | ( | double | a, | |
double | b | |||
) |
difference between two angles in degree, modulo 180
a | first angle | |
b | second angle |
Definition at line 327 of file kgazer_test.c.
Referenced by goto_xya().
int goto_xya | ( | double | curr_x, | |
double | curr_y, | |||
double | curr_a, | |||
double | goal_x, | |||
double | goal_y, | |||
double | goal_a, | |||
double * | dist2goal, | |||
double * | angle2goal | |||
) |
make one increment of moving robot to x,y,angle; must be called perodically very basic algorithm: rotate to be in direction to the goal xy, move straight to goal position and rotate to goal angle
curr_x | current x position [cm] | |
curr_y | current a position [cm] | |
curr_a | current angle position [degree] | |
goal_x | goal x position [cm] | |
goal_y | goal a position [cm] | |
goal_a | goal angle position [degree] | |
dist2goal | return computed distance to goal | |
angle2goal | return computed angle to goal (or to direction to goal when moving) |
Definition at line 381 of file kgazer_test.c.
References diff_angles(), END_ANGLE_GOAL_THRESH, END_POS_GOAL_THRESH, FAST_SPEED_POS_THRESH, kmot_SetPoint(), kMotRegSpeed, MOVE_ANGLE_DIR_THRESH, MOVE_FAST_SPEED, MOVE_SLOW_SPEED, ROTATE_ANGLE_TRESH, ROTATE_HIGH_SPEED, ROTATE_LOW_SPEED, and sum_angles().
Referenced by main().
int initKH3 | ( | void | ) |
initKH3 initialize various things in the kh3 then sequentialy open the various required handle to the three i2c devices on the khepera3 using knet_open from the knet.c libkorebot's modules. Finaly, this function initializes then configures the motor control unit.
Definition at line 134 of file kgazer_test.c.
References initMot(), kh3_init(), KNET_BUS_I2C, and knet_open().
Referenced by main().
int initMot | ( | knet_dev_t * | hDev | ) |
initMot initializes then configures the motor control unit.
hDev | device handle |
Definition at line 92 of file kgazer_test.c.
References kmot_ConfigurePID(), kmot_ResetError(), kmot_SetBlockedTime(), kmot_SetLimits(), kmot_SetMargin(), kmot_SetMode(), kmot_SetOptions(), kmot_SetSampleTime(), kmot_SetSpeedProfile(), kMotModeIdle, kMotRegCurrent, kMotRegPos, kMotRegSpeed, kMotSWOptDirectionInv, kMotSWOptStopMotorBlk, and kMotSWOptWindup.
Referenced by initKH3(), and InitMot().
int main | ( | int | argc, | |
char * | argv[] | |||
) |
Definition at line 554 of file kgazer_test.c.
References a_axis, ANGLE_CORRECTION, angle_rot, b_axis, BIGGEST_SCALE, CALIB_STDEV_MAX, DATA_FILE_NAME, DEFAULT_SPEED, DEFAULT_X_FACTOR, goto_xya(), initKH3(), kb_change_term_mode(), kb_clrscr(), kb_erase_line(), kb_gazer_calibration(), kb_gazer_get_version(), kb_gazer_start_computation(), kb_gazer_start_map_mode(), kb_gazer_wait_stop_computation(), kb_kbhit(), kb_move_cursor(), kb_stargazer_Close(), kb_stargazer_Init(), kb_stargazer_read_data(), KEYS_TEXT_EXT_LENGTH, kh3_battery_voltage(), kh3_mms_to_speed, kh3_revision(), kmot_SetPoint(), kMotRegSpeed, MARK_ALONE, MAX_SPEED, MAX_X_FACTOR, MAX_Y_FACTOR, MEDIUM_SCALE, MIN_SPEED, MIN_X_FACTOR, MIN_Y_FACTOR, NB_COLUMNS_MAX, NB_LINES_MAX, ROUND, set_Stargazer_parameters(), SPEED_FACTOR, stop(), timeval_diff(), x_factor, y_factor, and ZOOM_FACTOR.
int set_Stargazer_parameters | ( | int * | nb_land, | |
int * | ref_land, | |||
int * | land_type, | |||
int * | land_mode | |||
) |
set the Stargazer parameters
nb_land | number of lanmarks | |
ref_land | reference landmark for map mode | |
land_type | type of landmark | |
land_mode | landmark mode (alone or map) |
Definition at line 171 of file kgazer_test.c.
References HEIGHT_FIX_NO, HLD1S, kb_gazer_get_height_fix_mode(), kb_gazer_get_landmark_mode(), kb_gazer_get_landmark_number(), kb_gazer_get_landmark_type(), kb_gazer_get_ref_id(), kb_gazer_height_fix_modes, kb_gazer_landmark_modes, kb_gazer_landmark_types, kb_gazer_set_end_command(), kb_gazer_set_height_fix_mode(), kb_gazer_set_landmark_mode(), kb_gazer_set_landmark_number(), kb_gazer_set_landmark_type(), kb_gazer_set_ref_id(), MARK_ALONE, MARK_MAP, and NB_MARK_TYPES.
Referenced by main().
double sum_angles | ( | double | a, | |
double | b | |||
) |
sum between two angles in degree, modulo 180
a | first angle | |
b | second angle |
Definition at line 349 of file kgazer_test.c.
Referenced by goto_xya().
long long timeval_diff | ( | struct timeval * | difference, | |
struct timeval * | end_time, | |||
struct timeval * | start_time | |||
) |
compute time difference
*difference | returned time difference in * struct timeva | |
*end_time | finish time | |
*start_time | start time |
Definition at line 525 of file kgazer_test.c.
Referenced by main().
knet_dev_t* dsPic [static] |
Definition at line 80 of file kgazer_test.c.
knet_dev_t* mot1 [static] |
Definition at line 78 of file kgazer_test.c.
knet_dev_t* mot2 [static] |
Definition at line 79 of file kgazer_test.c.
double x_factor = DEFAULT_X_FACTOR [static] |
Definition at line 42 of file kgazer_test.c.
Referenced by main().
double y_factor = DEFAULT_Y_FACTOR [static] |
Definition at line 43 of file kgazer_test.c.
Referenced by main().