Update of /cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4552/server/drivers/mixed/wbr/914
Added Files:
Tag: release-2-0-patches
.cvsignore Makefile.am wbr914.cc wbr914.h
Log Message:
backported lots of stuff from HEAD
--- NEW FILE: wbr914.cc ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
[...1377 lines suppressed...]
//this could just be ntohl
return data;
}
int16_t wbr914::BytesToInt16(unsigned char *ptr)
{
unsigned char char0,char1;
int16_t data = 0;
char0 = ptr[1];
char1 = ptr[0];
data |= ((int)char0) & 0x000000FF;
data |= (((int)char1) << 8) & 0x0000FF00;
//this could just be ntohl
return data;
}
--- NEW FILE: .cvsignore ---
Makefile
Makefile.in
--- NEW FILE: Makefile.am ---
noinst_LTLIBRARIES =
if INCLUDE_WBR914
noinst_LTLIBRARIES += libwbr914.la
endif
AM_CPPFLAGS = -g -Wall -I$(top_srcdir)
libwbr914_la_SOURCES = wbr914.cc wbr914.h
--- NEW FILE: wbr914.h ---
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000
* Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/*
* $Id: wbr914.h,v 1.1.2.2 2006/06/07 16:12:52 gerkey Exp $
*
* the P2OS device. it's the parent device for all the P2 'sub-devices',
* like gripper, position, sonar, etc. there's a thread here that
* actually interacts with P2OS via the serial line. the other
* "devices" communicate with this thread by putting into and getting
* data out of shared buffers.
*/
#ifndef _WBR914_H
#define _WBR914_H
#include <pthread.h>
#include <sys/time.h>
#include <libplayercore/playercore.h>
#include <replace/replace.h>
// Default max speeds
#define MOTOR_DEF_MAX_SPEED 0.5
#define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
/* PMD3410 command codes */
#define NOOP 0x00
#define SETPOS 0x10
#define SETVEL 0x11
#define UPDATE 0x1A
#define GETCMDPOS 0x1D
#define GETEVENTSTATUS 0x31
#define RESETEVENTSTATUS 0x34
#define GETACTUALPOS 0x37
#define RESET 0x39
#define SETACTUALPOS 0x4D
#define GETSAMPLETIME 0x61
#define SETPHASECOUNTS 0x75
#define SETMOTORCMD 0x77
#define SETLIMITSWITCHMODE 0x80
#define GETVERSION 0x8F
#define SETACCEL 0x90
#define SETDECEL 0x91
#define SETPROFILEMODE 0xA0
#define GETACTUALVEL 0xAD
#define SETSTOPMODE 0xD0
#define SETMOTORMODE 0xDC
#define SETOUTPUTMODE 0xE0
#define READANALOG 0xEF
#define MOTOR_0 ((unsigned char)0x00)
#define MOTOR_1 ((unsigned char)0x01)
/* Robot configuration */
#define LEFT_MOTOR MOTOR_1
#define RIGHT_MOTOR MOTOR_0
/* Connection stuff */
#define DEFAULT_M3_PORT "/dev/ttyUSB0"
#define DELAY_US 10000
typedef enum {
TrapezoidalProfile = 0,
VelocityContouringProfile,
SCurveProfile,
} ProfileMode_t;
typedef enum {
NoStopMode = 0,
AbruptStopMode,
SmoothStopMode
} StopMode;
/* robot-specific info */
#define DEFAULT_MOTOR_0_DIR -1
#define DEFAULT_MOTOR_1_DIR 1
#define DEFAULT_AXLE_LENGTH .301
#define MAX_TICKS 48000
#define WHEEL_DIAMETER 0.125
#define WHEEL_RADIUS (WHEEL_DIAMETER/2.0)
#define WHEEL_CIRC (M_PI*WHEEL_DIAMETER)
#define MOTOR_STEP (1.8/MOTOR_TICKS_PER_STEP)
#define GEAR_RATIO 4.8
#define WHEEL_STEP (MOTOR_STEP/GEAR_RATIO)
#define M_PER_TICK (WHEEL_RADIUS/WHEEL_STEP)
#define MOTOR_TICKS_PER_STEP 64.0
#define MOTOR_TICKS_PER_REV 200.0*MOTOR_TICKS_PER_STEP
#define NUM_IR_SENSORS 8
/* for safety */
#define MAX_WHEELSPEED 8000
#define MPS_PER_TICK 1 // TODO: what is this?
#define FULL_STOP 0
#define STOP 1
#define DEFAULT_PERCENT_TORQUE 75
typedef struct
{
player_position2d_data_t position;
player_ir_data_t ir;
} __attribute__ ((packed)) player_data_t;
class wbr914 : public Driver
{
public:
wbr914(ConfigFile* cf, int section);
virtual ~wbr914();
virtual int Subscribe(player_devaddr_t id);
virtual int Unsubscribe(player_devaddr_t id);
/* the main thread */
virtual void Main();
virtual int Setup();
virtual int Shutdown();
// MessageHandler
virtual int ProcessMessage(MessageQueue * resp_queue,
player_msghdr * hdr,
void * data);
// Private Member Functions
private:
int ReadBuf(unsigned char* s, size_t len);
int WriteBuf(unsigned char* s, size_t len);
int sendCmdCom( unsigned char address, unsigned char c,
int cmd_num, unsigned char* arg,
int ret_num, unsigned char * ret );
int sendCmd0( unsigned char address, unsigned char c,
int ret_num, unsigned char * ret );
int sendCmd16( unsigned char address, unsigned char c,
int16_t arg, int ret_num, unsigned char * ret );
int sendCmd32( unsigned char address, unsigned char c,
int32_t arg, int ret_num, unsigned char * ret );
int32_t BytesToInt32( unsigned char *ptr );
int16_t BytesToInt16( unsigned char *ptr );
int ResetRawPositions();
int HandleConfig(MessageQueue* resp_queue,
player_msghdr * hdr,
void* data);
int HandleCommand(player_msghdr * hdr, void * data);
void GetAllData( void );
void GetPositionData( player_position2d_data_t* d );
void GetIRData( player_ir_data_t * d );
void PublishData(void);
void HandleVelocityCommand(player_position2d_cmd_vel_t* cmd );
/* Robot commands */
const char* GetPMDErrorString( int rc );
int InitRobot();
void UpdateM3();
void Stop( int StopMode );
bool EnableMotors( bool enable );
void SetVelocity( uint8_t chan, float mps );
void SetVelocity( float mpsL, float mpsR );
void SetVelocityInTicks( int32_t left, int32_t right );
void GetVelocityInTicks( int32_t* left, int32_t* right );
void Move( uint8_t chan, float meters );
void Move( float metersL, float metersR );
void SetPosition( uint8_t chan, float meters );
void SetPosition( float metersL, float metersR );
void SetActualPositionInTicks( int32_t left, int32_t right );
void SetActualPosition( float left, float right );
void GetPositionInTicks( int32_t* left, int32_t* right );
void SetAccelerationProfile();
void StopRobot();
int GetAnalogSensor(int s, short * val );
void SetOdometry( player_position2d_set_odom_req_t* od );
void SetContourMode( ProfileMode_t prof );
void SetMicrosteps();
/* Conversions */
int32_t Meters2Ticks( float meters );
float Ticks2Meters( int32_t ticks );
int32_t MPS2Vel( float mps );
float Vel2MPS( int32_t vel );
// Private Data members
private:
// Comm info for connection to M3 controller
int _fd;
bool _fd_blocking;
const char* _serial_port; // name of serial port device
player_data_t _data;
player_devaddr_t position_id;
player_devaddr_t ir_id;
// bookkeeping to only send new sound I/O commands
int position_subscriptions;
int ir_subscriptions;
int param_idx; // index in the RobotParams table for this robot
int direct_wheel_vel_control; // false -> separate trans and rot vel
player_position2d_cmd_vel_t last_position_cmd;
// Max motor speeds (mm/sec,deg/sec)
int motor_max_speed;
int motor_max_turnspeed;
// Max motor accel/decel (mm/sec/sec, deg/sec/sec)
short motor_max_trans_accel, motor_max_trans_decel;
short motor_max_rot_accel, motor_max_rot_decel;
// Geometry
// Robot Geometry
player_position2d_geom_t _robot2d_geom;
player_position3d_geom_t _robot3d_geom;
player_ir_pose_t _ir_geom;
// Odometry stuff
int32_t last_lpos;
int32_t last_rpos;
double x;
double y;
double yaw;
// State
bool _stopped;
bool _motorsEnabled;
int _debug;
int _usCycleTime;
double _velocityK;
double _positionK;
int _percentTorque;
};
#endif
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
