compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Improvements in odometry kinematics

Browse source
This commit is contained in:
Alexander Lampalzer 2022-06-09 10:25:11 +02:00
parent b3eb01ad6e
commit 80ed5e5bf9
1 changed files with 48 additions and 52 deletions
Download patch file Download diff file Expand all files Collapse all files

100
server_v2/src/OdometryController.cpp
View file

@ -10,84 +10,80 @@
#define US_IN_S (1000 * MS_IN_S) #define US_IN_S (1000 * MS_IN_S)
bool OdometryController::is_enabled() const { bool OdometryController::is_enabled() const {
return enabled; return enabled;
} }
void OdometryController::enable() { void OdometryController::enable() {
last_run = clock::now(); last_run = clock::now();
enabled = true; enabled = true;
} }
void OdometryController::disable() { void OdometryController::disable() {
spdlog::debug("OdometryController::disable()"); spdlog::debug("OdometryController::disable()");
enabled = false; enabled = false;
OdometryController::reset(); OdometryController::reset();
} }
void OdometryController::reset() { void OdometryController::reset() {
std::lock_guard<std::recursive_mutex> lock(odometry_mutex); std::lock_guard<std::recursive_mutex> lock(odometry_mutex);
current_odometry = Odometry(); current_odometry = Odometry();
last_run = clock::now(); last_run = clock::now();
} }
Odometry OdometryController::get() { Odometry OdometryController::get() {
return current_odometry; return current_odometry;
} }
OdometryController::OdometryController() { OdometryController::OdometryController() {
odometry_thread = std::thread(&OdometryController::odometry_loop, this); odometry_thread = std::thread(&OdometryController::odometry_loop, this);
odometry_thread.detach(); odometry_thread.detach();
} }
[[noreturn]] void OdometryController::odometry_loop() { [[noreturn]] void OdometryController::odometry_loop() {
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ); auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ);
while (true) { while (true) {
std::this_thread::sleep_for(sleep_duration); std::this_thread::sleep_for(sleep_duration);
std::lock_guard<std::recursive_mutex> lock(odometry_mutex); std::lock_guard<std::recursive_mutex> lock(odometry_mutex);
if (enabled) { if (enabled) {
last_run = clock::now(); last_run = clock::now();
auto encoder_positions = Encoders::getInstance().get_positions(); auto encoder_positions = Encoders::getInstance().get_positions();
auto current_position_left = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; auto current_position_left =
auto current_position_right = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER; auto current_position_right =
auto distance_right = (current_position_right - last_position_right) / ROBOT_TICKS_PER_METER; encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
last_position_left = current_position_left; auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER;
last_position_right = current_position_right; auto distance_right = (current_position_right - last_position_right) / ROBOT_TICKS_PER_METER;
last_position_left = current_position_left;
last_position_right = current_position_right;
// The section below implements differential drive kinematics. // The section below implements differential drive kinematics.
// Refer to Computational Principles of Mobile Robotics, Dudek and Jenkin // Refer to Computational Principles of Mobile Robotics, Dudek and Jenkin
// or Chapter "Mobile Robot Kinematics" Introduction to Autonomous Mobile Robots, Roland Siegwart // or Chapter "Mobile Robot Kinematics" Introduction to Autonomous Mobile Robots, Roland Siegwart
// and Illah R. Nourbakhsh // and Illah R. Nourbakhsh
auto v = (distance_right + distance_left) / 2.0;
auto w = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M;
// Forward and Rotational velocity have already been integrated. auto curr_x = current_odometry.get_x_position();
auto dist_forward = (distance_right + distance_left) / 2.0; auto curr_y = current_odometry.get_y_position();
auto dist_rot = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M; auto curr_theta = current_odometry.get_angular_orientation();
auto x = dist_forward * cos(dist_rot); auto x = v * cos(w);
auto y = dist_forward * sin(dist_rot); auto y = v * sin(w);
auto theta = dist_rot;
auto new_x_position = current_odometry.get_x_position(); auto new_x = curr_x + (cos(curr_theta) * x - sin(curr_theta) * y);
auto new_y_position = current_odometry.get_y_position(); auto new_y = curr_y + (sin(curr_theta) * x + cos(curr_theta) * y);
auto new_angular_orientation = current_odometry.get_angular_orientation(); auto new_theta = curr_theta + w;
if (dist_forward != 0) { new_theta = mathUtils::wrap_angle_to_pi(new_theta);
new_x_position = cos(theta) * x - sin(theta) * y + current_odometry.get_x_position(); current_odometry = Odometry(new_x, new_y, new_theta);
new_y_position = sin(theta) * x + cos(theta) * y + current_odometry.get_y_position();
}
if (theta != 0) {
new_angular_orientation = mathUtils::wrap_angle_to_pi(current_odometry.get_angular_orientation() + theta);
}
current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation); // spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}",
// current_position_left, current_position_right,
spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", // distance_left, distance_right,
current_position_left, current_position_right, // dist_forward, dist_rot);
distance_left, distance_right, }
dist_forward, dist_rot);
} }
}
} }