Main Page | Namespace List | Class Hierarchy | Alphabetical List | Data Structures | Directories | File List | Namespace Members | Data Fields | Globals

Pioneer2PacketManager.h

Go to the documentation of this file.
00001 /* Copyright (C) 2002 Dominic Letourneau (dominic.letourneau@courrier.usherb.ca)
00002 
00003    This library is free software; you can redistribute it and/or
00004    modify it under the terms of the GNU Lesser General Public
00005    License as published by the Free Software Foundation; either
00006    version 2.1 of the License, or (at your option) any later version.
00007    
00008    This library is distributed in the hope that it will be useful,
00009    but WITHOUT ANY WARRANTY; without even the implied warranty of
00010    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011    Lesser General Public License for more details.
00012    
00013    You should have received a copy of the GNU Lesser General Public
00014    License along with this library; if not, write to the Free Software
00015    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00016 */
00017 #ifndef _PIONEER2PACKETMANAGER_H_
00018 #define _PIONEER2PACKETMANAGER_H_
00019 
00020 #include "PSOSPacket.h"
00021 #include <pthread.h>
00022 #include <list>
00023 #include <string>
00024 #include "Object.h"
00025 #include "pseudosem.h"
00026 
00027 namespace RobotFlow {
00028 
00029 // data for all the robots
00030 #define P2OS_CYCLETIME_USEC 50000
00031 
00032 // data for the p2-dx robot from p2 operation manual 
00033 #define DX_AngleConvFactor 0.001534
00034 #define DX_DistConvFactor 0.826
00035 #define DX_VelConvFactor 1.0
00036 #define DX_RobotRadius 250.0
00037 #define DX_RobotDiagonal 120.0
00038 #define DX_Holonomic true
00039 #define DX_RangeConvFactor 0.268
00040 #define DX_RobotAxleLength 320.0
00041 
00042 // data for the p2-dx robot from p2 operation manual 
00043 #define AT_AngleConvFactor 0.001534
00044 #define AT_DistConvFactor 1.303
00045 #define AT_VelConvFactor 1.0
00046 #define AT_RobotRadius 500.0
00047 #define AT_RobotDiagonal 120.0
00048 #define AT_Holonomic true
00049 #define AT_RangeConvFactor 0.268
00050 #define AT_RobotAxleLength 320.0
00051 
00052 
00053 
00054 class Pioneer2Data : public FD::Object {
00055   
00056  public:
00057   
00058   unsigned char status;
00059   float xpos;
00060   float ypos;
00061   float thpos;
00062   float lvel;
00063   float rvel;
00064   float battery;
00065   unsigned short bumpers;
00066   float control;
00067   unsigned short ptu;
00068   float sonars[16];
00069   unsigned short timer;
00070   float analog;
00071   unsigned char digin;
00072   unsigned char digout;
00073 
00074   void printOn(std::ostream &out) const {
00075     print(out);
00076   }
00077 
00078   void print (std::ostream &out) const {
00079 
00080     out<<"status  : "<<(int)status<<std::endl;
00081     out<<"xpos    : "<<xpos<<std::endl;
00082     out<<"ypos    : "<<ypos<<std::endl;
00083     out<<"thpos   : "<<thpos<<std::endl;
00084     out<<"lvel    : "<<lvel<<std::endl;
00085     out<<"rvel    : "<<rvel<<std::endl;
00086     out<<"battery : "<<battery<<std::endl;
00087     out<<"bumpers : "<<bumpers<<std::endl;
00088     out<<"control : "<<control<<std::endl;
00089     out<<"ptu     : "<<ptu<<std::endl;
00090     for (int i = 0; i < 16; i++) {
00091       out<<"sonar "<<i<<" : "<<sonars[i]<<std::endl;
00092     }
00093     out<<"timer   : "<<timer<<std::endl;
00094     out<<"analog  : "<<analog<<std::endl;
00095     out<<"digin   : "<<(int)digin<<std::endl;
00096     out<<"digout  : "<<(int)digout<<std::endl;
00097   }
00098 
00099 };
00100 
00101 class Pioneer2PacketManager {
00102 
00103   friend void* Pioneer2PacketManager_recv_thread (void *user_data);
00104   friend void* Pioneer2PacketManager_send_thread (void *user_data);
00105 
00106  public:
00107 
00108   static Pioneer2PacketManager* instance();
00109   static void destroyInstance();
00110   Pioneer2PacketManager();
00111   virtual ~Pioneer2PacketManager();
00112   void initialize(const std::string &tty);
00113 
00114   //input accessors
00115   unsigned char get_status();
00116   float get_Xpos();
00117   float get_Ypos();
00118   float get_Thpos();
00119   float get_Lvel();
00120   float get_Rvel();
00121   float get_battery();
00122   unsigned short get_bumpers();
00123   float get_control();
00124   unsigned short get_ptu();
00125   float get_compass();
00126   float get_sonar(int s_number);
00127   unsigned short get_timer();
00128   float get_analog();
00129   unsigned char get_digin();
00130   unsigned char get_digout();
00131   void get_all(Pioneer2Data &data);
00132 
00133   //output commands
00134   void send_motor_command(short command);
00135   void send_reset_translational_acc(short command);
00136   void send_reset_translational_vel(short command);
00137   void send_reset_server();
00138   void send_reset_rotational_vel(short command);
00139   void send_velocity_command(short command);
00140   void send_heading_command(short command);
00141   void send_rel_heading_command(short command);
00142   void send_say_command(const std::string &command);
00143   void send_config_command(short command);
00144   void send_encoder_command(short command);
00145   void send_rotational_vel(short command);
00146   void send_colbert_relative_heading(short command);
00147   void send_rotational_acc(short command);
00148   void send_sonar_command(short command);
00149   void send_front_sonar_command();
00150   void send_rear_sonar_command();
00151   void send_stop_command();
00152   void send_digout_command(short command);
00153   void send_velocity2_command(short command);
00154   void send_gripper_command(short command);
00155   void send_ad_select_command(short command);
00156   void send_gripper_val_command(short command);
00157   void send_ptupos_command(short command);
00158   void send_tty2_command(const std::string& data);
00159   void send_tty2_command(const unsigned char* data, int size);
00160   void send_getaux_command(short command);
00161   void send_bumpstall_command(short command);
00162   void send_tcm2_command(short command);
00163   void send_emergency_stop();
00164   void send_pulse();
00165   void send_step();
00166 
00167 
00168 
00169  protected:
00170   
00171   unsigned char m_status;
00172   unsigned short m_xpos;
00173   unsigned short m_ypos;
00174   short m_thpos;
00175   short m_lvel;
00176   short m_rvel;
00177   unsigned char m_battery;
00178   short m_bumpers;
00179   short m_control;
00180   unsigned short m_ptu;
00181   unsigned char m_compass;
00182   unsigned short m_sonars[16];
00183   unsigned short m_timer;
00184   unsigned char m_analog;
00185   unsigned char m_digin;
00186   unsigned char m_digout;
00187 
00188   //robot dependant values
00189   float m_AngleConvFactor;
00190   float m_DistConvFactor;
00191   float m_VelConvFactor;
00192   float m_RobotRadius;
00193   float m_RobotDiagonal;
00194   bool m_Holonomic;
00195   float m_RangeConvFactor;
00196   float m_RobotAxleLength;
00197 
00198   
00199  private:
00200 
00201   int open_serial(const char *tty);
00202 
00203   void lock_send_thread();
00204   void unlock_send_thread();
00205   void lock_recv_thread();
00206   void unlock_recv_thread();
00207   void send_int_command(unsigned char command, short value);
00208   void send_signed_int_command(unsigned char command, short value);
00209   void send_noarg_command(unsigned char command);
00210   void send_string_command(unsigned char command, const std::string& data);
00211   void send_unsigned_char_command(unsigned char command, const unsigned char* data, int size);
00212   void configure_parameters(const std::string &robotType, const std::string &robotSubType);
00213   void reinitialize();
00214 
00215   //singleton
00216   static Pioneer2PacketManager *m_instance;
00217   pseudosem_t m_semaphore;
00218   pthread_t m_send_thread;
00219   pthread_t m_recv_thread;
00220   pthread_mutex_t m_send_mutex;
00221   pthread_mutex_t m_recv_mutex;
00222 
00223   volatile bool m_send_running;
00224   volatile bool m_recv_running;
00225 
00226   int m_file_descriptor;
00227   std::list<PSOSPacket> m_send_list;
00228   enum {NO_SYNC, AFTER_FIRST_SYNC, AFTER_SECOND_SYNC, READY} m_psos_state;
00229 
00230  public:
00231 
00232   
00233   static const unsigned char SYNC0;
00234   static const unsigned char SYNC1;
00235   static const unsigned char SYNC2;
00236   static const unsigned char PULSE;
00237   static const unsigned char OPEN;
00238   static const unsigned char CLOSE;
00239   static const unsigned char POLLING;
00240   static const unsigned char ENABLE;
00241   static const unsigned char SETA;
00242   static const unsigned char SETV;
00243   static const unsigned char SET0;
00244   static const unsigned char SETRV;
00245   static const unsigned char VEL;
00246   static const unsigned char HEAD;
00247   static const unsigned char DHEAD;
00248   static const unsigned char SAY;
00249   static const unsigned char CONFIG;
00250   static const unsigned char ENCODER;
00251   static const unsigned char RVEL;
00252   static const unsigned char DCHEAD;
00253   static const unsigned char SETRA;
00254   static const unsigned char SONAR;
00255   static const unsigned char STOP;
00256   static const unsigned char DIGOUT;
00257   static const unsigned char VEL2;
00258   static const unsigned char GRIPPER;
00259   static const unsigned char ADSEL;
00260   static const unsigned char GRIPPERVAL;
00261   static const unsigned char PTUPOS;
00262   static const unsigned char TTY2;
00263   static const unsigned char GETAUX;
00264   static const unsigned char BUMPSTALL;
00265   static const unsigned char TCM2;
00266   static const unsigned char E_STOP;
00267   static const unsigned char STEP;
00268 
00269   //gripper constants
00270   static const short GRIPOPEN;
00271   static const short GRIPCLOSE;
00272   static const short GRIPSTOP;
00273   static const short LIFTUP;
00274   static const short LIFTDOWN;
00275   static const short LIFTSTOP;
00276   static const short GRIPSTORE;
00277   static const short GRIPDEPLOY;
00278   static const short GRIPHALT;
00279   static const short GRIPPRESS;
00280   static const short LIFTCARRY;
00281   
00282 
00283   static const int MAX_QUEUE_SIZE;
00284 
00285 };
00286 
00287 
00288 
00289 }//namespace RobotFlow
00290 
00291 #endif

Generated on Wed Oct 5 14:36:12 2005 for RobotFlow by  doxygen 1.4.4