Go to the documentation of this file.00001
00010
00011
00012
00013
00014 #include "../../include/kobuki_driver/modules/diff_drive.hpp"
00015
00016
00017
00018
00019
00020 namespace kobuki {
00021
00022
00023
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
00033 radius(0.0), speed(0.0),
00034 point_velocity(2,0.0),
00035 bias(0.23),
00036 wheel_radius(0.035),
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
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
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
00128
00129 point_velocity[0]=vx;
00130 point_velocity[1]=wz;
00131 }
00132
00133 void DiffDrive::velocityCommands(const double &vx, const double &wz) {
00134
00135
00136 velocity_mutex.lock();
00137 const double epsilon = 0.0001;
00138
00139
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
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
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);
00169 radius = static_cast<double>(cmd_radius);
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);
00178 cmd[1] = bound(radius);
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 }