p2os.h
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 #define MOTOR_DEF_MAX_SPEED 0.5
00046 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
00047
00048
00049
00050
00051
00052
00053 #define P2OS_CYCLETIME_USEC 200000
00054
00055
00056
00057 #define P2OS_NOMINAL_VOLTAGE 12.0
00058
00059
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
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
00117
00118
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
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
00137 #define CMUCAM_IMAGE_WIDTH 80
00138 #define CMUCAM_IMAGE_HEIGHT 143
00139 #define CMUCAM_MESSAGE_LEN 10
00140
00141
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
00162 #include <sip.h>
00163
00164 #include "kinecalc.h"
00165
00166 class SIP;
00167
00168
00169
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
00191 bool sent_gripper_cmd;
00192 player_gripper_cmd_t last_gripper_cmd;
00193
00194
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
00200 bool sent_sound_cmd;
00201 player_sound_cmd_t last_sound_cmd;
00202
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
00215 void ToggleSonarPower(unsigned char val);
00216
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
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);
00236 void SetActArrayJointSpeed (int joint, double speed);
00237 void HandleActArrayPosCmd (player_actarray_position_cmd_t cmd);
00238 void HandleActArrayHomeCmd (player_actarray_home_cmd_t cmd);
00239
00241
00242 KineCalc *kineCalc;
00243 float armOffsetX, armOffsetY, armOffsetZ;
00244
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;
00254 int direct_wheel_vel_control;
00255 int psos_fd;
00256 const char* psos_serial_port;
00257 bool psos_use_tcp;
00258 const char* psos_tcp_host;
00259 int psos_tcp_port;
00260
00261
00262 struct timeval lastblob_tv;
00263
00264
00265 int motor_max_speed;
00266 int motor_max_turnspeed;
00267
00268
00269 bool use_vel_band;
00270
00271
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;
00276 int joystickp;
00277 int bumpstall;
00278
00279 float pulse;
00280 double lastPulseTime;
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
00292 virtual void Main();
00293
00294 virtual int Setup();
00295 virtual int Shutdown();
00296
00297
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
|