00001
00002
00026 #ifndef _ERRATICDEVICE_H
00027 #define _ERRATICDEVICE_H
00028
00029 #ifndef ERRATIC_VERSION
00030 #define ERRATIC_VERSION "1.0b"
00031 #endif
00032
00033 #ifndef ERRATIC_DATE
00034 #define ERRATIC_DATE "2006-05-07"
00035 #endif
00036
00037 #include <pthread.h>
00038 #include <sys/time.h>
00039 #include <queue>
00040
00041 #include <libplayercore/playercore.h>
00042 #include <replace/replace.h>
00043
00044 #include "packet.h"
00045 #include "robot_params.h"
00046
00047
00048
00049 #define CPU_VOLTAGE 3.5
00050
00051
00052 #define MOTOR_DEF_MAX_SPEED 0.5
00053 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00054
00055
00056 #define ROBOT_CYCLETIME 20000
00057
00058
00059
00060 #define VIDERE_NOMINAL_VOLTAGE 12.0
00061
00062
00063
00064 typedef enum command {
00065 pulse = 0,
00066 open_controller = 1,
00067 close_controller = 2,
00068 enable_motors = 4,
00069 set_max_trans_acc = 5,
00070 set_max_position_velocity = 6,
00071 reset_origo = 7,
00072 trans_vel = 11,
00073 rot_pos = 12,
00074 rot_dpos = 13,
00075 configuration = 18,
00076 rot_vel = 21,
00077 set_max_rot_acc = 23,
00078 set_sonar = 28,
00079 stop = 29,
00080 wheel_vel = 32,
00081 set_analog = 71,
00082 save_config = 72,
00083 set_pwm_freq = 73,
00084 set_pwm_max_on = 74,
00085 servo_pos = 75,
00086 set_pid_trans_p = 80,
00087 set_pid_trans_v = 81,
00088 set_pid_trans_i = 82,
00089 set_pid_rot_p = 83,
00090 set_pid_rot_v = 84,
00091 set_pid_rot_i = 85,
00092
00093 } command_e;
00094
00095
00096 typedef enum argtype {
00097 argint = 0x3B,
00098 argnint = 0x1B,
00099 argstr = 0x2B
00100 } argtype_e;
00101
00102
00103 typedef enum reply {
00104 debug = 0x15,
00105 config = 0x20,
00106 stopped = 0x32,
00107 moving = 0x33,
00108 motor = 0x80,
00109 encoder = 0x90,
00110 ain = 0x9a,
00111 sonar = 0x9b
00112 } reply_e;
00113
00114
00115 #define DEFAULT_VIDERE_PORT "/dev/erratic"
00116
00117 typedef struct player_erratic_data
00118 {
00119 player_position2d_data_t position;
00120 player_power_data_t power;
00121 player_aio_data_t aio;
00122 player_ir_data ir;
00123 player_sonar_data sonar;
00124 } __attribute__ ((packed)) player_erratic_data_t;
00125
00126
00127 #include "motorpacket.h"
00128
00129 extern bool debug_mode;
00130
00131 class ErraticMotorPacket;
00132
00133 class Erratic : public Driver
00134 {
00135 private:
00136 int mcount;
00137 player_erratic_data_t erratic_data;
00138
00139 player_devaddr_t position_id;
00140 player_devaddr_t power_id;
00141 player_devaddr_t aio_id;
00142 player_devaddr_t ir_id;
00143 player_devaddr_t sonar_id;
00144 player_devaddr_t ptz_id, ptz2_id;
00145
00146 int position_subscriptions;
00147 int aio_ir_subscriptions;
00148 int sonar_subscriptions;
00149 int ptz_subscriptions;
00150 int ptz2_subscriptions;
00151
00152
00153 ErraticMotorPacket *motor_packet;
00154 pthread_mutex_t motor_packet_mutex;
00155
00156 int Connect();
00157 int Disconnect();
00158
00159 void ResetRawPositions();
00160 void ToggleMotorPower(unsigned char val);
00161
00162 void ToggleAIn(unsigned char val);
00163 void ToggleSonar(unsigned char val);
00164
00165 int HandleConfig(MessageQueue* resp_queue, player_msghdr * hdr, void* data);
00166 int HandleCommand(player_msghdr * hdr, void * data);
00167 void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
00168 void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
00169 void HandlePtzCommand(player_ptz_cmd_t ptz_cmd, player_devaddr_t id);
00170
00171 void PublishAllData();
00172 void PublishPosition2D();
00173 void PublishPower();
00174 void PublishAIn();
00175 void PublishIR();
00176 void PublishSonar();
00177
00178 float IRRangeFromVoltage(float voltage);
00179 float IRFloorRange(float value);
00180
00181 void StartThreads();
00182 void StopThreads();
00183
00184 void Send(ErraticPacket *packet);
00185 void SendThread();
00186 static void *SendThreadDummy(void *driver);
00187 void ReceiveThread();
00188 static void *ReceiveThreadDummy(void *driver);
00189
00190 int read_fd, write_fd;
00191 const char* psos_serial_port;
00192
00193 player_position2d_cmd_vel_t last_position_cmd;
00194 player_position2d_cmd_car_t last_car_cmd;
00195
00196 std::queue<ErraticPacket *> send_queue;
00197 pthread_mutex_t send_queue_mutex;
00198 pthread_cond_t send_queue_cond;
00199
00200 pthread_t send_thread;
00201 pthread_t receive_thread;
00202
00203
00204
00205 bool direct_wheel_vel_control;
00206
00207 bool print_all_packets;
00208 bool print_status_summary;
00209
00210 bool save_settings_in_robot;
00211
00212 int param_idx;
00213
00214
00215 int motor_max_speed;
00216 int motor_max_turnspeed;
00217
00218
00219 int16_t pid_trans_p, pid_trans_v, pid_trans_i;
00220 int16_t pid_rot_p, pid_rot_v, pid_rot_i;
00221
00222
00223 uint16_t motor_pwm_frequency, motor_pwm_max_on;
00224
00225
00226 bool use_vel_band;
00227
00228
00229 short motor_max_trans_accel, motor_max_trans_decel;
00230 short motor_max_rot_accel, motor_max_rot_decel;
00231
00232 public:
00233
00234 Erratic(ConfigFile* cf, int section);
00235
00236 virtual int Subscribe(player_devaddr_t id);
00237 virtual int Unsubscribe(player_devaddr_t id);
00238
00239
00240 virtual void Main();
00241
00242 virtual int Setup();
00243 virtual int Shutdown();
00244
00245
00246 virtual int ProcessMessage(MessageQueue * resp_queue, player_msghdr * hdr, void * data);
00247 };
00248
00249
00250 #endif