diff_drive.cpp
Go to the documentation of this file.
00001 
00010 /*****************************************************************************
00011 ** Includes
00012 *****************************************************************************/
00013 
00014 #include "../../include/kobuki_driver/modules/diff_drive.hpp"
00015 
00016 /*****************************************************************************
00017 ** Namespaces
00018 *****************************************************************************/
00019 
00020 namespace kobuki {
00021 
00022 /*****************************************************************************
00023 ** Implementation
00024 *****************************************************************************/
00025 DiffDrive::DiffDrive() :
00026   last_velocity_left(0.0),
00027   last_velocity_right(0.0),
00028   last_tick_left(0),
00029   last_tick_right(0),
00030   last_rad_left(0.0),
00031   last_rad_right(0.0),
00032 //  v(0.0), w(0.0), // command velocities, in [m/s] and [rad/s]
00033   radius(0.0), speed(0.0), // command velocities, in [mm] and [mm/s]
00034   point_velocity(2,0.0), // command velocities, in [m/s] and [rad/s]
00035   bias(0.23), // wheelbase, wheel_to_wheel, in [m]
00036   wheel_radius(0.035), // radius of main wheel, in [m]
00037   imu_heading_offset(0),
00038   tick_to_rad(0.002436916871363930187454f),
00039   diff_drive_kinematics(bias, wheel_radius)
00040 {}
00041 
00053 void DiffDrive::update(const uint16_t &time_stamp,
00054             const uint16_t &left_encoder,
00055             const uint16_t &right_encoder,
00056             ecl::Pose2D<double> &pose_update,
00057             ecl::linear_algebra::Vector3d &pose_update_rates) {
00058   state_mutex.lock();
00059   static bool init_l = false;
00060   static bool init_r = false;
00061   double left_diff_ticks = 0.0f;
00062   double right_diff_ticks = 0.0f;
00063   unsigned short curr_tick_left = 0;
00064   unsigned short curr_tick_right = 0;
00065   unsigned short curr_timestamp = 0;
00066   curr_timestamp = time_stamp;
00067   curr_tick_left = left_encoder;
00068   if (!init_l)
00069   {
00070     last_tick_left = curr_tick_left;
00071     init_l = true;
00072   }
00073   left_diff_ticks = (double)(short)((curr_tick_left - last_tick_left) & 0xffff);
00074   last_tick_left = curr_tick_left;
00075   last_rad_left += tick_to_rad * left_diff_ticks;
00076 
00077   curr_tick_right = right_encoder;
00078   if (!init_r)
00079   {
00080     last_tick_right = curr_tick_right;
00081     init_r = true;
00082   }
00083   right_diff_ticks = (double)(short)((curr_tick_right - last_tick_right) & 0xffff);
00084   last_tick_right = curr_tick_right;
00085   last_rad_right += tick_to_rad * right_diff_ticks;
00086 
00087   // TODO this line and the last statements are really ugly; refactor, put in another place
00088   pose_update = diff_drive_kinematics.forward(tick_to_rad * left_diff_ticks, tick_to_rad * right_diff_ticks);
00089 
00090   if (curr_timestamp != last_timestamp)
00091   {
00092     last_diff_time = ((double)(short)((curr_timestamp - last_timestamp) & 0xffff)) / 1000.0f;
00093     last_timestamp = curr_timestamp;
00094     last_velocity_left = (tick_to_rad * left_diff_ticks) / last_diff_time;
00095     last_velocity_right = (tick_to_rad * right_diff_ticks) / last_diff_time;
00096   } else {
00097     // we need to set the last_velocity_xxx to zero?
00098   }
00099 
00100   pose_update_rates << pose_update.x()/last_diff_time,
00101                        pose_update.y()/last_diff_time,
00102                        pose_update.heading()/last_diff_time;
00103   state_mutex.unlock();
00104 }
00105 
00106 void DiffDrive::reset(const double& current_heading) {
00107   state_mutex.lock();
00108   last_rad_left = 0.0;
00109   last_rad_right = 0.0;
00110   last_velocity_left = 0.0;
00111   last_velocity_right = 0.0;
00112   imu_heading_offset = current_heading;
00113   state_mutex.unlock();
00114 }
00115 
00116 void DiffDrive::getWheelJointStates(double &wheel_left_angle, double &wheel_left_angle_rate,
00117                           double &wheel_right_angle, double &wheel_right_angle_rate) {
00118   state_mutex.lock();
00119   wheel_left_angle = last_rad_left;
00120   wheel_right_angle = last_rad_right;
00121   wheel_left_angle_rate = last_velocity_left;
00122   wheel_right_angle_rate = last_velocity_right;
00123   state_mutex.unlock();
00124 }
00125 
00126 void DiffDrive::setVelocityCommands(const double &vx, const double &wz) {
00127   // vx: in m/s
00128   // wz: in rad/s
00129   point_velocity[0]=vx;
00130   point_velocity[1]=wz;
00131 }
00132 
00133 void DiffDrive::velocityCommands(const double &vx, const double &wz) {
00134   // vx: in m/s
00135   // wz: in rad/s
00136   velocity_mutex.lock();
00137   const double epsilon = 0.0001;
00138 
00139   // Special Case #1 : Straight Run
00140   if( std::abs(wz) < epsilon ) {
00141     radius = 0.0f;
00142     speed  = 1000.0f * vx;
00143     velocity_mutex.unlock();
00144     return;
00145   }
00146 
00147   radius = vx * 1000.0f / wz;
00148   // Special Case #2 : Pure Rotation or Radius is less than or equal to 1.0 mm
00149   if( std::abs(vx) < epsilon || std::abs(radius) <= 1.0f ) {
00150     speed  = 1000.0f * bias * wz / 2.0f;
00151     radius = 1.0f;
00152     velocity_mutex.unlock();
00153     return;
00154   }
00155 
00156   // General Case :
00157   if( radius > 0.0f ) {
00158     speed  = (radius + 1000.0f * bias / 2.0f) * wz;
00159   } else {
00160     speed  = (radius - 1000.0f * bias / 2.0f) * wz;
00161   }
00162   velocity_mutex.unlock();
00163   return;
00164 }
00165 
00166 void DiffDrive::velocityCommands(const short &cmd_speed, const short &cmd_radius) {
00167   velocity_mutex.lock();
00168   speed = static_cast<double>(cmd_speed);   // In [mm/s]
00169   radius = static_cast<double>(cmd_radius); // In [mm]
00170   velocity_mutex.unlock();
00171   return;
00172 }
00173 
00174 std::vector<short> DiffDrive::velocityCommands() {
00175   velocity_mutex.lock();
00176   std::vector<short> cmd(2);
00177   cmd[0] = bound(speed);  // In [mm/s]
00178   cmd[1] = bound(radius); // In [mm]
00179   velocity_mutex.unlock();
00180   return cmd;
00181 }
00182 
00183 short DiffDrive::bound(const double &value) {
00184   if (value > static_cast<double>(SHRT_MAX)) return SHRT_MAX;
00185   if (value < static_cast<double>(SHRT_MIN)) return SHRT_MIN;
00186   return static_cast<short>(value);
00187 }
00188 
00189 } // namespace kobuki


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Mon Oct 6 2014 01:31:09