39 #include <libplayercore/playercore.h>
40 #include <replace/replace.h>
43 #define MOTOR_DEF_MAX_SPEED 0.3
44 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
45 #define ACCELERATION_DEFAULT 100
46 #define DECELERATION_DEFAULT 250
53 #define GETCMDPOS 0x1D
54 #define GETCMDVEL 0x1E
55 #define GETEVENTSTATUS 0x31
56 #define RESETEVENTSTATUS 0x34
57 #define GETACTUALPOS 0x37
59 #define SETACTUALPOS 0x4D
60 #define GETSAMPLETIME 0x61
61 #define SETPHASECOUNTS 0x75
62 #define SETMOTORCMD 0x77
63 #define SETLIMITSWITCHMODE 0x80
64 #define WRITEDIGITAL 0x82
65 #define READDIGITAL 0x83
66 #define GETVERSION 0x8F
69 #define SETPROFILEMODE 0xA0
70 #define GETACTUALVEL 0xAD
71 #define SETSTOPMODE 0xD0
72 #define SETMOTORMODE 0xDC
73 #define SETOUTPUTMODE 0xE0
74 #define READANALOG 0xEF
76 #define MOTOR_0 ((unsigned char)0x00)
77 #define MOTOR_1 ((unsigned char)0x01)
80 #define LEFT_MOTOR MOTOR_1
81 #define RIGHT_MOTOR MOTOR_0
84 #define DEFAULT_M3_PORT "/dev/ttyUSB0"
86 #define DELAY_US 10000
89 TrapezoidalProfile = 0,
90 VelocityContouringProfile,
101 #define DEFAULT_MOTOR_0_DIR -1
102 #define DEFAULT_MOTOR_1_DIR 1
103 #define DEFAULT_AXLE_LENGTH (0.301)
105 #define MAX_TICKS 48000
106 #define WHEEL_DIAMETER (0.125)
107 #define WHEEL_RADIUS (WHEEL_DIAMETER/2.0)
108 #define WHEEL_CIRC (M_PI*WHEEL_DIAMETER)
109 #define MOTOR_STEP (1.8/MOTOR_TICKS_PER_STEP)
110 #define GEAR_RATIO (4.8)
111 #define WHEEL_STEP (MOTOR_STEP/GEAR_RATIO)
112 #define M_PER_TICK (WHEEL_RADIUS/WHEEL_STEP)
113 #define MOTOR_TICKS_PER_STEP (64.0)
114 #define MOTOR_TICKS_PER_REV (200.0*MOTOR_TICKS_PER_STEP)
115 #define NUM_IR_SENSORS 8
118 #define MAX_WHEELSPEED 8000
119 #define MPS_PER_TICK 1 // TODO: what is this?
124 #define DEFAULT_PERCENT_TORQUE 75
150 virtual int MainSetup();
151 virtual void MainQuit();
160 bool RecvBytes(
unsigned char*s,
int len );
161 int ReadBuf(
unsigned char* s,
size_t len);
162 int WriteBuf(
unsigned char* s,
size_t len);
163 int sendCmdCom(
unsigned char address,
unsigned char c,
164 int cmd_num,
unsigned char* arg,
165 int ret_num,
unsigned char * ret );
166 int sendCmd0(
unsigned char address,
unsigned char c,
167 int ret_num,
unsigned char * ret );
168 int sendCmd16(
unsigned char address,
unsigned char c,
169 int16_t arg,
int ret_num,
unsigned char * ret );
170 int sendCmd32(
unsigned char address,
unsigned char c,
171 int32_t arg,
int ret_num,
unsigned char * ret );
173 int32_t BytesToInt32(
unsigned char *ptr );
174 int16_t BytesToInt16(
unsigned char *ptr );
176 int ResetRawPositions();
188 void GetAllData(
void );
194 void PublishData(
void);
197 const char* GetPMDErrorString(
int rc );
200 void Stop(
int StopMode );
202 bool EnableMotors(
bool enable );
204 void SetVelocity( uint8_t chan,
float mps );
205 void SetVelocity(
float mpsL,
float mpsR );
206 void SetVelocityInTicks( int32_t left, int32_t right );
207 void GetVelocityInTicks( int32_t* left, int32_t* right );
209 void Move( uint8_t chan,
float meters );
210 void Move(
float metersL,
float metersR );
212 void SetPosition( uint8_t chan,
float meters );
213 void SetPosition(
float metersL,
float metersR );
214 void SetActualPositionInTicks( int32_t left, int32_t right );
215 void SetActualPosition(
float left,
float right );
216 void GetPositionInTicks( int32_t* left, int32_t* right );
218 void SetAccelerationProfile();
220 int GetAnalogSensor(
int s,
short * val );
221 void GetDigitalIn(
unsigned short* digIn );
222 void SetDigitalOut(
unsigned short digOut );
224 void SetContourMode( ProfileMode_t prof );
225 void SetMicrosteps();
229 int32_t Meters2Ticks(
float meters );
230 float Ticks2Meters( int32_t ticks );
231 int32_t MPS2Vel(
float mps );
232 float Vel2MPS( int32_t vel );
237 struct termios _old_tio;
242 const char* _serial_port;
253 int position_subscriptions;
254 int ir_subscriptions;
255 int aio_subscriptions;
256 int dio_subscriptions;
259 int direct_wheel_vel_control;
265 int motor_max_turnspeed;
268 short motor_max_trans_accel, motor_max_trans_decel;
269 short motor_max_rot_accel, motor_max_rot_decel;
293 uint16_t _lastDigOut;