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 8184 2009-08-07 08:48:58Z gbiggs $
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 
00045 // Default max speeds
00046 #define MOTOR_DEF_MAX_SPEED 0.5
00047 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00048 
00049 /*
00050  * Apparently, newer kernel require a large value (200000) here.  It only
00051  * makes the initialization phase take a bit longer, and doesn't have any
00052  * impact on the speed at which packets are received from P2OS
00053  */
00054 #define P2OS_CYCLETIME_USEC 200000
00055 
00056 /* p2os constants */
00057 
00058 #define P2OS_NOMINAL_VOLTAGE 12.0
00059 
00060 /* Command numbers */
00061 #define SYNC0 0
00062 #define SYNC1 1
00063 #define SYNC2 2
00064 
00065 #define PULSE 0
00066 #define OPEN 1
00067 #define CLOSE 2
00068 #define ENABLE 4
00069 #define SETA 5
00070 #define SETV 6
00071 #define SETO 7
00072 #define VEL 11
00073 #define RVEL 21
00074 #define SETRA 23
00075 #define SONAR 28
00076 #define STOP 29
00077 #define VEL2 32
00078 #define GRIPPER 33
00079 #define GRIPPERVAL 36
00080 #define TTY2 42         // Added in AmigOS 1.2
00081 #define GETAUX 43       // Added in AmigOS 1.2
00082 #define BUMP_STALL 44
00083 #define JOYDRIVE 47
00084 #define GYRO 58         // Added in AROS 1.8
00085 #define ROTKP 82        // Added in P2OS1.M
00086 #define ROTKV 83        // Added in P2OS1.M
00087 #define ROTKI 84        // Added in P2OS1.M
00088 #define TRANSKP 85      // Added in P2OS1.M
00089 #define TRANSKV 86      // Added in P2OS1.M
00090 #define TRANSKI 87      // Added in P2OS1.M
00091 #define TTY3 66         // Added in AmigOS 1.3
00092 #define GETAUX2 67      // Added in AmigOS 1.3
00093 #define ARM_INFO 70
00094 #define ARM_STATUS 71
00095 #define ARM_INIT 72
00096 #define ARM_CHECK 73
00097 #define ARM_POWER 74
00098 #define ARM_HOME 75
00099 #define ARM_PARK 76
00100 #define ARM_POS 77
00101 #define ARM_SPEED 78
00102 #define ARM_STOP 79
00103 #define ARM_AUTOPARK 80
00104 #define ARM_GRIPPARK 81
00105 #define SOUND 90
00106 #define PLAYLIST 91
00107 
00108 /* Server Information Packet (SIP) types */
00109 #define STATUSSTOPPED   0x32
00110 #define STATUSMOVING    0x33
00111 #define ENCODER         0x90
00112 #define SERAUX          0xB0
00113 #define SERAUX2         0xB8    // Added in AmigOS 1.3
00114 #define GYROPAC         0x98    // Added AROS 1.8
00115 #define ARMPAC    160   // ARMpac
00116 #define ARMINFOPAC  161   // ARMINFOpac
00117 //#define PLAYLIST      0xD0
00118 
00119 /* Argument types */
00120 #define ARGINT          0x3B    // Positive int (LSB, MSB)
00121 #define ARGNINT         0x1B    // Negative int (LSB, MSB)
00122 #define ARGSTR          0x2B    // String (Note: 1st byte is length!!)
00123 
00124 /* gripper stuff */
00125 #define GRIPopen   1
00126 #define GRIPclose  2
00127 #define GRIPstop   3
00128 #define LIFTup     4
00129 #define LIFTdown   5
00130 #define LIFTstop   6
00131 #define GRIPstore  7
00132 #define GRIPdeploy 8
00133 #define GRIPhalt   15
00134 #define GRIPpress  16
00135 #define LIFTcarry  17
00136 
00137 /* CMUcam stuff */
00138 #define CMUCAM_IMAGE_WIDTH      80
00139 #define CMUCAM_IMAGE_HEIGHT     143
00140 #define CMUCAM_MESSAGE_LEN      10
00141 
00142 /* conection stuff */
00143 #define DEFAULT_P2OS_PORT "/dev/ttyS0"
00144 #define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost"
00145 #define DEFAULT_P2OS_TCP_REMOTE_PORT 8101
00146 
00147 /* Canon PTZ (VC-C4) Stuff */
00148 #define CAM_ERROR_NONE 0x30
00149 #define CAM_ERROR_BUSY 0x31
00150 #define CAM_ERROR_PARAM 0x35
00151 #define CAM_ERROR_MODE 0x39
00152 
00153 #define PTZ_SLEEP_TIME_USEC 100000
00154 
00155 #define MAX_PTZ_COMMAND_LENGTH 19
00156 #define MAX_PTZ_REQUEST_LENGTH 17
00157 #define COMMAND_RESPONSE_BYTES 6
00158 
00159 #define PTZ_PAN_MAX 98.0   // 875 units 0x36B
00160 #define PTZ_TILT_MAX 88.0  // 790 units 0x316
00161 #define PTZ_TILT_MIN -30.0 // -267 units 0x10B
00162 #define MAX_ZOOM 1960 //1900
00163 #define ZOOM_CONV_FACTOR 17
00164 
00165 #define PT_BUFFER_INC       512
00166 #define PT_READ_TIMEOUT   10000
00167 #define PT_READ_TRIALS        2
00168 
00169 typedef struct player_p2os_data
00170 {
00171   //Standard SIP
00172   player_position2d_data_t position;
00173   player_sonar_data_t sonar;
00174   player_gripper_data_t gripper;
00175   player_actarray_data_t lift;
00176   player_power_data_t power;
00177   player_bumper_data_t bumper;
00178   player_position2d_data_t compass;
00179   player_dio_data_t dio;
00180   player_aio_data_t aio;
00181 
00182   //Blobfinder
00183   player_blobfinder_data_t blobfinder;
00184 
00185   //Gyro
00186   player_position2d_data_t gyro;
00187 
00188   //ARMPAC
00189   player_actarray_data_t actArray;
00190   player_gripper_data_t armGripper;
00191 } __attribute__ ((packed)) player_p2os_data_t;
00192 
00193 // this is here because we need the above typedef's before including it.
00194 #include "sip.h"
00195 
00196 #include "kinecalc.h"
00197 
00198 class SIP;
00199 
00200 // Circular Buffer Used by PTZ camera
00201 class circbuf{
00202     public:
00203         circbuf(int size=512);
00204 
00205         void putOnBuf(unsigned char c);
00206         int  getFromBuf();
00207         bool haveData();
00208         int  size();
00209         void printBuf();
00210 
00211         bool gotPacket();
00212         void reset();
00213 
00214     private:
00215         unsigned char* buf;
00216         int  start;
00217         int  end;
00218         int  mysize;
00219         bool gotPack;
00220 };
00221 
00222 // Forward declaration of the KineCalc_Base class declared in kinecalc_base.h
00223 //class KineCalc;
00224 
00225 class P2OS : public ThreadedDriver
00226 {
00227   private:
00228     player_p2os_data_t p2os_data;
00229 
00230     player_devaddr_t position_id;
00231     player_devaddr_t sonar_id;
00232     player_devaddr_t aio_id;
00233     player_devaddr_t dio_id;
00234     player_devaddr_t gripper_id;
00235     player_devaddr_t lift_id;
00236     player_devaddr_t bumper_id;
00237     player_devaddr_t power_id;
00238     player_devaddr_t compass_id;
00239     player_devaddr_t gyro_id;
00240     player_devaddr_t blobfinder_id;
00241     player_devaddr_t audio_id;
00242     player_devaddr_t actarray_id;
00243     player_devaddr_t limb_id;
00244     player_devaddr_t ptz_id;
00245     player_devaddr_t armgripper_id;
00246 
00247     // Book keeping to only send new commands
00248     bool sentGripperCmd;
00249     uint8_t lastGripperCmd;
00250     uint8_t lastLiftCmd;
00251     player_actarray_position_cmd_t lastLiftPosCmd;
00252     bool sentArmGripperCmd;
00253     uint8_t lastArmGripperCmd;
00254     uint8_t lastActArrayCmd;
00255     player_actarray_position_cmd_t lastActArrayPosCmd;
00256     player_actarray_home_cmd_t lastActArrayHomeCmd;
00257 
00258     // bookkeeping to only send new audio I/O commands
00259     bool sent_audio_cmd;
00260     player_audio_sample_item_t last_audio_cmd;
00261     // PID settings
00262     int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;
00263 
00264 
00265     int position_subscriptions;
00266     int sonar_subscriptions;
00267     int actarray_subscriptions;
00268     int ptz_subscriptions;
00269 
00270     SIP* sippacket;
00271 
00272     int SendReceive(P2OSPacket* pkt, bool publish_data=true);
00273     void ResetRawPositions();
00274     /* toggle sonars on/off, according to val */
00275     void ToggleSonarPower(unsigned char val);
00276     /* toggle motors on/off, according to val */
00277     void ToggleMotorPower(unsigned char val);
00278     int HandleConfig(QueuePointer & resp_queue,
00279                      player_msghdr * hdr,
00280                      void* data);
00281     int HandleCommand(player_msghdr * hdr, void * data);
00282     void StandardSIPPutData(double timestampStandardSIP);
00283     void GyroPutData(double timestampGyro);
00284     void BlobfinderPutData(double timestampSERAUX);
00285     void ActarrayPutData(double timestampArm);
00286     void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
00287     int HandleGripperCommand (player_msghdr *hdr, void *data);
00288     int HandleLiftCommand (player_msghdr *hdr, void *data);
00289     int HandleArmGripperCommand (player_msghdr *hdr, void *data);
00290     void HandleAudioCommand(player_audio_sample_item_t audio_cmd);
00291 
00293     void get_ptz_packet(int s1, int s2=0);
00294     int SetupPtz();
00295 
00297     // Gripper stuff
00298     player_pose3d_t gripperPose;
00299     player_bbox3d_t gripperOuterSize;
00300     player_bbox3d_t gripperInnerSize;
00301     player_bbox3d_t armGripperOuterSize;
00302     player_bbox3d_t armGripperInnerSize;
00303     void OpenGripper (void);
00304     void CloseGripper (void);
00305     void StopGripper (void);
00306     void OpenArmGripper (void);
00307     void CloseArmGripper (void);
00308     void StopArmGripper (void);
00309 
00311     // Actarray stuff
00312     double aaLengths[6];
00313     double aaOrients[18];
00314     double aaAxes[18];
00315     player_point_3d_t aaBasePos;
00316     player_orientation_3d_t aaBaseOrient;
00317     inline double TicksToDegrees (int joint, unsigned char ticks);
00318     inline unsigned char DegreesToTicks (int joint, double degrees);
00319     inline double TicksToRadians (int joint, unsigned char ticks);
00320     inline unsigned char RadiansToTicks (int joint, double rads);
00321     inline double RadsPerSectoSecsPerTick (int joint, double speed);
00322     inline double SecsPerTicktoRadsPerSec (int joint, double secs);
00323     void ToggleActArrayPower (unsigned char val, bool lock = true);   // Toggle actarray power on/off
00324     void SetActArrayJointSpeed (int joint, double speed);             // Set a joint speed
00325     void HandleActArrayPosCmd (player_actarray_position_cmd_t cmd);
00326     void HandleActArrayHomeCmd (player_actarray_home_cmd_t cmd);
00327     int HandleActArrayCommand (player_msghdr * hdr, void * data);
00328 
00330     // Limb stuff
00331     KineCalc *kineCalc;
00332     float armOffsetX, armOffsetY, armOffsetZ;
00333     // This is here because we don't want it zeroed every time someone fills in some other data
00334     player_limb_data_t limb_data;
00335     void HandleLimbHomeCmd (void);
00336     void HandleLimbStopCmd (void);
00337     void HandleLimbSetPoseCmd (player_limb_setpose_cmd_t cmd);
00338     void HandleLimbSetPositionCmd (player_limb_setposition_cmd_t cmd);
00339     void HandleLimbVecMoveCmd (player_limb_vecmove_cmd_t cmd);
00340     int HandleLimbCommand (player_msghdr * hdr, void * data);
00341 
00342     int param_idx;  // index in the RobotParams table for this robot
00343     int direct_wheel_vel_control; // false -> separate trans and rot vel
00344     int psos_fd;               // p2os device file descriptor
00345     const char* psos_serial_port; // name of serial port device
00346     bool psos_use_tcp;    // use TCP port instead of serial port
00347     const char* psos_tcp_host;  // hostname to use if using TCP
00348     int psos_tcp_port;  // remote port to use if using TCP
00349 
00350     struct timeval lastblob_tv;
00351 
00352     // Max motor speeds (mm/sec,deg/sec)
00353     int motor_max_speed;
00354     int motor_max_turnspeed;
00355 
00356     // Bound the command velocities
00357     bool use_vel_band;
00358 
00359     // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
00360     short motor_max_trans_accel, motor_max_trans_decel;
00361     short motor_max_rot_accel, motor_max_rot_decel;
00362 
00363     int radio_modemp; // are we using a radio modem?
00364     int joystickp; // are we using a joystick?
00365     int bumpstall; // should we change the bumper-stall behavior?
00366     bool ignore_checksum;
00367 
00368     // PTZ Camera Stuff
00369     player_ptz_data_t  ptz_data;
00370     int maxfov, minfov;
00371     int maxzoom;
00372     int pandemand, tiltdemand, zoomdemand;
00373     int SendCommand(unsigned char *str, int len);
00374     int SendRequest(unsigned char* str, int len, unsigned char* reply, uint8_t camera = 1);
00375     void PrintPacket(char* str, unsigned char* cmd, int len);
00376     int SendAbsPanTilt(int pan, int tilt);
00377     int setDefaultTiltRange();
00378     int GetAbsPanTilt(int* pan, int* tilt);
00379     int GetAbsZoom(int* zoom);
00380     int SendAbsZoom(int zoom);
00381     int read_ptz(unsigned char *reply, int size);
00382     int ReceiveCommandAnswer(int asize);
00383     int ReceiveRequestAnswer(unsigned char *data, int s1, int s2);
00384     int setControlMode();
00385     int setNotifyCommand();
00386     int setPower(int on);
00387     int setOnScreenOff();
00388     int CheckHostControlMode();
00389     int sendInit();
00390     int GetMaxZoom(int * maxzoom);
00391     circbuf cb;
00392 
00393     float pulse; // Pulse time
00394     double lastPulseTime; // Last time of sending a pulse or command to the robot
00395     void SendPulse (void);
00396 
00397   public:
00398 
00399     P2OS(ConfigFile* cf, int section);
00400         ~P2OS (void);
00401 
00402     virtual int Subscribe(player_devaddr_t id);
00403     virtual int Unsubscribe(player_devaddr_t id);
00404 
00405     /* the main thread */
00406     virtual void Main();
00407 
00408     virtual int MainSetup();
00409     virtual void MainQuit();
00410 
00411     // MessageHandler
00412     virtual int ProcessMessage(QueuePointer & resp_queue,
00413                                player_msghdr * hdr,
00414                                void * data);
00415 
00416     void CMUcamReset(bool doLock = true);
00417     void CMUcamTrack(int rmin=0, int rmax=0, int gmin=0,
00418                           int gmax=0, int bmin=0, int bmax=0);
00419     void CMUcamStartTracking(bool doLock = true);
00420     void CMUcamStopTracking(bool doLock = true);
00421 };
00422 
00423 
00424 #endif