[Playerstage-commit] code/player/server/drivers/mixed/wbr/914 wbr914.cc, 1.4, 1.5

[Brian G. <ge...@us...>]

Update of /cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv27627/server/drivers/mixed/wbr/914

Modified Files:
	wbr914.cc 
Log Message:
applied patch 1602331

Index: wbr914.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914/wbr914.cc,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** wbr914.cc	20 Nov 2006 16:53:47 -0000	1.4
--- wbr914.cc	27 Nov 2006 23:51:38 -0000	1.5
***************
*** 363,366 ****
--- 363,373 ----
      struct serial_struct serial_info;
  
+     if ( ioctl( _fd, TIOCGSERIAL, &serial_info ) < 0)
+     {
+       // get the serial info
+       perror("config_serial_port: ioctl TIOCGSERIAL");
+       return(-1);
+     }
+ 
      // Custom baud rate of 416666 baud, the max the
      // motor controller will handle.
***************
*** 810,817 ****
  {
    // need to calculate the left and right velocities
!   int32_t transvel = MPS2Vel( velcmd->vel.px );
!   int32_t rotvel = MPS2Vel( velcmd->vel.pa * DEFAULT_AXLE_LENGTH/2.0 );
!   int32_t leftvel = transvel - rotvel;
!   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 );
--- 817,849 ----
  {
    // need to calculate the left and right velocities
!   float trans = velcmd->vel.px;
!   float rot = velcmd->vel.pa * DEFAULT_AXLE_LENGTH/2.0;
!   float l = trans - rot;
!   float r = trans + rot;
! 
!   // Reduce the speed on each wheel to the max
!   // speed to prevent the stepper motors from stalling
!   // Scale the speed of the other wheel to keep the
!   // turn of the same rate. Note the turn geometry
!   // will be affected.
!   if ( fabs( l ) > MOTOR_DEF_MAX_SPEED )
!   {
!     float dir = l/fabs(l);
!     float scale = l/MOTOR_DEF_MAX_SPEED;
!     l = dir*MOTOR_DEF_MAX_SPEED;
!     r = r/fabs(scale);
!   }
! 
!   if ( fabs( r ) > MOTOR_DEF_MAX_SPEED )
!   {
!     float dir = r/fabs(r);
!     float scale = r/MOTOR_DEF_MAX_SPEED;
!     r = dir*MOTOR_DEF_MAX_SPEED;
!     l = l/fabs(scale);
!   }
! 
!   int32_t leftvel = MPS2Vel( l );
!   int32_t rightvel = MPS2Vel( r );
! 
  
    //  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 );
***************
*** 1466,1470 ****
    unsigned char ret[6];
  
!   if ( sendCmd16( s / 8, READANALOG, s % 8, 6, ret )<0 )
    {
      printf( "Error reading Analog values\n" );
--- 1498,1502 ----
    unsigned char ret[6];
  
!   if ( sendCmd16( s / 8, READANALOG, s % 8, 4, ret )<0 )
    {
      printf( "Error reading Analog values\n" );
***************
*** 1488,1492 ****
    unsigned char ret[6];
  
!   if ( sendCmd16( 0, READDIGITAL, 0, 6, ret )<0)
    {
      printf( "Error reading Digital input values\n" );
--- 1520,1524 ----
    unsigned char ret[6];
  
!   if ( sendCmd16( 0, READDIGITAL, 0, 4, ret )<0)
    {
      printf( "Error reading Digital input values\n" );