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   tick_to_rad(0.002436916871363930187454f),
00038   diff_drive_kinematics(bias, wheel_radius)
00039 {}
00040 
00052 void DiffDrive::update(const uint16_t &time_stamp,
00053                        const uint16_t &left_encoder,
00054                        const uint16_t &right_encoder,
00055                        ecl::LegacyPose2D<double> &pose_update,
00056                        ecl::linear_algebra::Vector3d &pose_update_rates) {
00057   state_mutex.lock();
00058   static bool init_l = false;
00059   static bool init_r = false;
00060   double left_diff_ticks = 0.0f;
00061   double right_diff_ticks = 0.0f;
00062   unsigned short curr_tick_left = 0;
00063   unsigned short curr_tick_right = 0;
00064   unsigned short curr_timestamp = 0;
00065   curr_timestamp = time_stamp;
00066   curr_tick_left = left_encoder;
00067   if (!init_l)
00068   {
00069     last_tick_left = curr_tick_left;
00070     init_l = true;
00071   }
00072   left_diff_ticks = (double)(short)((curr_tick_left - last_tick_left) & 0xffff);
00073   last_tick_left = curr_tick_left;
00074   last_rad_left += tick_to_rad * left_diff_ticks;
00075 
00076   curr_tick_right = right_encoder;
00077   if (!init_r)
00078   {
00079     last_tick_right = curr_tick_right;
00080     init_r = true;
00081   }
00082   right_diff_ticks = (double)(short)((curr_tick_right - last_tick_right) & 0xffff);
00083   last_tick_right = curr_tick_right;
00084   last_rad_right += tick_to_rad * right_diff_ticks;
00085 
00086   // TODO this line and the last statements are really ugly; refactor, put in another place
00087   pose_update = diff_drive_kinematics.forward(tick_to_rad * left_diff_ticks, tick_to_rad * right_diff_ticks);
00088 
00089   if (curr_timestamp != last_timestamp)
00090   {
00091     last_diff_time = ((double)(short)((curr_timestamp - last_timestamp) & 0xffff)) / 1000.0f;
00092     last_timestamp = curr_timestamp;
00093     last_velocity_left = (tick_to_rad * left_diff_ticks) / last_diff_time;
00094     last_velocity_right = (tick_to_rad * right_diff_ticks) / last_diff_time;
00095   } else {
00096     // we need to set the last_velocity_xxx to zero?
00097   }
00098 
00099   pose_update_rates << pose_update.x()/last_diff_time,
00100                        pose_update.y()/last_diff_time,
00101                        pose_update.heading()/last_diff_time;
00102   state_mutex.unlock();
00103 }
00104 
00105 void DiffDrive::reset() {
00106   state_mutex.lock();
00107   last_rad_left = 0.0;
00108   last_rad_right = 0.0;
00109   last_velocity_left = 0.0;
00110   last_velocity_right = 0.0;
00111   state_mutex.unlock();
00112 }
00113 
00114 void DiffDrive::getWheelJointStates(double &wheel_left_angle, double &wheel_left_angle_rate,
00115                                     double &wheel_right_angle, double &wheel_right_angle_rate) {
00116   state_mutex.lock();
00117   wheel_left_angle = last_rad_left;
00118   wheel_right_angle = last_rad_right;
00119   wheel_left_angle_rate = last_velocity_left;
00120   wheel_right_angle_rate = last_velocity_right;
00121   state_mutex.unlock();
00122 }
00123 
00124 void DiffDrive::setVelocityCommands(const double &vx, const double &wz) {
00125   // vx: in m/s
00126   // wz: in rad/s
00127   std::vector<double> cmd_vel;
00128   cmd_vel.push_back(vx);
00129   cmd_vel.push_back(wz);
00130   point_velocity = cmd_vel;
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 std::vector<double> DiffDrive::pointVelocity() const {
00184   return point_velocity;
00185 }
00186 
00187 short DiffDrive::bound(const double &value) {
00188   if (value > static_cast<double>(SHRT_MAX)) return SHRT_MAX;
00189   if (value < static_cast<double>(SHRT_MIN)) return SHRT_MIN;
00190   return static_cast<short>(value);
00191 }
00192 
00193 } // namespace kobuki


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Thu Jun 6 2019 20:24:37