reb.h
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000
00004  *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
00005  *
00006  *
00007  *  This program is free software; you can redistribute it and/or modify
00008  *  it under the terms of the GNU General Public License as published by
00009  *  the Free Software Foundation; either version 2 of the License, or
00010  *  (at your option) any later version.
00011  *
00012  *  This program is distributed in the hope that it will be useful,
00013  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *  GNU General Public License for more details.
00016  *
00017  *  You should have received a copy of the GNU General Public License
00018  *  along with this program; if not, write to the Free Software
00019  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  *
00021  */
00022 
00023 /* Copyright (C) 2002
00024  *   John Sweeney, UMASS, Amherst, Laboratory for Perceptual Robotics
00025  *
00026  * $Id: reb.h 7297 2009-01-24 23:14:21Z thjc $
00027  *
00028  * Header for the REB device.  This is the K-Team Robotics Extension
00029  * Board attached to their Kameleon 376BC.  We connect to it via
00030  * the serial port of our ADS Bitsy (StrongARM based).  SO the
00031  * architecture is similar to the P2OS device, in that the position, IR and
00032  * power services all need to go through a single serial port and
00033  * base device class.  So this code was copied from p2osdevice and
00034  * modified to taste.
00035  *
00036  */
00037 
00038 #ifndef _REBDEVICE_H
00039 #define _REBDEVICE_H
00040 
00041 #include <pthread.h>
00042 #include <sys/time.h>
00043 #include <errno.h>
00044 
00045 // for poll
00046 #include <replace/replace.h>
00047 #include <libplayercore/playercore.h>
00048 
00049 #include <reb_params.h>
00050 
00051 
00052 #define REB_CONFIG_BUFFER_SIZE 1024
00053 #define REB_BAUDRATE B38400
00054 #define REB_DEFAULT_SERIAL_PORT "/dev/ttySA1"
00055 
00056 #define REB_MOTOR_RIGHT 0
00057 #define REB_MOTOR_LEFT 2
00058 
00059 #define REB_BATTERY_CHANNEL 15
00060 
00061 #define REB_AD_OFF 0
00062 #define REB_AD_ON 1
00063 
00064 #define REB_FIXED_FACTOR 10000
00065 
00066 #define REB_MAX_ACC 100
00067 #define REB_MIN_ACC 10
00068 
00069 #define REB_POS_MODE_STRAIGHT 0
00070 #define REB_POS_MODE_ROTATION 1
00071 
00072 #define REB_IR_START 1
00073 #define REB_IR_STOP 0
00074 
00075 #define CRLF "\r\n"
00076 #define REB_RESTART_COMMAND "restart\r\n"
00077 #define REB_COMMAND_PROMPT ":\r\n"
00078 
00079 #ifndef ABS
00080 #define ABS(x) ((x) < 0 ? -(x) : (x))
00081 #endif
00082 
00083 #ifndef SGN
00084 #define SGN(x) ((x) < 0 ? -1 : 1)
00085 #endif
00086 
00087 /*
00088 typedef struct {
00089   player_position_data_t position;
00090   player_ir_data_t ir;
00091   player_power_data_t power;
00092 } __attribute__ ((packed)) player_reb_data_t;
00093 
00094 typedef struct {
00095   player_position_cmd_t position;
00096 } __attribute__ ((packed)) player_reb_cmd_t;
00097 */
00098 
00099 
00100 class REB : public ThreadedDriver
00101 {
00102 public:
00103 
00104   REB(ConfigFile *cf, int section);
00105 
00106   /* the main thread */
00107   virtual void Main();
00108 
00109   int ProcessMessage(ClientData * client, player_msghdr * hdr, uint8_t * data, uint8_t * resp_data, size_t * resp_len);
00110 
00111   // we override these, because we will maintain our own subscription count
00112   virtual int Subscribe(player_device_id_t id);
00113   virtual int Unsubscribe(player_device_id_t id);
00114 
00115   virtual int MainSetup();
00116   virtual void MainQuit();
00117 
00118   void Restart();
00119 
00120   void ReadConfig();
00121 
00122   void SetOdometry(int, int, short);
00123 
00124   // handle IR
00125   void SetIRState(int);
00126 
00127   void UpdateData(void);
00128 
00129   void UpdateIRData(player_ir_data_t *);
00130   void UpdatePowerData(player_power_data_t *);
00131   void UpdatePosData(player_position_data_t *);
00132 
00133   // the following are all interface functions to the REB
00134   // this handles the A/D device which deals with IR for us
00135   void ConfigAD(int, int);
00136   unsigned short ReadAD(int);
00137   void ReadAllIR(uint16_t * ir);
00138 
00139   // this handles motor control
00140   void SetSpeed(int, int );
00141   int ReadSpeed(int);
00142 
00143   void SetPos(int, int);
00144 
00145   void SetPosCounter(int, int);
00146   int ReadPos(int);
00147 
00148   unsigned char ReadStatus(int, int *, int *);
00149   void ConfigPosPID(int, int, int, int);
00150   void ConfigSpeedPID(int, int, int, int);
00151   void ConfigSpeedProfile(int, int, int);
00152 
00153 private:
00154 
00155   int write_serial(char *, int);
00156   int read_serial_until(char *, int, char *, int);
00157   int write_command(char *buf, int len, int maxsize);
00158 
00159   player_device_id_t ir_id;
00160   player_device_id_t position_id;
00161   player_device_id_t power_id;
00162 
00163   int ir_subscriptions;
00164   int position_subscriptions;
00165 
00166   int param_index;  // index in the RobotParams table for this robot
00167   int reb_fd;               // reb device file descriptor
00168 
00169   struct timeval last_position; // last position update
00170   bool refresh_last_position;
00171   int last_lpos, last_rpos;
00172   int last_x_f, last_y_f;
00173   double last_theta;
00174 
00175   struct timeval last_pos_update; // time of last pos update
00176   struct timeval last_power_update;
00177   struct timeval last_ir_update;
00178 
00179   int pos_update_period;
00180 
00181   int current_heading;
00182   short desired_heading;
00183 
00184   int ir_sequence;
00185   struct timeval last_ir;
00186 
00187   bool motors_enabled;
00188   bool velocity_mode;
00189   bool direct_velocity_control;
00190 
00191   // device used to communicate with reb
00192   char reb_serial_port[MAX_FILENAME_SIZE];
00193 
00194   struct pollfd write_pfd, read_pfd;
00195 
00196   // holding vars for command processing
00197   int ProcessCommand(player_position_cmd_t * poscmd);
00198   short last_trans_command, last_rot_command;
00199   int leftvel, rightvel;
00200   int leftpos, rightpos;
00201 };
00202 
00203 
00204 #endif