89 lines
3.2 KiB
C++
89 lines
3.2 KiB
C++
#include <cmath>
|
|
|
|
#include "include/OdometryController.hpp"
|
|
#include "include/Robot.hpp"
|
|
#include "include/Encoders.hpp"
|
|
#include <spdlog/spdlog.h>
|
|
#include "include/mathUtils.hpp"
|
|
|
|
#define MS_IN_S 1000
|
|
#define US_IN_S (1000 * MS_IN_S)
|
|
|
|
bool OdometryController::is_enabled() const {
|
|
return enabled;
|
|
}
|
|
|
|
void OdometryController::enable() {
|
|
last_run = clock::now();
|
|
enabled = true;
|
|
}
|
|
|
|
void OdometryController::disable() {
|
|
spdlog::debug("OdometryController::disable()");
|
|
enabled = false;
|
|
OdometryController::reset();
|
|
}
|
|
|
|
void OdometryController::reset() {
|
|
std::lock_guard<std::recursive_mutex> lock(odometry_mutex);
|
|
current_odometry = Odometry();
|
|
last_run = clock::now();
|
|
}
|
|
|
|
Odometry OdometryController::get() {
|
|
return current_odometry;
|
|
}
|
|
|
|
|
|
OdometryController::OdometryController() {
|
|
odometry_thread = std::thread(&OdometryController::odometry_loop, this);
|
|
odometry_thread.detach();
|
|
}
|
|
|
|
[[noreturn]] void OdometryController::odometry_loop() {
|
|
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ);
|
|
while (true) {
|
|
std::this_thread::sleep_for(sleep_duration);
|
|
std::lock_guard<std::recursive_mutex> lock(odometry_mutex);
|
|
if (enabled) {
|
|
last_run = clock::now();
|
|
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_right = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
|
|
auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER;
|
|
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;
|
|
|
|
auto distance = (distance_left + distance_right) * 2.0;
|
|
auto theta = (distance_left - distance_right) / ROBOT_ARBOR_LENGTH_M;
|
|
|
|
auto new_x_position = current_odometry.get_x_position();
|
|
auto new_y_position = current_odometry.get_y_position();
|
|
auto new_angular_orientation = current_odometry.get_angular_orientation();
|
|
if (distance != 0) {
|
|
auto current_orientation = current_odometry.get_angular_orientation();
|
|
|
|
auto distance_x = cos(theta) * distance;
|
|
auto distance_y = -sin(theta) * distance;
|
|
|
|
new_x_position = current_odometry.get_x_position() +
|
|
(cos(current_orientation) * distance_x -
|
|
sin(current_orientation) * distance_y);
|
|
new_y_position = current_odometry.get_y_position() +
|
|
(sin(current_orientation) * distance_x +
|
|
cos(current_orientation) * distance_y);
|
|
}
|
|
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("{} {} {} {} {} {}",
|
|
// current_position_left, current_position_right,
|
|
// distance_left, distance_right,
|
|
// distance, theta);
|
|
}
|
|
}
|
|
}
|