mbasedriver.h
00001 // -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-
00002 
00037 #ifndef _mbasedriverDEVICE_H
00038 #define _mbasedriverDEVICE_H
00039 
00040 #ifndef mbasedriver_VERSION
00041 #define mbasedriver_VERSION "2.0"
00042 #endif
00043 
00044 #ifndef mbasedriver_DATE
00045 #define mbasedriver_DATE "2010-08-31"
00046 #endif
00047 
00048 #ifndef mbase_author 
00049 #define mbase_author "Hernandez Malagon, Ana Teresa"
00050 #endif
00051 
00052 
00053 #include <pthread.h>
00054 #include <sys/time.h>
00055 #include <queue>
00056 
00057 #include <libplayercore/playercore.h>
00058 #include <replace/replace.h>
00059 
00060 #include "packet.h"
00061 #include "robot_params.h"
00062 
00063 //#include <stdint.h>
00064 
00065 #define GradToRad(x) (M_PI * ((double)(x)) / 180.0)
00066 #define RadToGrad(x) ((short)((x) * 180.0) / M_PI)
00067 
00068 // angular constants, angular units are 4096 / rev
00069 #define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
00070 #define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
00071 #define RTOA(x) ((short)((x) * 2048.0) / M_PI)
00072 
00073 // Default max speeds
00074 #define MOTOR_DEF_MAX_SPEED 0.5
00075 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00076 
00077 // This merely sets a delay policy in the initial connection
00078 #define ROBOT_CYCLETIME 20000
00079 
00080 /* mbasedriver constants */
00081 #define VIDERE_NOMINAL_VOLTAGE 13.0
00082 
00083 //mm/s/s
00084 #define MOTOR_MAX_TRANS_ACEL    1500
00085 #define MOTOR_DEF_TRANS_ACEL    500     
00086 //gados/s/s
00087 #define MOTOR_DEF_MAX_ROT_ACEL  135     
00088 #define PID_P   10
00089 #define PID_I   10
00090 #define PID_V   6000
00091 #define DRIFFACTOR      0 
00092 #define ROBOT_WHEEL_MR5         190
00093 #define ROBOT_WHEEL_MR7         310
00094 #define ROBOT_WIDTH_MR5         410
00095 #define ROBOT_WIDTH_MR7         495
00096 
00097 //constantes para transformas voltage infrarrojo
00098 #define INFRAmult       5
00099 #define INFRAdiv        4096
00100 
00101 //constantes para calcular distancia sonar
00102 //para incrementar la frecuencia a la que trabaja el micro 
00103 #define PLL             8 
00104 #define FREC_OSC_MICRO  3.6864  
00105 #define FACTOR_ESCALA   256
00106 
00107 #define tipo_MR7        7
00108 #define tipo_MR5        5
00109 #define DEBUG   0
00110 #define IR_AN   0
00111 
00112 // Commands for the robot
00113 typedef enum command 
00114 {
00115         open_controller         =       0x01,//1,
00116         close_controller        =       0x02,//2,
00117         enable_motors   =       0x04,//4,
00118         reset_origo     =       0x07,//7,
00119         trans_vel       =       0x0B,//11,      // mm/s
00120         rot_vel         =       0x15,//21,      // deg/s
00121         set_sonar       =       0x1C,//28,
00122         stop            =       0x1D,//29,
00123         set_analog      =       0x47,//71,
00124         set_pid_p       =       0x50,// 80,
00125         set_pid_i       =       0x51,// 81,
00126         set_pid_v       =       0x52,// 82,
00127         set_trans_acel  =       0x5A,// 90,
00128         set_rot_acel    =       0x5B,// 91,
00129         set_driffactor  =       0x5C,// 92,
00130         set_robot_width =       0x5D,// 93,
00131         set_robot_wheel =       0x5E,// 94
00132         set_odometria   =       0x5F,//95
00133         set_sensores    =       0x60
00134 } command_e;
00135 
00136 // Argument types used in robot commands
00137 typedef enum argtype {
00138         argint =  0x3B,
00139         argnint = 0x1B,
00140         argstr =  0x2B
00141 } argtype_e;
00142 
00143 // Types of replies from the robot
00144 typedef enum reply {
00145         debug   =       0x15,
00146         config  =       0x20,
00147         stopped =       0x32,
00148         moving  =       0x33,
00149         motor   =       0x80,
00150         encoder =       0x90,
00151         ain     =       0x9a,
00152         sonar   =       0x9b,
00153         sensores        =       0x90
00154 } reply_e;
00155 
00156 
00157 #define DEFAULT_VIDERE_PORT "/dev/ttyS0"
00158 
00159 typedef struct player_mbasedriver_data
00160 {
00161   player_position2d_data_t position;
00162   player_power_data_t power;
00163   player_aio_data_t aio;
00164   player_ir_data ir;
00165   player_sonar_data sonar;
00166 } __attribute__ ((packed)) player_mbasedriver_data_t;
00167 
00168 // this is here because we need the above typedef's before including it.
00169 #include "motorpacket.h"
00170 
00171 extern bool debug_mbasedriver;
00172 extern bool debug_send;
00173 extern bool debug_receive_aio;
00174 extern bool debug_receive_sonar;
00175 extern bool debug_receive_motor;
00176 extern bool debug_susbcribe;
00177 extern bool debug_mbase_send_msj;
00178 
00179 class mbasedriverMotorPacket;
00180 
00181 class mbasedriver : public Driver
00182 {
00183         private:
00184                 int mcount;
00185                 player_mbasedriver_data_t mbasedriver_data;
00186 
00187                 player_devaddr_t position_id;
00188                 player_devaddr_t power_id;
00189                 player_devaddr_t aio_id;
00190                 player_devaddr_t ir_id;
00191                 player_devaddr_t sonar_id;
00192 
00193                 int position_subscriptions;
00194                 int aio_ir_subscriptions;
00195                 int sonar_subscriptions;
00196 
00197                 //mbasedriverMotorPacket* sippacket;
00198                 mbasedriverMotorPacket *motor_packet;
00199                 pthread_mutex_t motor_packet_mutex;
00200 
00201                 int Connect();
00202                 int Disconnect();
00203                 
00204                 void ResetRawPositions();
00205                 void ToggleMotorPower(unsigned char val);
00206 
00207                 void ToggleAIn(unsigned char val);
00208                 void ToggleSonar(unsigned char val);
00209 
00210                 int HandleConfig(QueuePointer &resp_queue, player_msghdr * hdr, void* data);
00211                 int HandleCommand(player_msghdr * hdr, void * data);
00212                 void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
00213 
00214                 void PublishAllData();
00215                 void PublishPosition2D();
00216                 void PublishPower();
00217                 void PublishAIn();
00218                 void PublishIR();
00219                 void PublishSonar();
00220 
00221                 void StartThreads();
00222                 void StopThreads();
00223 
00224                 void Send(mbasedriverPacket *packet);
00225                 void SendThread();
00226                 static void *SendThreadDummy(void *driver);
00227                 void ReceiveThread();
00228                 static void *ReceiveThreadDummy(void *driver);
00229 
00230                 int read_fd, write_fd;
00231                 const char* psos_serial_port;
00232 
00233                 player_position2d_cmd_vel_t last_position_cmd;
00234                 player_position2d_cmd_car_t last_car_cmd;
00235 
00236                 std::queue<mbasedriverPacket *> send_queue;
00237                 pthread_mutex_t send_queue_mutex;
00238                 pthread_cond_t send_queue_cond;
00239 
00240                 pthread_t send_thread;
00241                 pthread_t receive_thread;
00242 
00243                 // Parameters
00244                 bool print_all_packets;
00245                 int param_idx;  // index in the RobotParams table for this robot
00246 
00247                 // Max motor speeds (mm/sec,deg/sec)
00248                 int motor_max_speed;
00249                 int motor_max_turnspeed;
00250 
00251                 int getms();
00252 
00253                 //mbase
00254                 int motor_trans_acel;
00255                 int motor_rot_acel;
00256                 int trans_ant;
00257                 int rot_ant;
00258                 int16_t pid_p, pid_v, pid_i;
00259                 int16_t driffactor;
00260                 int16_t dist_ejes;
00261                 int16_t diametro;
00262                 bool debug_usuario;
00263                 bool ir_analog;
00264 
00265         public:
00266                 mbasedriver(ConfigFile* cf, int section);
00267 
00268                 virtual int Subscribe(player_devaddr_t id);
00269                 virtual int Unsubscribe(player_devaddr_t id);
00270 
00271                 /* the main thread */
00272                 virtual void Main();
00273 
00274                 virtual int Setup();
00275                 virtual int Shutdown();
00276 
00277                 // MessageHandler
00278                 virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);
00279 };
00280 
00281 
00282 #endif