velocity_commands.cpp
Go to the documentation of this file.
00001 
00010 /*****************************************************************************
00011 ** Includes
00012 *****************************************************************************/
00013 
00014 #include <iostream>
00015 #include <cstdio>
00016 #include <cmath>
00017 
00018 double bias, radius, speed;
00019 
00020 void velocityCommand(const double& vx, const double &wz) {
00021   const double epsilon = 0.0001;
00022   if ( std::abs(wz) < epsilon ) {
00023     radius = 0;
00024   } else if ( (std::abs(vx) < epsilon ) && ( wz > epsilon ) ) {
00025     radius = 1;
00026   } else if ((std::abs(vx) < epsilon ) && ( wz < -1*epsilon ) ) {
00027     radius = -1;
00028   } else {
00029     radius = (short)(vx * 1000.0f / wz);
00030   }
00031   if ( vx < 0.0 ) {
00032     speed = (short)(1000.0f * std::min(vx + bias * wz / 2.0f, vx - bias * wz / 2.0f));
00033   } else {
00034     speed = (short)(1000.0f * std::max(vx + bias * wz / 2.0f, vx - bias * wz / 2.0f));
00035   }
00036 }
00037 
00038 class Rb2Vw
00039 {
00040 public:
00041     int half_wheel_2_wheel;
00042     Rb2Vw( const int wheelToWheelInMM = 230 ) : half_wheel_2_wheel(wheelToWheelInMM/2) {}
00043 
00044     void getVW( const int leftWheelVelocity, const int rightWheelVelocity, int & linearSpeed, int & rotationalSpeedInDegree )
00045     {
00046         if( std::abs( static_cast<float>(leftWheelVelocity ))
00047             > std::abs( static_cast<float>(rightWheelVelocity) ) )
00048             linearSpeed = leftWheelVelocity;
00049         else
00050             linearSpeed = rightWheelVelocity;
00051 
00052         rotationalSpeedInDegree = ( rightWheelVelocity - leftWheelVelocity  ) * 180 / (half_wheel_2_wheel*2) / 3.14152;
00053     }
00054 
00055     void getSR( const int leftWheelVelocity, const int rightWheelVelocity, int & desiredSpeed, int & desiredRadius )
00056     {
00057         if( leftWheelVelocity == rightWheelVelocity )
00058         {
00059             desiredSpeed    = leftWheelVelocity;
00060             desiredRadius   = 0;
00061         }
00062         else if( std::abs(static_cast<float>(leftWheelVelocity)) == std::abs(static_cast<float>(rightWheelVelocity)) )
00063         {
00064             desiredSpeed = std::abs(static_cast<float>(leftWheelVelocity));
00065             if( leftWheelVelocity < rightWheelVelocity )    desiredRadius = +1;
00066             else                                            desiredRadius = -1;
00067         }
00068         else
00069         {
00070             desiredRadius   = -(leftWheelVelocity+rightWheelVelocity)*half_wheel_2_wheel/(leftWheelVelocity-rightWheelVelocity);
00071             if( std::abs(static_cast<float>(leftWheelVelocity)) < std::abs(static_cast<float>(rightWheelVelocity)) )
00072             {
00073                 desiredSpeed    = rightWheelVelocity;
00074             }
00075             else
00076             {
00077                 desiredSpeed = leftWheelVelocity;
00078                 //desiredRadius = -desiredRadius;
00079             }
00080         }
00081     }
00082 
00083     void getWheelVelocity( const double & linearVelInMeter, const double & rotationalVelInRadian, double & leftWheelVel, double & rightWheelVel )
00084     {
00085         leftWheelVel  = linearVelInMeter - rotationalVelInRadian * static_cast<double>( half_wheel_2_wheel ) * 0.001;
00086         rightWheelVel = linearVelInMeter + rotationalVelInRadian * static_cast<double>( half_wheel_2_wheel ) * 0.001;
00087     }
00088 
00089     void update( const int desiredSpeed, const int desiredRadius, int & leftWheelVelocity, int & rightWheelVelocity )
00090     {
00091         bool change_direction(false);
00092 
00093         if( desiredSpeed == 0 && desiredRadius == 0 )
00094         {
00095             leftWheelVelocity = 0;
00096             rightWheelVelocity = 0;
00097             return;
00098         }
00099 
00100         if( desiredRadius == 0 )
00101         {
00102             leftWheelVelocity   = desiredSpeed;
00103             rightWheelVelocity  = desiredSpeed;
00104         }
00105         else if( desiredRadius == -1 || desiredRadius == +1 )
00106         {
00107             leftWheelVelocity = -desiredRadius * desiredSpeed;
00108             rightWheelVelocity = desiredRadius * desiredSpeed;
00109         }
00110         else
00111         {
00112             int radius( desiredRadius );
00113             if( radius < 0 )
00114             {
00115                 radius = -radius;
00116                 change_direction = true;
00117             }
00118 
00119             int left( radius - half_wheel_2_wheel );
00120             int right( radius + half_wheel_2_wheel );
00121 
00122             if( change_direction )
00123             {
00124                 leftWheelVelocity   = (desiredSpeed*right)/right;
00125                 rightWheelVelocity  = (desiredSpeed*left)/right;
00126             }
00127             else
00128             {
00129                 leftWheelVelocity   = (desiredSpeed*left)/right;
00130                 rightWheelVelocity  = (desiredSpeed*right)/right;
00131             }
00132         }
00133     }
00134 };
00135 
00136 
00137 int main(int argc, char **argv) {
00138   std::cout << "Hello Dude" << std::endl;
00139 
00140   bias = 0.23;
00141   double vx = -0.10;
00142   double wz = 0.15;
00143 
00144   Rb2Vw fw;
00145 //      const unsigned int steps = 30;
00146 //      for (unsigned int i = 0; i < steps; ++i ) {
00147 //              velocityCommand(vx,wz);
00148 //              std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
00149 //              vx += 0.2/static_cast<double>(steps);
00150 //      }
00151 //      vx = -0.10;
00152 //      wz = -0.15;
00153 //      for ( i = 0; i < steps; ++i ) {
00154 //              velocityCommand(vx,wz);
00155 //              std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
00156 //              vx += (0.2/static_cast<double>(steps));
00157 //      }
00158 
00159   std::cout << "Reproduce the issue with slow motion command" << std::endl;
00160   vx = -0.03;
00161   wz = 0.4;
00162   velocityCommand(vx,wz);
00163   std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
00164 
00165   int left_wheel_speed;
00166   int right_wheel_speed;
00167   fw.update( speed, radius, left_wheel_speed, right_wheel_speed );
00168   printf("fw will generate left right wheel speed as [%d mm/sec | %d mm/sec] \n", left_wheel_speed, right_wheel_speed );
00169 
00170   double left_wheel_speed_in_meter;
00171   double right_wheel_speed_in_meter;
00172   int desire_speed, desired_radius;
00173   fw.getWheelVelocity( vx, wz, left_wheel_speed_in_meter, right_wheel_speed_in_meter );
00174   printf("When you have vx(%f), wz(%f), robot have to generate left right wheel speed as %f, %f \n", vx, wz, left_wheel_speed_in_meter, right_wheel_speed_in_meter );
00175   fw.getSR( left_wheel_speed_in_meter*1000, right_wheel_speed_in_meter*1000, desire_speed, desired_radius );
00176   printf("My quick code result for speed and radius are %d, %d \n", desire_speed, desired_radius );
00177 
00178   fw.update( desire_speed, desired_radius, left_wheel_speed, right_wheel_speed );
00179   printf("fw will generate left right wheel speed as [%d mm/sec | %d mm/sec] \n", left_wheel_speed, right_wheel_speed );
00180 
00181   double err_l, err_r;
00182   for( vx=-0.3;vx<0.3; vx+=0.001 )
00183   {
00184           for( wz=-0.5;wz<0.5; wz += 0.1 )
00185           {
00186                   velocityCommand(vx,wz);
00187                   fw.update( speed, radius, left_wheel_speed, right_wheel_speed );
00188                   fw.getWheelVelocity( vx, wz, left_wheel_speed_in_meter, right_wheel_speed_in_meter );
00189 
00190                   err_l = left_wheel_speed - left_wheel_speed_in_meter*1000.0;
00191                   err_r = right_wheel_speed - right_wheel_speed_in_meter*1000.0;
00192 
00193                   if( fabs(err_l) > 2.5 || fabs(err_r) > 2.5 )
00194                   {
00195                           printf("something wrong [%f,%f] --> [%f,%f] [%d,%d][%f,%f]\n", vx, wz, speed, radius,
00196                                   left_wheel_speed, right_wheel_speed,
00197                                   left_wheel_speed_in_meter*1000.0, right_wheel_speed_in_meter*1000.0 );
00198                   }
00199           }
00200   }
00201 
00202   for( vx=-0.3;vx<0.3; vx+=0.001 )
00203   {
00204           for( wz=-0.5;wz<0.5; wz += 0.1 )
00205           {
00206                   fw.getWheelVelocity( vx, wz, left_wheel_speed_in_meter, right_wheel_speed_in_meter );
00207                   fw.getSR( left_wheel_speed_in_meter*1000, right_wheel_speed_in_meter*1000, desire_speed, desired_radius );
00208                   fw.update( desire_speed, desired_radius, left_wheel_speed, right_wheel_speed );
00209 
00210 
00211                   err_l = left_wheel_speed - left_wheel_speed_in_meter*1000.0;
00212                   err_r = right_wheel_speed - right_wheel_speed_in_meter*1000.0;
00213 
00214                   if( fabs(err_l) > 2.5 || fabs(err_r) > 2.5 )
00215                   {
00216                           printf("something wrong [%f,%f] --> [%f,%f] [%d,%d][%f,%f]\n", vx, wz, speed, radius,
00217                                   left_wheel_speed, right_wheel_speed,
00218                                   left_wheel_speed_in_meter*1000.0, right_wheel_speed_in_meter*1000.0 );
00219                   }
00220           }
00221   }
00222 
00223   return 0;
00224 }
00225 
00226 
00227 
00229 //** Main
00230 //*****************************************************************************/
00231 //
00232 //int main(int argc, char **argv) {
00233 //
00234 //  std::cout << "Hello Dude" << std::endl;
00235 //
00236 //  bias = 0.23;
00237 //  double vx = -0.10;
00238 //  double wz = 0.4;
00239 //  const unsigned int steps = 20;
00240 //  for (unsigned int i = 0; i < steps; ++i ) {
00241 //    velocityCommand(vx,wz);
00242 //    std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
00243 //    vx += 0.2/static_cast<double>(steps);
00244 //  }
00245 //  vx = -0.10;
00246 //  wz = 0.4;
00247 //  for (unsigned int i = 0; i < steps; ++i ) {
00248 //    velocityCommand(vx,wz);
00249 //    std::cout << "[" << vx << "," << wz << "] -> [" << speed << "," << radius << "]" << std::endl;
00250 //    vx += 0.2/static_cast<double>(steps);
00251 //  }
00252 //  return 0;
00253 //}


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Thu Aug 27 2015 13:43:58