rflex_configs.h
00001 /* some basic structures and conversions needed by everything 00002 * 00003 * data for the conversions and everything else will be grabbed out of 00004 * the general configuration file 00005 * 00006 * note - they way it is now is the right way to do it, BUT: 00007 * if your a hacker and want things to run fast - put all the configurations 00008 * here as #define's, this will allow the precompiler to precompute combined 00009 * conversions, this is much preferable to modifying the rest of the code 00010 * if you really need the speed that badly 00011 */ 00012 00013 // NOTICE! - this file declares rflex_configs extern, intended to link to 00014 // the rflex_configs declared in rflex.cc 00015 00016 #ifndef RFLEX_CONFIGS_H 00017 #define RFLEX_CONFIGS_H 00018 00019 #include <math.h> 00020 #include <libplayercore/player.h> 00021 00022 //normalizes an angle in radians to -M_PI<theta<M_PI 00023 inline double normalize_theta(double theta){ 00024 while(theta>M_PI) 00025 theta-=2*M_PI; 00026 while(theta<-M_PI) 00027 theta+=2*M_PI; 00028 return theta; 00029 } 00030 00031 //structures for holding general configuration of robot 00032 typedef struct rflex_config_t{ 00033 char serial_port[256]; 00034 //length of the robot in m 00035 double m_length; 00036 //width of the robot in m 00037 double m_width; 00038 //m*odo_distance_conversion : m to rflex arbitrary odometry units (trans) 00039 double odo_distance_conversion; 00040 //rad*odo_angle_conversion : rad to rflex arbitrary odometry units (rot) 00041 double odo_angle_conversion; 00042 //mm*range_distance_conversion : m to rflex arbitrary range units 00043 double range_distance_conversion; 00044 //default translational acceleration in m/sec 00045 double mPsec2_trans_acceleration; 00046 //default rotational acceleration in rad/sec 00047 double radPsec2_rot_acceleration; 00048 00049 // absolute heading dio address (if ommited then absolute heading not used) 00050 int heading_home_address; 00051 // home on startup 00052 bool home_on_start; 00053 00054 // use rflex joystick to command robot? 00055 bool use_joystick; 00056 double joy_pos_ratio, joy_ang_ratio; 00057 00058 //maximum number of sonar supported by modules 00059 //(generally 16*number of sonar controller boards, or banks) 00060 int max_num_sonars; 00061 //total number of physical sonar 00062 int num_sonars; 00063 //how long to buffer for filter (filter just takes smallest of last n readings) 00064 int sonar_age; 00065 //number of physical sonar sonar controller boards or banks on your robot 00066 int num_sonar_banks; 00067 // number of sonars that can be attached to each sonar controller (16) 00068 int num_sonars_possible_per_bank; 00069 // number of actual sonar attached to each sonar controller, (in each bank) 00070 int *num_sonars_in_bank; 00071 // pose of each sonar on the robot (x,y,t) in rad and mm 00072 // note i is forwards, j is left 00073 player_pose3d_t *mrad_sonar_poses; 00074 //not sure what these do yet actually 00075 long sonar_echo_delay; 00076 long sonar_ping_delay; 00077 long sonar_set_delay; 00078 // options to support 2nd sonar bank 00079 long sonar_2nd_bank_start; 00080 long sonar_1st_bank_end; 00081 long sonar_max_range; // in mm 00082 00083 00084 // bumper configs 00085 unsigned short bumper_count; 00086 int bumper_address; 00088 int bumper_style; 00089 player_bumper_define_t * bumper_def; 00090 00091 // power configs 00092 float power_offset; 00093 00094 // ir configs 00095 player_ir_pose_t ir_poses; 00096 int ir_base_bank; 00097 int ir_bank_count; 00098 int ir_total_count; 00099 int * ir_count; 00100 double * ir_a; 00101 double * ir_b; 00102 float ir_min_range; 00103 float ir_max_range; 00104 } rflex_config_t; 00105 00106 //notice - every file that includes this header gets a GLOBAL rflex_configs!! 00107 // be careful 00108 extern rflex_config_t rflex_configs; 00109 00110 /************ conversions ********************/ 00111 /*** rather obvious - ARB stands for arbitrary (look above) ***/ 00112 00113 //player uses degrees, but I use radians (so cos, and sin work correctly) 00114 #define ARB2RAD_ODO_CONV(x) ((x)/rflex_configs.odo_angle_conversion) 00115 #define RAD2ARB_ODO_CONV(x) ((x)*rflex_configs.odo_angle_conversion) 00116 #define ARB2M_ODO_CONV(x) ((x)/rflex_configs.odo_distance_conversion) 00117 #define M2ARB_ODO_CONV(x) ((x)*rflex_configs.odo_distance_conversion) 00118 00119 #define ARB2M_RANGE_CONV(x) (x/rflex_configs.range_distance_conversion) 00120 #define M2ARB_RANGE_CONV(x) (x*rflex_configs.range_distance_conversion) 00121 00122 #endif 00123 00124 00125 00126 00127 00128 00129 00130 00131