b21rconst.hh

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

#ifndef _B21R_CONST_H
#define _B21R_CONST_H


/* sensor coordinates in the robot frame */
typedef struct RFLEX_CONFIG {
	double x;
	double y;
	double theta;
} RFLEX_CONFIG;

typedef struct RFLEX_SONARS_CONFIG {
	RFLEX_CONFIG tab[48];
} RFLEX_SONARS_CONFIG;


/*     US configuration of B21     */

/*             Front Up             */
/*          10 11 | 12 13             */
/*        09             14           */
/*      08                 15         */
/*      -                   -         */ 
/*      07                 16         */
/*      06	           17         */
/*      05                 18         */
/*      04                 19         */
/*      -                   -         */ 
/*      03                 20         */
/*        02             21           */
/*          01 00 | 23 22             */


/*                   Down           */
/*          46 47 | 24 25             */
/*        45             26           */
/*        -               -         */ 
/*       44               27         */
/*       43               28         */
/*       42	          29         */
/*       -                 -         */ 
/*       41               30         */
/*       40               31         */
/*       39               32         */
/*        -               -         */ 
/*        38            33           */
/*          37 36 | 35 34             */




/* robot upper radius*/
#define RA_UP_TH    0.2525
#define RA_UP    (0.66/2)
/* robot down radius*/
#define RA_DOWN_TH 0.2625
#define RA_DOWN (0.66/2)
/* angle between two upper US */
#define THETA_US_UP (0.065/RA_UP_TH)
#define THETA_UP_2 (THETA_US_UP/2)
/* angle between two down US */
/* #define THETA_US_DOWN (7/RA_DOWN_TH) */
/* #define THETA_DOWN_2 (THETA_US_DOWN/2) */
#define THETA_US_DOWN THETA_US_UP 
#define THETA_DOWN_2 (THETA_US_DOWN/2)

/* position X and Y of US */
#define X_SONAR_UP(ang) RA_UP*cos(ang)
#define Y_SONAR_UP(ang) RA_UP*sin(ang)
#define X_SONAR_DOWN(ang) RA_DOWN*cos(ang)
#define Y_SONAR_DOWN(ang) RA_DOWN*sin(ang)


#define RFLEX_SONARS_UP_CONFIG_CONST \
{X_SONAR_UP(+23*THETA_UP_2), Y_SONAR_UP(+23*THETA_UP_2),  +23*THETA_UP_2},/* Sonar 00 */ \
{X_SONAR_UP(+21*THETA_UP_2), Y_SONAR_UP(+21*THETA_UP_2),  +21*THETA_UP_2},/* Sonar 01 */ \
{X_SONAR_UP(+19*THETA_UP_2), Y_SONAR_UP(+19*THETA_UP_2),  +19*THETA_UP_2},/* Sonar 02 */ \
{X_SONAR_UP(+17*THETA_UP_2), Y_SONAR_UP(+17*THETA_UP_2),  +17*THETA_UP_2},/* Sonar 03 */\
{X_SONAR_UP(+15*THETA_UP_2), Y_SONAR_UP(+15*THETA_UP_2),  +15*THETA_UP_2},/* Sonar 04 */ \
{X_SONAR_UP(+13*THETA_UP_2), Y_SONAR_UP(+13*THETA_UP_2),  +13*THETA_UP_2},/* Sonar 05 */ \
{X_SONAR_UP(+11*THETA_UP_2), Y_SONAR_UP(+11*THETA_UP_2),  +11*THETA_UP_2},/* Sonar 06 */ \
{X_SONAR_UP(+9*THETA_UP_2), Y_SONAR_UP(+9*THETA_UP_2),  +9*THETA_UP_2},/* Sonar 07 */ \
{X_SONAR_UP(+7*THETA_UP_2), Y_SONAR_UP(+7*THETA_UP_2),  +7*THETA_UP_2},/* Sonar 08 */ \
{X_SONAR_UP(+5*THETA_UP_2), Y_SONAR_UP(+5*THETA_UP_2),  +5*THETA_UP_2},/* Sonar 09 */ \
{X_SONAR_UP(+3*THETA_UP_2), Y_SONAR_UP(+3*THETA_UP_2),  +3*THETA_UP_2},/* Sonar 10 */ \
{X_SONAR_UP(+THETA_UP_2)  , Y_SONAR_UP(+THETA_UP_2),    +THETA_UP_2},  /* Sonar 11 */ \
{X_SONAR_UP(-THETA_UP_2)  , Y_SONAR_UP(-THETA_UP_2),    -THETA_UP_2},  /* Sonar 12*/ \
{X_SONAR_UP(-3*THETA_UP_2), Y_SONAR_UP(-3*THETA_UP_2),  -3*THETA_UP_2},/* Sonar 13 */ \
{X_SONAR_UP(-5*THETA_UP_2), Y_SONAR_UP(-5*THETA_UP_2),  -5*THETA_UP_2},/* Sonar 14 */ \
{X_SONAR_UP(-7*THETA_UP_2), Y_SONAR_UP(-7*THETA_UP_2),  -7*THETA_UP_2},/* Sonar 15 */ \
{X_SONAR_UP(-9*THETA_UP_2), Y_SONAR_UP(-9*THETA_UP_2),  -9*THETA_UP_2},/* Sonar 16 */ \
{X_SONAR_UP(-11*THETA_UP_2), Y_SONAR_UP(-11*THETA_UP_2),  -11*THETA_UP_2},/* Sonar 17 */ \
{X_SONAR_UP(-13*THETA_UP_2), Y_SONAR_UP(-13*THETA_UP_2),  -13*THETA_UP_2},/* Sonar 18 */ \
{X_SONAR_UP(-15*THETA_UP_2), Y_SONAR_UP(-15*THETA_UP_2),  -15*THETA_UP_2},/* Sonar 19 */ \
{X_SONAR_UP(-17*THETA_UP_2), Y_SONAR_UP(-17*THETA_UP_2),  -17*THETA_UP_2},/* Sonar 20 */ \
{X_SONAR_UP(-19*THETA_UP_2), Y_SONAR_UP(-19*THETA_UP_2),  -19*THETA_UP_2},/* Sonar 21 */ \
{X_SONAR_UP(-21*THETA_UP_2), Y_SONAR_UP(-21*THETA_UP_2),  -21*THETA_UP_2},/* Sonar 22 */ \
{X_SONAR_UP(-23*THETA_UP_2), Y_SONAR_UP(-23*THETA_UP_2),  -23*THETA_UP_2}/* Sonar 23 */ 

#define RFLEX_SONARS_DOWN_CONFIG_CONST \
{X_SONAR_DOWN(-THETA_DOWN_2)  , Y_SONAR_DOWN(-THETA_DOWN_2),    -THETA_DOWN_2},  /* Sonar 24*/ \
{X_SONAR_DOWN(-3*THETA_DOWN_2), Y_SONAR_DOWN(-3*THETA_DOWN_2),  -3*THETA_DOWN_2},/* Sonar 25*/ \
{X_SONAR_DOWN(-5*THETA_DOWN_2), Y_SONAR_DOWN(-5*THETA_DOWN_2),  -5*THETA_DOWN_2},/* Sonar 26*/ \
{X_SONAR_DOWN(-7*THETA_DOWN_2), Y_SONAR_DOWN(-7*THETA_DOWN_2),  -7*THETA_DOWN_2},/* Sonar 27*/ \
{X_SONAR_DOWN(-9*THETA_DOWN_2), Y_SONAR_DOWN(-9*THETA_DOWN_2),  -9*THETA_DOWN_2},/* Sonar 28*/ \
{X_SONAR_DOWN(-11*THETA_DOWN_2), Y_SONAR_DOWN(-11*THETA_DOWN_2),  -11*THETA_DOWN_2},/* Sonar 29*/ \
{X_SONAR_DOWN(-13*THETA_DOWN_2), Y_SONAR_DOWN(-13*THETA_DOWN_2),  -13*THETA_DOWN_2},/* Sonar 30*/ \
{X_SONAR_DOWN(-15*THETA_DOWN_2), Y_SONAR_DOWN(-15*THETA_DOWN_2),  -15*THETA_DOWN_2},/* Sonar 31*/ \
{X_SONAR_DOWN(-17*THETA_DOWN_2), Y_SONAR_DOWN(-17*THETA_DOWN_2),  -17*THETA_DOWN_2},/* Sonar 32*/ \
{X_SONAR_DOWN(-19*THETA_DOWN_2), Y_SONAR_DOWN(-19*THETA_DOWN_2),  -19*THETA_DOWN_2},/* Sonar 33*/ \
{X_SONAR_DOWN(-21*THETA_DOWN_2), Y_SONAR_DOWN(-21*THETA_DOWN_2),  -21*THETA_DOWN_2},/* Sonar 34 */ \
{X_SONAR_DOWN(-23*THETA_DOWN_2), Y_SONAR_DOWN(-23*THETA_DOWN_2),  -23*THETA_DOWN_2},/* Sonar 35 */ \
{X_SONAR_DOWN(+23*THETA_DOWN_2), Y_SONAR_DOWN(+23*THETA_DOWN_2),  +23*THETA_DOWN_2},/* Sonar 36 */ \
{X_SONAR_DOWN(+21*THETA_DOWN_2), Y_SONAR_DOWN(+21*THETA_DOWN_2),  +21*THETA_DOWN_2},/* Sonar 37 */ \
{X_SONAR_DOWN(+19*THETA_DOWN_2), Y_SONAR_DOWN(+19*THETA_DOWN_2),  +19*THETA_DOWN_2},/* Sonar 38 */ \
{X_SONAR_DOWN(+17*THETA_DOWN_2), Y_SONAR_DOWN(+17*THETA_DOWN_2),  +17*THETA_DOWN_2},/* Sonar 39 */ \
{X_SONAR_DOWN(+15*THETA_DOWN_2), Y_SONAR_DOWN(+15*THETA_DOWN_2),  +15*THETA_DOWN_2},/* Sonar 40 */ \
{X_SONAR_DOWN(+13*THETA_DOWN_2), Y_SONAR_DOWN(+13*THETA_DOWN_2),  +13*THETA_DOWN_2},/* Sonar 41 */ \
{X_SONAR_DOWN(+11*THETA_DOWN_2), Y_SONAR_DOWN(+11*THETA_DOWN_2),  +11*THETA_DOWN_2},/* Sonar 42 */ \
{X_SONAR_DOWN(+9*THETA_DOWN_2), Y_SONAR_DOWN(+9*THETA_DOWN_2),  +9*THETA_DOWN_2},/* Sonar 43 */ \
{X_SONAR_DOWN(+7*THETA_DOWN_2), Y_SONAR_DOWN(+7*THETA_DOWN_2),  +7*THETA_DOWN_2},/* Sonar 44 */ \
{X_SONAR_DOWN(+5*THETA_DOWN_2), Y_SONAR_DOWN(+5*THETA_DOWN_2),  +5*THETA_DOWN_2},/* Sonar 45 */ \
{X_SONAR_DOWN(+3*THETA_DOWN_2), Y_SONAR_DOWN(+3*THETA_DOWN_2),  +3*THETA_DOWN_2},/* Sonar 46 */ \
{X_SONAR_DOWN(+THETA_DOWN_2)  , Y_SONAR_DOWN(+THETA_DOWN_2),    +THETA_DOWN_2}  /* Sonar 47 */

#define RFLEX_SONARS_CONFIG_CONST { \
 RFLEX_SONARS_UP_CONFIG_CONST, \
 RFLEX_SONARS_DOWN_CONFIG_CONST \
}


#endif