rflex.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef _RFLEXDEVICE_H
00033 #define _RFLEXDEVICE_H
00034
00035 #include <pthread.h>
00036 #include <sys/time.h>
00037
00038 #include <libplayercore/playercore.h>
00039
00040 #include "rflex_commands.h"
00041 #include "rflex-io.h"
00042
00043 #include "rflex_configs.h"
00044
00045 #define RFLEX_MOTORS_REQUEST_ON 0
00046 #define RFLEX_MOTORS_ON 1
00047 #define RFLEX_MOTORS_REQUEST_OFF 2
00048 #define RFLEX_MOTORS_OFF 3
00049
00050 #define RFLEX_CONFIG_BUFFER_SIZE 256
00051
00052 #define DEFAULT_RFLEX_PORT "/dev/ttyS0"
00053
00054 #define DEFAULT_RFLEX_BUMPER_ADDRESS 0x40
00055 #define RFLEX_BUMPER_STYLE_BIT "bit"
00056 #define RFLEX_BUMPER_STYLE_ADDR "addr"
00057 #define DEFAULT_RFLEX_BUMPER_STYLE RFLEX_BUMPER_STYLE_ADDR
00058
00059 enum
00060 {
00061 BUMPER_BIT,
00062 BUMPER_ADDR
00063 };
00064
00065 #define DEFAULT_RFLEX_POWER_OFFSET 0
00066
00067 #define MAX_NUM_LOOPS 30
00068 #define B_STX 0x02
00069 #define B_ETX 0x03
00070 #define B_ESC 0x1b
00071
00072 typedef struct player_rflex_data
00073 {
00074 player_position2d_data_t position;
00075 player_sonar_data_t sonar;
00076 player_sonar_data_t sonar2;
00077 player_gripper_data_t gripper;
00078 player_power_data_t power;
00079 player_bumper_data_t bumper;
00080 player_dio_data_t dio;
00081 player_aio_data_t aio;
00082 player_ir_data ir;
00083 } __attribute__ ((packed)) player_rflex_data_t;
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096 class RFLEX : public Driver
00097 {
00098 private:
00099 player_devaddr_t position_id;
00100 player_devaddr_t sonar_id;
00101 player_devaddr_t sonar_id_2;
00102 player_devaddr_t ir_id;
00103 player_devaddr_t bumper_id;
00104 player_devaddr_t power_id;
00105 player_devaddr_t aio_id;
00106 player_devaddr_t dio_id;
00107 player_position2d_cmd_vel_t command;
00108 int command_type;
00109
00110 int position_subscriptions;
00111 int sonar_subscriptions;
00112 int ir_subscriptions;
00113 int bumper_subscriptions;
00114
00115 int rflex_fd;
00116
00117
00118 char rflex_serial_port[MAX_FILENAME_SIZE];
00119 double m_odo_x;
00120 double m_odo_y;
00121 double rad_odo_theta;
00122
00123 void ResetRawPositions();
00124 int initialize_robot();
00125 void reset_odometry();
00126 void set_odometry(float,float,float);
00127 void update_everything(player_rflex_data_t* d);
00128
00129 void set_config_defaults();
00130
00131 public:
00132 RFLEX(ConfigFile* cf, int section);
00133
00134
00135 virtual void Main();
00136
00137
00138 virtual int Subscribe(player_devaddr_t addr);
00139 virtual int Unsubscribe(player_devaddr_t addr);
00140
00141 virtual int Setup();
00142 virtual int Shutdown();
00143
00148 virtual void StartThread(void);
00149
00154 virtual void StopThread(void);
00155
00156 static int joy_control;
00157
00158
00159 int ProcessMessage(MessageQueue* resp_queue, player_msghdr * hdr,
00160 void * data);
00161
00162 bool ThreadAlive;
00163 };
00164
00165
00166 #endif
Last updated 12 September 2005 21:38:45
|