p2os.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 /*
00024  * $Id: p2os.h,v 1.30 2006/04/04 21:31:37 gbiggs Exp $
00025  *
00026  *   the P2OS device.  it's the parent device for all the P2 'sub-devices',
00027  *   like gripper, position, sonar, etc.  there's a thread here that
00028  *   actually interacts with P2OS via the serial line.  the other
00029  *   "devices" communicate with this thread by putting into and getting
00030  *   data out of shared buffers.
00031  */
00032 #ifndef _P2OSDEVICE_H
00033 #define _P2OSDEVICE_H
00034 
00035 #include <pthread.h>
00036 #include <sys/time.h>
00037 
00038 #include <libplayercore/playercore.h>
00039 #include <replace/replace.h>
00040 
00041 #include "packet.h"
00042 #include "robot_params.h"
00043 
00044 // Default max speeds
00045 #define MOTOR_DEF_MAX_SPEED 0.5
00046 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00047 
00048 /*
00049  * Apparently, newer kernel require a large value (200000) here.  It only
00050  * makes the initialization phase take a bit longer, and doesn't have any
00051  * impact on the speed at which packets are received from P2OS
00052  */
00053 #define P2OS_CYCLETIME_USEC 200000
00054 
00055 /* p2os constants */
00056 
00057 #define P2OS_NOMINAL_VOLTAGE 12.0
00058 
00059 /* Command numbers */
00060 #define SYNC0 0
00061 #define SYNC1 1
00062 #define SYNC2 2
00063 
00064 #define PULSE 0
00065 #define OPEN 1
00066 #define CLOSE 2
00067 #define ENABLE 4
00068 #define SETA 5
00069 #define SETV 6
00070 #define SETO 7
00071 #define VEL 11
00072 #define RVEL 21
00073 #define SETRA 23
00074 #define SONAR 28
00075 #define STOP 29
00076 #define VEL2 32
00077 #define GRIPPER 33
00078 #define GRIPPERVAL 36
00079 #define TTY2 42         // Added in AmigOS 1.2
00080 #define GETAUX 43       // Added in AmigOS 1.2
00081 #define BUMP_STALL 44
00082 #define JOYDRIVE 47
00083 #define GYRO 58         // Added in AROS 1.8
00084 #define ROTKP 82        // Added in P2OS1.M
00085 #define ROTKV 83        // Added in P2OS1.M
00086 #define ROTKI 84        // Added in P2OS1.M
00087 #define TRANSKP 85      // Added in P2OS1.M
00088 #define TRANSKV 86      // Added in P2OS1.M
00089 #define TRANSKI 87      // Added in P2OS1.M
00090 #define TTY3 66         // Added in AmigOS 1.3
00091 #define GETAUX2 67      // Added in AmigOS 1.3
00092 #define ARM_INFO 70
00093 #define ARM_STATUS 71
00094 #define ARM_INIT 72
00095 #define ARM_CHECK 73
00096 #define ARM_POWER 74
00097 #define ARM_HOME 75
00098 #define ARM_PARK 76
00099 #define ARM_POS 77
00100 #define ARM_SPEED 78
00101 #define ARM_STOP 79
00102 #define ARM_AUTOPARK 80
00103 #define ARM_GRIPPARK 81
00104 #define SOUND 90
00105 #define PLAYLIST 91
00106 
00107 /* Server Information Packet (SIP) types */
00108 #define STATUSSTOPPED   0x32
00109 #define STATUSMOVING    0x33
00110 #define ENCODER         0x90
00111 #define SERAUX          0xB0
00112 #define SERAUX2         0xB8    // Added in AmigOS 1.3
00113 #define GYROPAC         0x98    // Added AROS 1.8
00114 #define ARMPAC    160   // ARMpac
00115 #define ARMINFOPAC  161   // ARMINFOpac
00116 //#define PLAYLIST      0xD0
00117 
00118 /* Argument types */
00119 #define ARGINT          0x3B    // Positive int (LSB, MSB)
00120 #define ARGNINT         0x1B    // Negative int (LSB, MSB)
00121 #define ARGSTR          0x2B    // String (Note: 1st byte is length!!)
00122 
00123 /* gripper stuff */
00124 #define GRIPopen   1
00125 #define GRIPclose  2
00126 #define GRIPstop   3
00127 #define LIFTup     4
00128 #define LIFTdown   5
00129 #define LIFTstop   6
00130 #define GRIPstore  7
00131 #define GRIPdeploy 8
00132 #define GRIPhalt   15
00133 #define GRIPpress  16
00134 #define LIFTcarry  17
00135 
00136 /* CMUcam stuff */
00137 #define CMUCAM_IMAGE_WIDTH      80
00138 #define CMUCAM_IMAGE_HEIGHT     143
00139 #define CMUCAM_MESSAGE_LEN      10
00140 
00141 /* conection stuff */
00142 #define DEFAULT_P2OS_PORT "/dev/ttyS0"
00143 #define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost"
00144 #define DEFAULT_P2OS_TCP_REMOTE_PORT 8101
00145 
00146 typedef struct player_p2os_data
00147 {
00148   player_position2d_data_t position;
00149   player_sonar_data_t sonar;
00150   player_gripper_data_t gripper;
00151   player_power_data_t power;
00152   player_bumper_data_t bumper;
00153   player_dio_data_t dio;
00154   player_aio_data_t aio;
00155   player_blobfinder_data_t blobfinder;
00156   player_position2d_data_t compass;
00157   player_position2d_data_t gyro;
00158   player_actarray_data_t actarray;
00159 } __attribute__ ((packed)) player_p2os_data_t;
00160 
00161 // this is here because we need the above typedef's before including it.
00162 #include <sip.h>
00163 
00164 #include "kinecalc.h"
00165 
00166 class SIP;
00167 
00168 // Forward declaration of the KineCalc_Base class declared in kinecalc_base.h
00169 //class KineCalc;
00170 
00171 class P2OS : public Driver
00172 {
00173   private:
00174     player_p2os_data_t p2os_data;
00175 
00176     player_devaddr_t position_id;
00177     player_devaddr_t sonar_id;
00178     player_devaddr_t aio_id;
00179     player_devaddr_t dio_id;
00180     player_devaddr_t gripper_id;
00181     player_devaddr_t bumper_id;
00182     player_devaddr_t power_id;
00183     player_devaddr_t compass_id;
00184     player_devaddr_t gyro_id;
00185     player_devaddr_t blobfinder_id;
00186     player_devaddr_t sound_id;
00187     player_devaddr_t actarray_id;
00188     player_devaddr_t limb_id;
00189 
00190     // bookkeeping to only send new gripper I/O commands
00191     bool sent_gripper_cmd;
00192     player_gripper_cmd_t last_gripper_cmd;
00193 
00194     // Same for actarray commands
00195     bool last_actarray_cmd_was_pos;
00196     player_actarray_position_cmd_t last_actarray_pos_cmd;
00197     player_actarray_home_cmd_t last_actarray_home_cmd;
00198 
00199     // bookkeeping to only send new sound I/O commands
00200     bool sent_sound_cmd;
00201     player_sound_cmd_t last_sound_cmd;
00202     // PID settings
00203     int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;
00204 
00205 
00206     int position_subscriptions;
00207     int sonar_subscriptions;
00208     int actarray_subscriptions;
00209 
00210     SIP* sippacket;
00211 
00212     int SendReceive(P2OSPacket* pkt, bool publish_data=true);
00213     void ResetRawPositions();
00214     /* toggle sonars on/off, according to val */
00215     void ToggleSonarPower(unsigned char val);
00216     /* toggle motors on/off, according to val */
00217     void ToggleMotorPower(unsigned char val);
00218     int HandleConfig(MessageQueue* resp_queue,
00219                      player_msghdr * hdr,
00220                      void* data);
00221     int HandleCommand(player_msghdr * hdr, void * data);
00222     void PutData(void);
00223     void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
00224     void HandleGripperCommand(player_gripper_cmd_t gripper_cmd);
00225     void HandleSoundCommand(player_sound_cmd_t sound_cmd);
00226 
00228     // Actarray stuff
00229     inline double TicksToDegrees (int joint, unsigned char ticks);
00230     inline unsigned char DegreesToTicks (int joint, double degrees);
00231     inline double TicksToRadians (int joint, unsigned char ticks);
00232     inline unsigned char RadiansToTicks (int joint, double rads);
00233     inline double RadsPerSectoSecsPerTick (int joint, double speed);
00234     inline double SecsPerTicktoRadsPerSec (int joint, double secs);
00235     void ToggleActArrayPower (unsigned char val, bool lock = true);   // Toggle actarray power on/off
00236     void SetActArrayJointSpeed (int joint, double speed);             // Set a joint speed
00237     void HandleActArrayPosCmd (player_actarray_position_cmd_t cmd);
00238     void HandleActArrayHomeCmd (player_actarray_home_cmd_t cmd);
00239 
00241     // Limb stuff
00242     KineCalc *kineCalc;
00243     float armOffsetX, armOffsetY, armOffsetZ;
00244     // This is here because we don't want it zeroed every time someone fills in some other data
00245     player_limb_data_t limb_data;
00246 
00247     void HandleLimbHomeCmd (void);
00248     void HandleLimbStopCmd (void);
00249     void HandleLimbSetPoseCmd (player_limb_setpose_cmd_t cmd);
00250     void HandleLimbSetPositionCmd (player_limb_setposition_cmd_t cmd);
00251     void HandleLimbVecMoveCmd (player_limb_vecmove_cmd_t cmd);
00252 
00253     int param_idx;  // index in the RobotParams table for this robot
00254     int direct_wheel_vel_control; // false -> separate trans and rot vel
00255     int psos_fd;               // p2os device file descriptor
00256     const char* psos_serial_port; // name of serial port device
00257     bool psos_use_tcp;    // use TCP port instead of serial port
00258     const char* psos_tcp_host;  // hostname to use if using TCP
00259     int psos_tcp_port;  // remote port to use if using TCP
00260 
00261 
00262     struct timeval lastblob_tv;
00263 
00264     // Max motor speeds (mm/sec,deg/sec)
00265     int motor_max_speed;
00266     int motor_max_turnspeed;
00267 
00268     // Bound the command velocities
00269     bool use_vel_band;
00270 
00271     // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
00272     short motor_max_trans_accel, motor_max_trans_decel;
00273     short motor_max_rot_accel, motor_max_rot_decel;
00274 
00275     int radio_modemp; // are we using a radio modem?
00276     int joystickp; // are we using a joystick?
00277     int bumpstall; // should we change the bumper-stall behavior?
00278 
00279     float pulse; // Pulse time
00280     double lastPulseTime; // Last time of sending a pulse or command to the robot
00281     void SendPulse (void);
00282 
00283   public:
00284 
00285     P2OS(ConfigFile* cf, int section);
00286         ~P2OS (void);
00287 
00288     virtual int Subscribe(player_devaddr_t id);
00289     virtual int Unsubscribe(player_devaddr_t id);
00290 
00291     /* the main thread */
00292     virtual void Main();
00293 
00294     virtual int Setup();
00295     virtual int Shutdown();
00296 
00297     // MessageHandler
00298     virtual int ProcessMessage(MessageQueue * resp_queue,
00299                                player_msghdr * hdr,
00300                                void * data);
00301 
00302     void CMUcamReset(bool doLock = true);
00303     void CMUcamTrack(int rmin=0, int rmax=0, int gmin=0,
00304                           int gmax=0, int bmin=0, int bmax=0);
00305     void CMUcamStartTracking(bool doLock = true);
00306     void CMUcamStopTracking(bool doLock = true);
00307 };
00308 
00309 
00310 #endif

Last updated 12 September 2005 21:38:45