khepera.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
00033
00034 #ifndef _KHEPERADEVICE_H
00035 #define _KHEPERADEVICE_H
00036
00037 #include <pthread.h>
00038 #include <sys/time.h>
00039 #include <errno.h>
00040
00041 #include <libplayercore/playercore.h>
00042
00043 #include <khepera_serial.h>
00044
00045 #define KHEPERA_CONFIG_BUFFER_SIZE 1024
00046 #define KHEPERA_BAUDRATE B38400
00047 #define KHEPERA_DEFAULT_SERIAL_PORT "/dev/ttyUSB0"
00048 #define KHEPERA_DEFAULT_SCALE 10
00049 #define KHEPERA_DEFAULT_ENCODER_RES (1.0/12.0)
00050 #define KHEPERA_DEFAULT_IR_CALIB_A (64.158)
00051 #define KHEPERA_DEFAULT_IR_CALIB_B (-0.1238)
00052
00053 #define KHEPERA_MOTOR_LEFT 0
00054 #define KHEPERA_MOTOR_RIGHT 1
00055
00056 #define KHEPERA_FIXED_FACTOR 10000
00057
00058 #define CRLF "\r\n"
00059 #define KHEPERA_COMMAND_PROMPT "\r\n"
00060
00061 #ifndef ABS
00062 #define ABS(x) ((x) < 0 ? -(x) : (x))
00063 #endif
00064
00065 #ifndef SGN
00066 #define SGN(x) ((x) < 0 ? -1 : 1)
00067 #endif
00068
00069 typedef struct player_khepera_geom
00070 {
00071 char * PortName;
00072 double scale;
00073 player_ir_pose_t ir;
00074 double * ir_calib_a;
00075 double * ir_calib_b;
00076 player_position2d_geom_t position;
00077 double encoder_res;
00078 } __attribute__ ((packed)) player_khepera_geom_t;
00079
00080
00081 class Khepera : public Driver
00082 {
00083 public:
00084
00085 Khepera(ConfigFile *cf, int section);
00086
00087
00088 virtual void Main();
00089
00090 virtual int Subscribe(player_devaddr_t addr);
00091 virtual int Unsubscribe(player_devaddr_t addr);
00092
00093 virtual int Setup();
00094 virtual int Shutdown();
00095
00096 int ResetOdometry();
00097
00098
00099 void SetIRState(int);
00100
00101 void UpdateData(void);
00102
00103 void UpdateIRData(player_ir_data_t *);
00104 void UpdatePosData(player_position2d_data_t *);
00105
00106
00107
00108
00109 unsigned short ReadAD(int);
00110 int ReadAllIR(player_ir_data_t *);
00111
00112
00113 int SetSpeed(int, int);
00114 int ReadSpeed(int*, int*);
00115
00116 int SetPos(int, int);
00117
00118 int SetPosCounter(int, int);
00119 int ReadPos(int *, int*);
00120
00121
00122
00123
00124 int ProcessMessage(MessageQueue* resp_queue, player_msghdr * hdr, void * data);
00125
00126 private:
00127 player_devaddr_t ir_addr;
00128 player_devaddr_t position_addr;
00129 int position_subscriptions;
00130 int ir_subscriptions;
00131
00132 KheperaSerial * Serial;
00133
00134 player_khepera_geom_t* geometry;
00135
00136 int param_index;
00137 int khepera_fd;
00138
00139 struct timeval last_position;
00140 bool refresh_last_position;
00141 int last_lpos, last_rpos;
00142 double x,y,yaw;
00143 int last_x_f, last_y_f;
00144 double last_theta;
00145
00146 struct timeval last_pos_update;
00147 struct timeval last_ir_update;
00148
00149 int pos_update_period;
00150
00151 short desired_heading;
00152
00153 int ir_sequence;
00154 struct timeval last_ir;
00155
00156 bool motors_enabled;
00157 bool velocity_mode;
00158 bool direct_velocity_control;
00159
00160
00161 char khepera_serial_port[MAX_FILENAME_SIZE];
00162
00163
00164
00165
00166 };
00167
00168
00169 #endif