Update of /cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv15704/server/drivers/mixed/wbr/914
Modified Files:
wbr914.cc
Log Message:
made -lltdl optional, applied wbr914 patch, enabled accel_calib driver
Index: wbr914.cc
===================================================================
RCS file:
/cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914/wbr914.cc,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** wbr914.cc 2 May 2006 20:59:41 -0000 1.1
--- wbr914.cc 5 Jul 2006 22:36:20 -0000 1.2
***************
*** 87,107 ****
#endif
! #include <fcntl.h>
! #include <signal.h>
! #include <sys/stat.h>
! #include <sys/types.h>
#include <stdio.h>
#include <string.h>
- #include <unistd.h>
#include <math.h>
#include <stdlib.h> /* for abs() */
#include <netinet/in.h>
#include <termios.h>
#include <sys/socket.h>
#include <netinet/tcp.h>
#include <netdb.h>
- //#include <inttypes.h>
- #include <stdint.h>
- //#include <assert.h>
#include "wbr914.h"
--- 87,108 ----
#endif
! #include <unistd.h>
! #include <inttypes.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h> /* for abs() */
+
+ #include <fcntl.h>
+ #include <linux/serial.h>
+ #include <signal.h>
+ #include <sys/stat.h>
+ #include <sys/types.h>
#include <netinet/in.h>
+ #include <termio.h>
#include <termios.h>
#include <sys/socket.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include "wbr914.h"
***************
*** 180,191 ****
// Constrain torque (power to motor phases) between 0 and 100.
// Smaller numbers mean less torque, but less power used and less
! // heat generated.
if ( _percentTorque > 100 )
{
_percentTorque = 100;
}
! else if ( _percentTorque < 0 )
{
! _percentTorque = 0;
}
--- 181,193 ----
// Constrain torque (power to motor phases) between 0 and 100.
// Smaller numbers mean less torque, but less power used and less
! // heat generated. Not much use reducing the torque setting below
! // 20%.
if ( _percentTorque > 100 )
{
_percentTorque = 100;
}
! else if ( _percentTorque < 20 )
{
! _percentTorque = 20;
}
***************
*** 200,206 ****
// Width in meters
! _robot2d_geom.size.sw = 0.3;
// Length in meters
! _robot2d_geom.size.sl = 0.3;
_robot3d_geom.pose.px = _robot2d_geom.pose.px;
--- 202,208 ----
// Width in meters
! _robot2d_geom.size.sw = 0.37;
// Length in meters
! _robot2d_geom.size.sl = 0.42;
_robot3d_geom.pose.px = _robot2d_geom.pose.px;
***************
*** 218,252 ****
_ir_geom.poses_count = NUM_IR_SENSORS;
! _ir_geom.poses[ 0 ].px = 0.100;
! _ir_geom.poses[ 0 ].py = 0.200;
! _ir_geom.poses[ 0 ].pa = DTOR( 90 );
! _ir_geom.poses[ 1 ].px = 0.180;
! _ir_geom.poses[ 1 ].py = 0.170;
! _ir_geom.poses[ 1 ].pa = DTOR( 60 );
! _ir_geom.poses[ 2 ].px = 0.230;
! _ir_geom.poses[ 2 ].py = 0.110;
! _ir_geom.poses[ 2 ].pa = DTOR( 30 );
! _ir_geom.poses[ 3 ].px = 0.250;
! _ir_geom.poses[ 3 ].py = 0.030;
! _ir_geom.poses[ 3 ].pa = DTOR( 0 );
! _ir_geom.poses[ 4 ].px = 0.250;
! _ir_geom.poses[ 4 ].py = -0.030;
! _ir_geom.poses[ 4 ].pa = DTOR( 0 );
! _ir_geom.poses[ 5 ].px = 0.230;
! _ir_geom.poses[ 5 ].py = -0.110;
! _ir_geom.poses[ 5 ].pa = DTOR( -30 );
! _ir_geom.poses[ 6 ].px = 0.180;
! _ir_geom.poses[ 6 ].py = -0.170;
! _ir_geom.poses[ 6 ].pa = DTOR( -60 );
! _ir_geom.poses[ 7 ].px = 0.100;
! _ir_geom.poses[ 7 ].py = -0.200;
! _ir_geom.poses[ 7 ].pa = DTOR( -90 );
}
--- 220,255 ----
_ir_geom.poses_count = NUM_IR_SENSORS;
! _ir_geom.poses[ 0 ].px = 0.030;
! _ir_geom.poses[ 0 ].py = -0.190;
! _ir_geom.poses[ 0 ].pa = DTOR( -90 );
! _ir_geom.poses[ 1 ].px = 0.190;
! _ir_geom.poses[ 1 ].py = -0.090;
! _ir_geom.poses[ 1 ].pa = DTOR( -30 );
! _ir_geom.poses[ 2 ].px = 0.210;
! _ir_geom.poses[ 2 ].py = 0.0;
! _ir_geom.poses[ 2 ].pa = DTOR( 0 );
! _ir_geom.poses[ 3 ].px = 0.190;
! _ir_geom.poses[ 3 ].py = 0.090;
! _ir_geom.poses[ 3 ].pa = DTOR( 30 );
! _ir_geom.poses[ 4 ].px = 0.030;
! _ir_geom.poses[ 4 ].py = 0.190;
! _ir_geom.poses[ 4 ].pa = DTOR( 90 );
! // These 3 sensor have a z value of 0.35m and a pitch of 30 degrees down
! _ir_geom.poses[ 5 ].px = 0.200;
! _ir_geom.poses[ 5 ].py = -0.060;
! _ir_geom.poses[ 5 ].pa = DTOR( -60 );
! _ir_geom.poses[ 6 ].px = 0.210;
! _ir_geom.poses[ 6 ].py = 0.0;
! _ir_geom.poses[ 6 ].pa = DTOR( 0 );
! _ir_geom.poses[ 7 ].px = 0.200;
! _ir_geom.poses[ 7 ].py = 0.060;
! _ir_geom.poses[ 7 ].pa = DTOR( 60 );
}
***************
*** 286,291 ****
cfmakeraw( &term );
! cfsetispeed( &term, B115200 );
! cfsetospeed( &term, B115200 );
// 2 stop bits
--- 289,294 ----
cfmakeraw( &term );
! cfsetispeed( &term, B38400 );
! cfsetospeed( &term, B38400 );
// 2 stop bits
***************
*** 301,306 ****
}
_fd_blocking = false;
! printf( "InitRobot\n" );
fflush(stdout);
if(InitRobot() < 0)
--- 304,329 ----
}
+ {
+ struct serial_struct serial_info;
+
+ // Custom baud rate of 416666 baud, the max the
+ // motor controller will handle.
+ // round off to get the closest divisor.
+ serial_info.flags = ASYNC_SPD_CUST | ASYNC_LOW_LATENCY;
+ serial_info.custom_divisor = (int)((float)24000000.0/(float)416666.0 +
0.5);
+ if ( _debug )
+ printf( "Custom divisor = %d\n", serial_info.custom_divisor );
+
+ if ( ioctl( _fd, TIOCSSERIAL, &serial_info ) < 0)
+ {
+ perror("config_serial_port: ioctl TIOCSSERIAL");
+ return(-1);
+ }
+ }
+
_fd_blocking = false;
!
! if ( _debug )
! printf( "InitRobot\n" );
fflush(stdout);
if(InitRobot() < 0)
***************
*** 350,363 ****
/* This might be a good time to reset the odometry values */
! printf( "ResetRawPositions\n" );
fflush( stdout );
ResetRawPositions();
! printf( "SetAccelerationProfile\n" );
SetAccelerationProfile();
UpdateM3();
/* now spawn reading thread */
! printf( "Starting Thread...\n" );
StartThread();
return(0);
--- 373,389 ----
/* This might be a good time to reset the odometry values */
! if ( _debug )
! printf( "ResetRawPositions\n" );
fflush( stdout );
ResetRawPositions();
! if ( _debug )
! printf( "SetAccelerationProfile\n" );
SetAccelerationProfile();
UpdateM3();
/* now spawn reading thread */
! if ( _debug )
! printf( "Starting Thread...\n" );
StartThread();
return(0);
***************
*** 374,378 ****
sendCmd0( RIGHT_MOTOR, RESET, 2, buf );
! printf( "GetVersion\n" );
if( sendCmd0( LEFT_MOTOR, GETVERSION, 6, buf ) < 0)
{
--- 400,405 ----
sendCmd0( RIGHT_MOTOR, RESET, 2, buf );
! if ( _debug )
! printf( "GetVersion\n" );
if( sendCmd0( LEFT_MOTOR, GETVERSION, 6, buf ) < 0)
{
***************
*** 382,386 ****
/* TODO check return value to match 0x00A934100013 */
! printf( "GetVersion\n" );
if(sendCmd0( RIGHT_MOTOR, GETVERSION, 6, buf ) < 0)
{
--- 409,414 ----
/* TODO check return value to match 0x00A934100013 */
! if ( _debug )
! printf( "GetVersion\n" );
if(sendCmd0( RIGHT_MOTOR, GETVERSION, 6, buf ) < 0)
{
***************
*** 496,500 ****
int last_position_subscrcount=0;
! PLAYER_MSG0( 0, "Main\n" );
for(;;)
--- 524,529 ----
int last_position_subscrcount=0;
! if ( _debug )
! PLAYER_MSG0( 0, "Main\n" );
for(;;)
***************
*** 513,517 ****
{
// enable motor power
! PLAYER_MSG0( 0, "enabling motors\n" );
this->EnableMotors( true );
}
--- 542,547 ----
{
// enable motor power
! if ( _debug )
! PLAYER_MSG0( 0, "enabling motors\n" );
this->EnableMotors( true );
}
***************
*** 523,527 ****
if(!this->InQueue->Empty())
{
! PLAYER_MSG0( 0, "processing messages\n" );
ProcessMessages();
}
--- 553,558 ----
if(!this->InQueue->Empty())
{
! if ( _debug )
! PLAYER_MSG0( 0, "processing messages\n" );
ProcessMessages();
}
***************
*** 533,539 ****
}
! PLAYER_MSG0( 0, "GetAllData\n" );
GetAllData();
! PLAYER_MSG0( 0, "PublishData\n" );
PublishData();
}
--- 564,575 ----
}
! if ( _debug )
! PLAYER_MSG0( 0, "GetAllData\n" );
!
GetAllData();
!
! if ( _debug )
! PLAYER_MSG0( 0, "PublishData\n" );
!
PublishData();
}
***************
*** 558,562 ****
void * data)
{
! printf( "HandleConfig\n" );
// check for position config requests
if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
--- 594,600 ----
void * data)
{
! if ( _debug )
! printf( "HandleConfig\n" );
!
// check for position config requests
if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,
***************
*** 672,676 ****
int32_t rightvel = transvel + rotvel;
! printf( "VelCmd: px=%1.3f, pa=%1.3f, trvel=%d, rotvel=%d, rvel=%d,
lvel=%d\n", velcmd->vel.px, velcmd->vel.pa, transvel, rotvel, leftvel, rightvel
);
SetContourMode( VelocityContouringProfile );
--- 710,714 ----
int32_t rightvel = transvel + rotvel;
! // printf( "VelCmd: px=%1.3f, pa=%1.3f, trvel=%d, rotvel=%d, rvel=%d,
lvel=%d\n", velcmd->vel.px, velcmd->vel.pa, transvel, rotvel, leftvel, rightvel
);
SetContourMode( VelocityContouringProfile );
***************
*** 786,797 ****
// Therefore lets choose V=0.25V at 80cm and V=2.8V (2.25+0.55) at 10cm
// Assume the formula for mm = 270 * (voltage)^-1.1 for 80cm to 10cm
! // Assume DAC max output is 5.0V at full (10bit) count (1024)
// float v80 = 0.25;
// float deltaV = 2.25;
// float v10 = v80+deltaV;
! float dacLo = 0.0;
! float dacHi = 5.0;
! float vPerCount = (dacHi-dacLo)/1024.0;
// float mmPerVolt = (800.0-100.0)/(v80-v10);
--- 824,835 ----
// Therefore lets choose V=0.25V at 80cm and V=2.8V (2.25+0.55) at 10cm
// Assume the formula for mm = 270 * (voltage)^-1.1 for 80cm to 10cm
! // 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);
***************
*** 803,817 ****
int16_t val = 0;
! GetAnalogSensor( i, &val );
float voltage = (float)val*vPerCount;
d->voltages[ i ] = voltage;
// Convert 10 bit value to a distance in meters
! float meters = 0.27*pow(voltage,-1.1);
! d->ranges[ i ] = meters;
! //printf("(%d,%d) ",ntohs(d->ir.ranges[i]),ntohs(d->ir.voltages[i]));
}
- //printf("\n");
}
--- 841,873 ----
int16_t val = 0;
! GetAnalogSensor( i+8, &val );
float voltage = (float)val*vPerCount;
d->voltages[ i ] = voltage;
+ // Range values are useless further out than 80-90 cm
+ // with the Sharp sensors, so truncate them accordingly
+ if ( val < 80 )
+ {
+ val = 80;
+ }
+
// Convert 10 bit value to a distance in meters
! float meters;
! // Formula for range conversion is different for long range
! // sensors than short range ones. Use appropriate formula.
! if ( i == 5 || i == 7 )
! {
! // Beak side firing sensors are longer range sensors
! // Sharp GP2Y0A02 sensors 20-150cm
! meters = ((16933.0/((float)val - 8.0)) - 13.0)/100.0;
! }
! else
! {
! // Sharp GP2Y0A21 sensors 10-80cm
! meters = ((6787.0/((float)val - 3.0)) - 4.0)/100.0;
! }
! d->ranges[ i ] = meters;
}
}
***************
*** 832,836 ****
"Block Out of Bounds",
"Trace buffer zero",
! "Bad cmd checksum",
"Not primary port",
"Invalid negative value",
--- 888,892 ----
"Block Out of Bounds",
"Trace buffer zero",
! "Bad checksum",
"Not primary port",
"Invalid negative value",
***************
*** 852,861 ****
int wbr914::ResetRawPositions()
{
! printf("Reset Odometry\n");
int Values[2];
Values[0] = 0;
Values[1] = 0;
! printf( "SetActualPositionInTicks\n" );
SetActualPositionInTicks( 0, 0 );
UpdateM3();
--- 908,919 ----
int wbr914::ResetRawPositions()
{
! if ( _debug )
! printf("Reset Odometry\n");
int Values[2];
Values[0] = 0;
Values[1] = 0;
! if ( _debug )
! printf( "SetActualPositionInTicks\n" );
SetActualPositionInTicks( 0, 0 );
UpdateM3();
***************
*** 940,944 ****
// PMD 3410 error code in first byte
! if ( s[0] != 0 )
{
const char* err = GetPMDErrorString( s[0] );
--- 998,1002 ----
// PMD 3410 error code in first byte
! if ( s[0] != 0 && s[0] != 1 )
{
const char* err = GetPMDErrorString( s[0] );
***************
*** 1257,1261 ****
unsigned char ret[8];
! printf( "Stop\n" );
/* Start with motor 0*/
_stopped = true;
--- 1315,1321 ----
unsigned char ret[8];
! if ( _debug )
! printf( "Stop\n" );
!
/* Start with motor 0*/
_stopped = true;
Using Tomcat but need to do more? Need to support web services, security?
Get stuff done quickly with pre-integrated technology to make your job easier
Download IBM WebSphere Application Server v.1.0.1 based on Apache Geronimo
http://sel.as-us.falkag.net/sel?cmd=lnk&kid=120709&bid=263057&dat=121642
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
