38 #include <libplayercore/playercore.h>
39 #include <replace/replace.h>
42 #include "robot_params.h"
46 #define MOTOR_DEF_MAX_SPEED 0.5
47 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
54 #define P2OS_CYCLETIME_USEC 200000
58 #define P2OS_NOMINAL_VOLTAGE 12.0
80 #define TTY2 42 // Added in AmigOS 1.2
81 #define GETAUX 43 // Added in AmigOS 1.2
84 #define GYRO 58 // Added in AROS 1.8
85 #define ROTKP 82 // Added in P2OS1.M
86 #define ROTKV 83 // Added in P2OS1.M
87 #define ROTKI 84 // Added in P2OS1.M
88 #define TRANSKP 85 // Added in P2OS1.M
89 #define TRANSKV 86 // Added in P2OS1.M
90 #define TRANSKI 87 // Added in P2OS1.M
91 #define TTY3 66 // Added in AmigOS 1.3
92 #define GETAUX2 67 // Added in AmigOS 1.3
103 #define ARM_AUTOPARK 80
104 #define ARM_GRIPPARK 81
109 #define STATUSSTOPPED 0x32
110 #define STATUSMOVING 0x33
113 #define SERAUX2 0xB8 // Added in AmigOS 1.3
114 #define GYROPAC 0x98 // Added AROS 1.8
115 #define ARMPAC 160 // ARMpac
116 #define ARMINFOPAC 161 // ARMINFOpac
120 #define ARGINT 0x3B // Positive int (LSB, MSB)
121 #define ARGNINT 0x1B // Negative int (LSB, MSB)
122 #define ARGSTR 0x2B // String (Note: 1st byte is length!!)
138 #define CMUCAM_IMAGE_WIDTH 80
139 #define CMUCAM_IMAGE_HEIGHT 143
140 #define CMUCAM_MESSAGE_LEN 10
143 #define DEFAULT_P2OS_PORT "/dev/ttyS0"
144 #define DEFAULT_P2OS_TCP_REMOTE_HOST "localhost"
145 #define DEFAULT_P2OS_TCP_REMOTE_PORT 8101
148 #define CAM_ERROR_NONE 0x30
149 #define CAM_ERROR_BUSY 0x31
150 #define CAM_ERROR_PARAM 0x35
151 #define CAM_ERROR_MODE 0x39
153 #define PTZ_SLEEP_TIME_USEC 100000
155 #define MAX_PTZ_COMMAND_LENGTH 19
156 #define MAX_PTZ_REQUEST_LENGTH 17
157 #define COMMAND_RESPONSE_BYTES 6
159 #define PTZ_PAN_MAX 98.0 // 875 units 0x36B
160 #define PTZ_TILT_MAX 88.0 // 790 units 0x316
161 #define PTZ_TILT_MIN -30.0 // -267 units 0x10B
162 #define MAX_ZOOM 1960 //1900
163 #define ZOOM_CONV_FACTOR 17
165 #define PT_BUFFER_INC 512
166 #define PT_READ_TIMEOUT 10000
167 #define PT_READ_TRIALS 2
196 #include "kinecalc.h"
205 void putOnBuf(
unsigned char c);
228 player_p2os_data_t p2os_data;
249 uint8_t lastGripperCmd;
252 bool sentArmGripperCmd;
253 uint8_t lastArmGripperCmd;
254 uint8_t lastActArrayCmd;
262 int rot_kp, rot_kv, rot_ki, trans_kp, trans_kv, trans_ki;
265 int position_subscriptions;
266 int sonar_subscriptions;
267 int actarray_subscriptions;
268 int ptz_subscriptions;
272 int SendReceive(
P2OSPacket* pkt,
bool publish_data=
true);
273 void ResetRawPositions();
275 void ToggleSonarPower(
unsigned char val);
277 void ToggleMotorPower(
unsigned char val);
282 void StandardSIPPutData(
double timestampStandardSIP);
283 void GyroPutData(
double timestampGyro);
284 void BlobfinderPutData(
double timestampSERAUX);
285 void ActarrayPutData(
double timestampArm);
289 int HandleArmGripperCommand (
player_msghdr *hdr,
void *data);
293 void get_ptz_packet(
int s1,
int s2=0);
303 void OpenGripper (
void);
304 void CloseGripper (
void);
305 void StopGripper (
void);
306 void OpenArmGripper (
void);
307 void CloseArmGripper (
void);
308 void StopArmGripper (
void);
313 double aaOrients[18];
317 inline double TicksToDegrees (
int joint,
unsigned char ticks);
318 inline unsigned char DegreesToTicks (
int joint,
double degrees);
319 inline double TicksToRadians (
int joint,
unsigned char ticks);
320 inline unsigned char RadiansToTicks (
int joint,
double rads);
321 inline double RadsPerSectoSecsPerTick (
int joint,
double speed);
322 inline double SecsPerTicktoRadsPerSec (
int joint,
double secs);
323 void ToggleActArrayPower (
unsigned char val,
bool lock =
true);
324 void SetActArrayJointSpeed (
int joint,
double speed);
327 int HandleActArrayCommand (
player_msghdr * hdr,
void * data);
332 float armOffsetX, armOffsetY, armOffsetZ;
335 void HandleLimbHomeCmd (
void);
336 void HandleLimbStopCmd (
void);
343 int direct_wheel_vel_control;
345 const char* psos_serial_port;
347 const char* psos_tcp_host;
350 struct timeval lastblob_tv;
354 int motor_max_turnspeed;
360 short motor_max_trans_accel, motor_max_trans_decel;
361 short motor_max_rot_accel, motor_max_rot_decel;
366 bool ignore_checksum;
372 int pandemand, tiltdemand, zoomdemand;
373 int SendCommand(
unsigned char *str,
int len);
374 int SendRequest(
unsigned char* str,
int len,
unsigned char* reply, uint8_t camera = 1);
375 void PrintPacket(
char* str,
unsigned char* cmd,
int len);
376 int SendAbsPanTilt(
int pan,
int tilt);
377 int setDefaultTiltRange();
378 int GetAbsPanTilt(
int* pan,
int* tilt);
379 int GetAbsZoom(
int* zoom);
380 int SendAbsZoom(
int zoom);
381 int read_ptz(
unsigned char *reply,
int size);
382 int ReceiveCommandAnswer(
int asize);
383 int ReceiveRequestAnswer(
unsigned char *data,
int s1,
int s2);
384 int setControlMode();
385 int setNotifyCommand();
386 int setPower(
int on);
387 int setOnScreenOff();
388 int CheckHostControlMode();
390 int GetMaxZoom(
int * maxzoom);
394 double lastPulseTime;
395 void SendPulse (
void);
416 void CMUcamReset(
bool doLock =
true);
417 void CMUcamTrack(
int rmin=0,
int rmax=0,
int gmin=0,
418 int gmax=0,
int bmin=0,
int bmax=0);
419 void CMUcamStartTracking(
bool doLock =
true);
420 void CMUcamStopTracking(
bool doLock =
true);