00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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
00046 #define MOTOR_DEF_MAX_SPEED 0.5
00047 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00048
00049
00050
00051
00052
00053
00054 #define P2OS_CYCLETIME_USEC 200000
00055
00056
00057
00058 #define P2OS_NOMINAL_VOLTAGE 12.0
00059
00060
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
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
00118
00119
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
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
00138 #define CMUCAM_IMAGE_WIDTH 80
00139 #define CMUCAM_IMAGE_HEIGHT 143
00140 #define CMUCAM_MESSAGE_LEN 10
00141
00142
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
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
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
00183 player_blobfinder_data_t blobfinder;
00184
00185
00186 player_position2d_data_t gyro;
00187
00188
00189 player_actarray_data_t actArray;
00190 player_gripper_data_t armGripper;
00191 } __attribute__ ((packed)) player_p2os_data_t;
00192
00193
00194 #include "sip.h"
00195
00196 #include "kinecalc.h"
00197
00198 class SIP;
00199
00200
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
00223
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
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
00259 bool sent_audio_cmd;
00260 player_audio_sample_item_t last_audio_cmd;
00261
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
00275 void ToggleSonarPower(unsigned char val);
00276
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
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
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);
00324 void SetActArrayJointSpeed (int joint, double 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
00331 KineCalc *kineCalc;
00332 float armOffsetX, armOffsetY, armOffsetZ;
00333
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;
00343 int direct_wheel_vel_control;
00344 int psos_fd;
00345 const char* psos_serial_port;
00346 bool psos_use_tcp;
00347 const char* psos_tcp_host;
00348 int psos_tcp_port;
00349
00350 struct timeval lastblob_tv;
00351
00352
00353 int motor_max_speed;
00354 int motor_max_turnspeed;
00355
00356
00357 bool use_vel_band;
00358
00359
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;
00364 int joystickp;
00365 int bumpstall;
00366 bool ignore_checksum;
00367
00368
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;
00394 double lastPulseTime;
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
00406 virtual void Main();
00407
00408 virtual int MainSetup();
00409 virtual void MainQuit();
00410
00411
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