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 4356 2008-02-15 08:53:55Z thjc $
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   //Standard SIP
00149   player_position2d_data_t position;
00150   player_sonar_data_t sonar;
00151   player_gripper_data_t gripper;
00152   player_actarray_data_t lift;
00153   player_power_data_t power;
00154   player_bumper_data_t bumper;
00155   player_position2d_data_t compass;
00156   player_dio_data_t dio;
00157   player_aio_data_t aio;
00158   
00159   //Blobfinder
00160   player_blobfinder_data_t blobfinder;
00161 
00162   //Gyro
00163   player_position2d_data_t gyro;
00164   
00165   //ARMPAC
00166   player_actarray_data_t actArray;
00167   player_gripper_data_t armGripper;
00168 } __attribute__ ((packed)) player_p2os_data_t;
00169 
00170 // this is here because we need the above typedef's before including it.
00171 #include <sip.h>
00172 
00173 #include "kinecalc.h"
00174 
00175 class SIP;
00176 
00177 // Forward declaration of the KineCalc_Base class declared in kinecalc_base.h
00178 //class KineCalc;
00179 
00180 class P2OS : public Driver
00181 {
00182   private:
00183     player_p2os_data_t p2os_data;
00184 
00185     player_devaddr_t position_id;
00186     player_devaddr_t sonar_id;
00187     player_devaddr_t aio_id;
00188     player_devaddr_t dio_id;
00189     player_devaddr_t gripper_id;
00190     player_devaddr_t lift_id;
00191     player_devaddr_t bumper_id;
00192     player_devaddr_t power_id;
00193     player_devaddr_t compass_id;
00194     player_devaddr_t gyro_id;
00195     player_devaddr_t blobfinder_id;
00196     player_devaddr_t audio_id;
00197     player_devaddr_t actarray_id;
00198     player_devaddr_t limb_id;
00199     player_devaddr_t armgripper_id;
00200 
00201     // Book keeping to only send new commands
00202     bool sentGripperCmd;
00203     uint8_t lastGripperCmd;
00204     uint8_t lastLiftCmd;
00205     player_actarray_position_cmd_t lastLiftPosCmd;
00206     bool sentArmGripperCmd;
00207     uint8_t lastArmGripperCmd;
00208     uint8_t lastActArrayCmd;
00209     player_actarray_position_cmd_t lastActArrayPosCmd;
00210     player_actarray_home_cmd_t lastActArrayHomeCmd;
00211 
00212     // bookkeeping to only send new audio I/O commands
00213     bool sent_audio_cmd;
00214     player_audio_sample_item_t last_audio_cmd;
00215     // PID settings
00216     int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;
00217 
00218 
00219     int position_subscriptions;
00220     int sonar_subscriptions;
00221     int actarray_subscriptions;
00222 
00223     SIP* sippacket;
00224 
00225     int SendReceive(P2OSPacket* pkt, bool publish_data=true);
00226     void ResetRawPositions();
00227     /* toggle sonars on/off, according to val */
00228     void ToggleSonarPower(unsigned char val);
00229     /* toggle motors on/off, according to val */
00230     void ToggleMotorPower(unsigned char val);
00231     int HandleConfig(QueuePointer & resp_queue,
00232                      player_msghdr * hdr,
00233                      void* data);
00234     int HandleCommand(player_msghdr * hdr, void * data);
00235     void StandardSIPPutData(double timestampStandardSIP);
00236     void GyroPutData(double timestampGyro);
00237     void BlobfinderPutData(double timestampSERAUX);
00238     void ActarrayPutData(double timestampArm);
00239     void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
00240     int HandleGripperCommand (player_msghdr *hdr, void *data);
00241     int HandleLiftCommand (player_msghdr *hdr, void *data);
00242     int HandleArmGripperCommand (player_msghdr *hdr, void *data);
00243     void HandleAudioCommand(player_audio_sample_item_t audio_cmd);
00244 
00246     // Gripper stuff
00247     player_pose3d_t gripperPose;
00248     player_bbox3d_t gripperOuterSize;
00249     player_bbox3d_t gripperInnerSize;
00250     player_bbox3d_t armGripperOuterSize;
00251     player_bbox3d_t armGripperInnerSize;
00252     void OpenGripper (void);
00253     void CloseGripper (void);
00254     void StopGripper (void);
00255     void OpenArmGripper (void);
00256     void CloseArmGripper (void);
00257     void StopArmGripper (void);
00258 
00260     // Actarray stuff
00261     double aaLengths[6];
00262     double aaOrients[18];
00263     double aaAxes[18];
00264     player_point_3d_t aaBasePos;
00265     player_orientation_3d_t aaBaseOrient;
00266     inline double TicksToDegrees (int joint, unsigned char ticks);
00267     inline unsigned char DegreesToTicks (int joint, double degrees);
00268     inline double TicksToRadians (int joint, unsigned char ticks);
00269     inline unsigned char RadiansToTicks (int joint, double rads);
00270     inline double RadsPerSectoSecsPerTick (int joint, double speed);
00271     inline double SecsPerTicktoRadsPerSec (int joint, double secs);
00272     void ToggleActArrayPower (unsigned char val, bool lock = true);   // Toggle actarray power on/off
00273     void SetActArrayJointSpeed (int joint, double speed);             // Set a joint speed
00274     void HandleActArrayPosCmd (player_actarray_position_cmd_t cmd);
00275     void HandleActArrayHomeCmd (player_actarray_home_cmd_t cmd);
00276     int HandleActArrayCommand (player_msghdr * hdr, void * data);
00277 
00279     // Limb stuff
00280     KineCalc *kineCalc;
00281     float armOffsetX, armOffsetY, armOffsetZ;
00282     // This is here because we don't want it zeroed every time someone fills in some other data
00283     player_limb_data_t limb_data;
00284     void HandleLimbHomeCmd (void);
00285     void HandleLimbStopCmd (void);
00286     void HandleLimbSetPoseCmd (player_limb_setpose_cmd_t cmd);
00287     void HandleLimbSetPositionCmd (player_limb_setposition_cmd_t cmd);
00288     void HandleLimbVecMoveCmd (player_limb_vecmove_cmd_t cmd);
00289     int HandleLimbCommand (player_msghdr * hdr, void * data);
00290 
00291     int param_idx;  // index in the RobotParams table for this robot
00292     int direct_wheel_vel_control; // false -> separate trans and rot vel
00293     int psos_fd;               // p2os device file descriptor
00294     const char* psos_serial_port; // name of serial port device
00295     bool psos_use_tcp;    // use TCP port instead of serial port
00296     const char* psos_tcp_host;  // hostname to use if using TCP
00297     int psos_tcp_port;  // remote port to use if using TCP
00298 
00299 
00300     struct timeval lastblob_tv;
00301 
00302     // Max motor speeds (mm/sec,deg/sec)
00303     int motor_max_speed;
00304     int motor_max_turnspeed;
00305 
00306     // Bound the command velocities
00307     bool use_vel_band;
00308 
00309     // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
00310     short motor_max_trans_accel, motor_max_trans_decel;
00311     short motor_max_rot_accel, motor_max_rot_decel;
00312 
00313     int radio_modemp; // are we using a radio modem?
00314     int joystickp; // are we using a joystick?
00315     int bumpstall; // should we change the bumper-stall behavior?
00316 
00317     float pulse; // Pulse time
00318     double lastPulseTime; // Last time of sending a pulse or command to the robot
00319     void SendPulse (void);
00320 
00321   public:
00322 
00323     P2OS(ConfigFile* cf, int section);
00324         ~P2OS (void);
00325 
00326     virtual int Subscribe(player_devaddr_t id);
00327     virtual int Unsubscribe(player_devaddr_t id);
00328 
00329     /* the main thread */
00330     virtual void Main();
00331 
00332     virtual int Setup();
00333     virtual int Shutdown();
00334 
00335     // MessageHandler
00336     virtual int ProcessMessage(QueuePointer & resp_queue,
00337                                player_msghdr * hdr,
00338                                void * data);
00339 
00340     void CMUcamReset(bool doLock = true);
00341     void CMUcamTrack(int rmin=0, int rmax=0, int gmin=0,
00342                           int gmax=0, int bmin=0, int bmax=0);
00343     void CMUcamStartTracking(bool doLock = true);
00344     void CMUcamStopTracking(bool doLock = true);
00345 };
00346 
00347 
00348 #endif

Last updated 12 September 2005 21:38:45