Update of /cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv9485/server/drivers/mixed/wbr/914
Modified Files:
Tag: release-2-0-patches
wbr914.cc wbr914.h
Log Message:
merged many changes from HEAD
Index: wbr914.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914/wbr914.cc,v
retrieving revision 1.1.2.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** wbr914.cc 5 Jul 2006 22:42:22 -0000 1.1.2.3
--- wbr914.cc 22 Sep 2006 23:58:35 -0000 1.1.2.4
***************
*** 30,38 ****
/** @ingroup drivers */
/** @{ */
! /** @defgroup driver_wbr914 wbr914 TODO
* @brief White Box Robotics Model 914 robot
The White Box Robotics Model 914 computer communicates with the M3 I/O and
! motor control board over a serial-to-USB driver.
@par Compile-time dependencies
--- 30,40 ----
/** @ingroup drivers */
/** @{ */
! /** @defgroup driver_wbr914 wbr914
* @brief White Box Robotics Model 914 robot
The White Box Robotics Model 914 computer communicates with the M3 I/O and
! motor control board over a serial-to-USB driver. The serial commands are
! used to communicate with two PMD motion control chips that drive the
! stepper motors and control the onboard I/O.
@par Compile-time dependencies
***************
*** 47,54 ****
--- 49,64 ----
- @ref interface_position2d
- This interface returns position data, and accepts velocity commands.
+ - PLAYER_POSITION2D_CMD_VEL
- @ref interface_ir
- This interface returns the IR range data.
+ - @ref interface_aio
+ - This interface returns the analog input data from the optional 2nd I/O
board.
+
+ - @ref interface_dio
+ - This interface returns the digital input information and allows control
of the digital outputs on all installed White Box Robotics I/O boards. The
first I/O board supplies 8 digital inputs and outputs and the optional second
I/O board supplies an additional 8 digital inputs and outputs.
+
+
@par Supported configuration requests
***************
*** 62,65 ****
--- 72,84 ----
- PLAYER_IR_POSE
+ @par Supported commands
+
+ - @ref interface_position2d :
+ - PLAYER_POSITION2D_CMD_VEL
+
+ - @ref interface_dio :
+ - PLAYER_DIO_CMD_VALUES
+
+
@par Configuration file options
***************
*** 73,77 ****
(
name "wbr914"
! provides [ "position2d:0" "ir:0" ]
port "/dev/ttyUSB0"
)
--- 92,96 ----
(
name "wbr914"
! provides [ "position2d:0" "ir:0" "aio:0" "dio:0" ]
port "/dev/ttyUSB0"
)
***************
*** 131,138 ****
wbr914::wbr914(ConfigFile* cf, int section)
! : Driver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN)
{
- _stopped = true;
- _motorsEnabled = false;
last_lpos = 0;
last_rpos = 0;
--- 150,156 ----
wbr914::wbr914(ConfigFile* cf, int section)
! : Driver(cf,section,true,PLAYER_MSGQUEUE_DEFAULT_MAXLEN),
! _stopped( true ), _motorsEnabled( false ), _lastDigOut( 0 )
{
last_lpos = 0;
last_rpos = 0;
***************
*** 172,175 ****
--- 190,215 ----
}
+ // Do we create an Analog I/O interface?
+ if(cf->ReadDeviceAddr(&(this->aio_id), section, "provides",
+ PLAYER_AIO_CODE, -1, NULL) == 0)
+ {
+ if(this->AddInterface(this->aio_id) != 0)
+ {
+ this->SetError(-1);
+ return;
+ }
+ }
+
+ // Do we create a Digital I/O interface?
+ if(cf->ReadDeviceAddr(&(this->dio_id), section, "provides",
+ PLAYER_DIO_CODE, -1, NULL) == 0)
+ {
+ if(this->AddInterface(this->dio_id) != 0)
+ {
+ this->SetError(-1);
+ return;
+ }
+ }
+
// Read config file options
***************
*** 241,244 ****
--- 281,286 ----
// These 3 sensor have a z value of 0.35m and a pitch of 30 degrees down
+ // This type of pose is not currently handled by Player so do the best we
+ // can with what we have.
_ir_geom.poses[ 5 ].px = 0.200;
_ir_geom.poses[ 5 ].py = -0.060;
***************
*** 408,414 ****
}
- /* TODO check return value to match 0x00A934100013 */
if ( _debug )
printf( "GetVersion\n" );
if(sendCmd0( RIGHT_MOTOR, GETVERSION, 6, buf ) < 0)
{
--- 450,456 ----
}
if ( _debug )
printf( "GetVersion\n" );
+
if(sendCmd0( RIGHT_MOTOR, GETVERSION, 6, buf ) < 0)
{
***************
*** 416,422 ****
return -1;
}
- /* TODO check return value to match 0x00A934100013 */
-
-
_stopped = true;
--- 458,461 ----
***************
*** 464,467 ****
--- 503,514 ----
this->ir_subscriptions++;
}
+ else if(Device::MatchDeviceAddress(id, this->aio_id))
+ {
+ this->aio_subscriptions++;
+ }
+ else if(Device::MatchDeviceAddress(id, this->dio_id))
+ {
+ this->dio_subscriptions++;
+ }
}
***************
*** 474,480 ****
// do the unsubscription
if( shutdownResult == 0 )
{
- // also decrement the appropriate subscription counter
if(Device::MatchDeviceAddress(id, this->position_id))
{
--- 521,527 ----
// do the unsubscription
+ // and decrement the appropriate subscription counter
if( shutdownResult == 0 )
{
if(Device::MatchDeviceAddress(id, this->position_id))
{
***************
*** 482,486 ****
assert(this->position_subscriptions >= 0);
}
- // also decrement the appropriate subscription counter
else if(Device::MatchDeviceAddress(id, this->ir_id))
{
--- 529,532 ----
***************
*** 488,491 ****
--- 534,547 ----
assert(this->ir_subscriptions >= 0);
}
+ else if(Device::MatchDeviceAddress(id, this->aio_id))
+ {
+ this->aio_subscriptions--;
+ assert(this->aio_subscriptions >= 0);
+ }
+ else if(Device::MatchDeviceAddress(id, this->dio_id))
+ {
+ this->dio_subscriptions--;
+ assert(this->dio_subscriptions >= 0);
+ }
}
***************
*** 518,521 ****
--- 574,599 ----
NULL);
}
+
+ if ( aio_subscriptions )
+ {
+ // put Analog Input data
+ this->Publish(this->aio_id, NULL,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_AIO_DATA_STATE,
+ (void*)&(_data.aio),
+ sizeof(_data.aio),
+ NULL);
+ }
+
+ if ( dio_subscriptions )
+ {
+ // put Digital Input data
+ this->Publish(this->dio_id, NULL,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_DIO_DATA_VALUES,
+ (void*)&(_data.dio),
+ sizeof(_data.dio),
+ NULL);
+ }
}
***************
*** 699,702 ****
--- 777,790 ----
return(0);
}
+ else if ( Message::MatchMessage(hdr,
+ PLAYER_MSGTYPE_CMD,
+ PLAYER_DIO_CMD_VALUES,
+ this->dio_id))
+ {
+ HandleDigitalOutCommand( (player_dio_cmd_t*)data );
+ return(0);
+ }
+
+
return(-1);
}
***************
*** 715,719 ****
--- 803,809 ----
// now we set the speed
if ( this->_motorsEnabled )
+ {
SetVelocityInTicks( leftvel, rightvel );
+ }
else
{
***************
*** 723,726 ****
--- 813,821 ----
}
+ void wbr914::HandleDigitalOutCommand( player_dio_cmd_t* doutCmd )
+ {
+ SetDigitalData( doutCmd );
+ }
+
void wbr914::GetAllData( void )
{
***************
*** 735,738 ****
--- 830,843 ----
GetIRData( &_data.ir );
}
+
+ if ( aio_subscriptions )
+ {
+ GetAnalogData( &_data.aio );
+ }
+
+ if ( dio_subscriptions )
+ {
+ GetDigitalData( &_data.dio );
+ }
}
***************
*** 826,835 ****
// Assume ADC input of 5.0V gives max value of 1023
- // float v80 = 0.25;
- // float deltaV = 2.25;
- // float v10 = v80+deltaV;
float adcLo = 0.0;
float adcHi = 5.0;
float vPerCount = (adcHi-adcLo)/1023.0;
// float mmPerVolt = (800.0-100.0)/(v80-v10);
--- 931,940 ----
// Assume ADC input of 5.0V gives max value of 1023
float adcLo = 0.0;
float adcHi = 5.0;
float vPerCount = (adcHi-adcLo)/1023.0;
+ // float v80 = 0.25;
+ // float deltaV = 2.25;
+ // float v10 = v80+deltaV;
// float mmPerVolt = (800.0-100.0)/(v80-v10);
***************
*** 872,875 ****
--- 977,1062 ----
}
+ /*
+ Update the Analog input part of the client data
+
+ We cannot reliably detect whether there is an I/O
+ board attached to the M3 so blindly return the data.
+ */
+ void wbr914::GetAnalogData(player_aio_data_t * d)
+ {
+ // Read the 8 analog inputs on the second I/O board
+ d->voltages_count = 8;
+
+ float adcLo = 0.0;
+ float adcHi = 5.0;
+ float vPerCount = (adcHi-adcLo)/1023.0;
+
+ for (uint32_t i=0; i < d->voltages_count; i++)
+ {
+ int16_t val = 0;
+
+ GetAnalogSensor( i, &val );
+ float voltage = (float)val*vPerCount;
+ d->voltages[ i ] = voltage;
+ }
+ }
+
+ /*
+ Update the Digital input part of the client data
+
+ We cannot reliably detect whether there is an I/O
+ board attached to the M3 so blindly return the data.
+ */
+ void wbr914::GetDigitalData(player_dio_data_t * d)
+ {
+ // Read the 16 digital inputs
+ uint16_t din;
+
+ d->count = 16;
+ GetDigitalIn( &din );
+
+ // Byte flip the data to make the Input from the
+ // optional I/O board show up in the upper byte.
+ d->digin = (uint32_t)( (din>>8) | (din<<8));
+ }
+
+ /*
+ Set the Digital outputs on the robot
+
+ We cannot reliably detect whether there is an I/O
+ board attached to the M3 so blindly set the data.
+ */
+ void wbr914::SetDigitalData( player_dio_cmd_t * d )
+ {
+ // We only have 16 bits of Dig out, so strip extra bits
+ uint16_t data = d->digout & 0xFFFF;
+
+ // Different number of digital bits being requested to
+ // be set than we must actually set in the hardware.
+ // Handle by using part of the last sent data.
+ if ( d->count < 16 )
+ {
+ // Keep the last dig out bits that have not changed
+ uint16_t mask = (0xffff << d->count);
+ uint16_t oldPart = _lastDigOut & mask;
+
+ // Invert the mask and keep the bits that are to change.
+ mask = mask ^ 0xFFFF;
+ data &= mask;
+
+ // Build the output data
+ data |= oldPart;
+ }
+
+ _lastDigOut = data;
+
+ // Byte flip the data to make the Output to from the
+ // optional I/O board show up in the upper byte.
+ data = ( (data>>8) | (data<<8) );
+
+ // Always set 16 bits of data
+ SetDigitalOut( data );
+ }
+
//-------------------------------------------------------
***************
*** 1189,1192 ****
--- 1376,1395 ----
+ void wbr914::GetDigitalIn( uint16_t* d )
+ {
+ unsigned char ret[6];
+
+ sendCmd16( 0, READDIGITAL, 0, 6, ret );
+
+ *d = (uint16_t)BytesToInt16( &(ret[2]) );
+ }
+
+ void wbr914::SetDigitalOut( uint16_t d )
+ {
+ unsigned char ret[2];
+
+ sendCmd32( 0, WRITEDIGITAL, d, 2, ret );
+ }
+
/*
Robot commands
***************
*** 1304,1311 ****
int32_t accel = (int32_t)MOTOR_TICKS_PER_STEP*2;
! sendCmd32( LEFT_MOTOR, SETACCEL, accel, 2, ret );
! sendCmd32( RIGHT_MOTOR, SETACCEL, accel, 2, ret );
! sendCmd32( LEFT_MOTOR, SETDECEL, accel, 2, ret );
! sendCmd32( RIGHT_MOTOR, SETDECEL, accel, 2, ret );
SetContourMode( TrapezoidalProfile );
}
--- 1507,1515 ----
int32_t accel = (int32_t)MOTOR_TICKS_PER_STEP*2;
! // Decelerate faster than accelerating.
! sendCmd32( LEFT_MOTOR, SETACCEL, accel, ACCELERATION_DEFAULT, ret );
! sendCmd32( RIGHT_MOTOR, SETACCEL, accel, ACCELERATION_DEFAULT, ret );
! sendCmd32( LEFT_MOTOR, SETDECEL, accel, DECELERATION_DEFAULT, ret );
! sendCmd32( RIGHT_MOTOR, SETDECEL, accel, DECELERATION_DEFAULT, ret );
SetContourMode( TrapezoidalProfile );
}
***************
*** 1326,1337 ****
sendCmd16( LEFT_MOTOR, SETSTOPMODE, AbruptStopMode, 2, ret );
sendCmd32( LEFT_MOTOR, SETVEL, 0, 2, ret );
! sendCmd32( LEFT_MOTOR, SETACCEL, 0, 2, ret );
! sendCmd32( LEFT_MOTOR, SETDECEL, 0, 2, ret );
sendCmd16( RIGHT_MOTOR, RESETEVENTSTATUS, 0x0000, 2, ret );
sendCmd16( RIGHT_MOTOR, SETSTOPMODE, AbruptStopMode, 2, ret );
sendCmd32( RIGHT_MOTOR, SETVEL, 0, 2, ret );
! sendCmd32( RIGHT_MOTOR, SETACCEL, 0, 2, ret );
! sendCmd32( RIGHT_MOTOR, SETDECEL, 0, 2, ret );
SetContourMode( VelocityContouringProfile );
--- 1530,1541 ----
sendCmd16( LEFT_MOTOR, SETSTOPMODE, AbruptStopMode, 2, ret );
sendCmd32( LEFT_MOTOR, SETVEL, 0, 2, ret );
! sendCmd32( LEFT_MOTOR, SETACCEL, 0, ACCELERATION_DEFAULT, ret );
! sendCmd32( LEFT_MOTOR, SETDECEL, 0, DECELERATION_DEFAULT, ret );
sendCmd16( RIGHT_MOTOR, RESETEVENTSTATUS, 0x0000, 2, ret );
sendCmd16( RIGHT_MOTOR, SETSTOPMODE, AbruptStopMode, 2, ret );
sendCmd32( RIGHT_MOTOR, SETVEL, 0, 2, ret );
! sendCmd32( RIGHT_MOTOR, SETACCEL, 0, ACCELERATION_DEFAULT, ret );
! sendCmd32( RIGHT_MOTOR, SETDECEL, 0, DECELERATION_DEFAULT, ret );
SetContourMode( VelocityContouringProfile );
Index: wbr914.h
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914/wbr914.h,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -d -r1.1.2.2 -r1.1.2.3
*** wbr914.h 7 Jun 2006 16:12:52 -0000 1.1.2.2
--- wbr914.h 22 Sep 2006 23:58:35 -0000 1.1.2.3
***************
*** 42,45 ****
--- 42,47 ----
#define MOTOR_DEF_MAX_SPEED 0.5
#define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
+ #define ACCELERATION_DEFAULT 2
+ #define DECELERATION_DEFAULT 10
/* PMD3410 command codes */
***************
*** 58,61 ****
--- 60,65 ----
#define SETMOTORCMD 0x77
#define SETLIMITSWITCHMODE 0x80
+ #define WRITEDIGITAL 0x82
+ #define READDIGITAL 0x83
#define GETVERSION 0x8F
#define SETACCEL 0x90
***************
*** 124,127 ****
--- 128,133 ----
player_position2d_data_t position;
player_ir_data_t ir;
+ player_aio_data_t aio;
+ player_dio_data_t dio;
} __attribute__ ((packed)) player_data_t;
***************
*** 169,182 ****
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 );
--- 175,194 ----
player_msghdr * hdr,
void* data);
+
+ // Command handlers
int HandleCommand(player_msghdr * hdr, void * data);
+ void HandleVelocityCommand(player_position2d_cmd_vel_t* cmd );
+ void HandleDigitalOutCommand( player_dio_cmd_t* doutCmd );
+ void SetDigitalData( player_dio_cmd_t * d );
+ // Robot data retrievers
void GetAllData( void );
void GetPositionData( player_position2d_data_t* d );
void GetIRData( player_ir_data_t * d );
+ void GetAnalogData( player_aio_data_t * d );
+ void GetDigitalData( player_dio_data_t * d );
void PublishData(void);
/* Robot commands */
const char* GetPMDErrorString( int rc );
***************
*** 204,207 ****
--- 216,221 ----
void StopRobot();
int GetAnalogSensor(int s, short * val );
+ void GetDigitalIn( unsigned short* digIn );
+ void SetDigitalOut( unsigned short digOut );
void SetOdometry( player_position2d_set_odom_req_t* od );
void SetContourMode( ProfileMode_t prof );
***************
*** 226,234 ****
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
--- 240,251 ----
player_devaddr_t position_id;
player_devaddr_t ir_id;
+ player_devaddr_t aio_id;
+ player_devaddr_t dio_id;
! // bookkeeping to track whether an interface has been subscribed
int position_subscriptions;
int ir_subscriptions;
+ int aio_subscriptions;
+ int dio_subscriptions;
int param_idx; // index in the RobotParams table for this robot
***************
*** 266,269 ****
--- 283,288 ----
double _positionK;
int _percentTorque;
+
+ uint16_t _lastDigOut;
};
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys -- and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
