From: Brian G. <ge...@us...> - 2006-11-27 15:53:01
|
Update of /cvsroot/playerstage/code/player/server/drivers/mixed/wbr/914 In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv28014/server/drivers/mixed/wbr/914 Modified Files: Tag: release-2-0-patches 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.1.2.5 retrieving revision 1.1.2.6 diff -C2 -d -r1.1.2.5 -r1.1.2.6 *** wbr914.cc 20 Nov 2006 16:57:32 -0000 1.1.2.5 --- wbr914.cc 27 Nov 2006 23:52:59 -0000 1.1.2.6 *************** *** 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" ); |