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

Fix goToAngle with new odom

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-06-09 22:44:26 +02:00
parent 80ed5e5bf9
commit 2c5f283a46
5 changed files with 71 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

3
server_v2/CMakeLists.txt
View file

@ -22,10 +22,11 @@ if (UNIX AND ${CMAKE_SYSTEM_PROCESSOR} STREQUAL armv7l)
set(LIBRARIES "pigpio" "spdlog::spdlog") set(LIBRARIES "pigpio" "spdlog::spdlog")
set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi") set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi")
set(IS_RASPI) set(IS_RASPI true)
endif () endif ()
if (NOT IS_RASPI) if (NOT IS_RASPI)
message("NOT Running on RaspberryPi")
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -gdwarf-3") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -gdwarf-3")
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -Wall -Wextra -gdwarf-3") set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -Wall -Wextra -gdwarf-3")

4
server_v2/include/Robot.hpp
View file

@ -34,9 +34,9 @@
#define ROBOT_GO_TO_DISTANCE_RATE_HZ 200 #define ROBOT_GO_TO_DISTANCE_RATE_HZ 200
#define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0 #define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0
#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 4.0 #define ROBOT_GO_TO_DISTANCE_VELOCITY_P 10.0
#define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target #define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target
#define ROBOT_GO_TO_DISTANCE_MIN_VEL 0.025 #define ROBOT_GO_TO_DISTANCE_MIN_VEL 0.075
#define ROBOT_GO_TO_ANGLE_RATE_HZ 200 #define ROBOT_GO_TO_ANGLE_RATE_HZ 200
#define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target #define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target

9
server_v2/src/GoToController.cpp
View file

@ -12,8 +12,8 @@
// Source: http://faculty.salina.k-state.edu/tim/robot_prog/MobileBot/Steering/unicycle.html#calculating-wheel-velocities // Source: http://faculty.salina.k-state.edu/tim/robot_prog/MobileBot/Steering/unicycle.html#calculating-wheel-velocities
// Source: https://www.roboticsbook.org/S52_diffdrive_actions.html#kinematics-in-code // Source: https://www.roboticsbook.org/S52_diffdrive_actions.html#kinematics-in-code
void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s) { void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s) {
auto vr_rad_s = (2 * v_m_s - w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; auto vr_rad_s = (2 * v_m_s + w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
auto vl_rad_s = (2 * v_m_s + w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; auto vl_rad_s = (2 * v_m_s - w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s); ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s);
ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s); ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s);
@ -100,6 +100,11 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
} }
} }
ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
std::this_thread::sleep_for(std::chrono::milliseconds(50));
ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);

96
server_v2/src/OdometryController.cpp
View file

@ -10,80 +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 = auto current_position_left =
encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
auto current_position_right = auto current_position_right =
encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; 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_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER;
auto distance_right = (current_position_right - last_position_right) / 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_left = current_position_left;
last_position_right = current_position_right; 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 v = (distance_right + distance_left) / 2.0;
auto w = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M; auto w = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M;
auto curr_x = current_odometry.get_x_position(); auto curr_x = current_odometry.get_x_position();
auto curr_y = current_odometry.get_y_position(); auto curr_y = current_odometry.get_y_position();
auto curr_theta = current_odometry.get_angular_orientation(); auto curr_theta = current_odometry.get_angular_orientation();
auto x = v * cos(w); auto x = v * cos(w);
auto y = v * sin(w); auto y = v * sin(w);
auto new_x = curr_x + (cos(curr_theta) * x - sin(curr_theta) * y); auto new_x = curr_x + (cos(curr_theta) * x - sin(curr_theta) * y);
auto new_y = curr_y + (sin(curr_theta) * x + cos(curr_theta) * y); auto new_y = curr_y + (sin(curr_theta) * x + cos(curr_theta) * y);
auto new_theta = curr_theta + w; auto new_theta = curr_theta + w;
new_theta = mathUtils::wrap_angle_to_pi(new_theta); new_theta = mathUtils::wrap_angle_to_pi(new_theta);
current_odometry = Odometry(new_x, new_y, new_theta); current_odometry = Odometry(new_x, new_y, new_theta);
// spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}",
// current_position_left, current_position_right, current_position_left, current_position_right,
// distance_left, distance_right, distance_left, distance_right,
// dist_forward, dist_rot); v, w);
}
} }
}
} }

21
server_v2/src/main.cpp
View file

@ -39,7 +39,7 @@ int main(int argc, char *argv[]) {
// GoToGoalController::go_to_point(2, 0, 0.2); // GoToGoalController::go_to_point(2, 0, 0.2);
// GoToController::drive_distance(-4, 0.2); // GoToController::drive_distance(-4, 0.2);
// GoToController::turn_degrees(90, M_PI); // GoToController::turn_degrees(180, M_PI);
// while (true) { // while (true) {
// this_thread::sleep_for(std::chrono::milliseconds(10)); // this_thread::sleep_for(std::chrono::milliseconds(10));
// ClosedLoopMotorController::getInstance().set_power(0, 100); // ClosedLoopMotorController::getInstance().set_power(0, 100);
@ -50,17 +50,20 @@ int main(int argc, char *argv[]) {
// std::this_thread::sleep_for(std::chrono::seconds(2)); // std::this_thread::sleep_for(std::chrono::seconds(2));
// GoToController::drive_distance(-2, 0.3); // GoToController::drive_distance(-2, 0.3);
// GoToController::drive_distance(2, 0.2); // while (true) {
// GoToController::turn_degrees(-90, M_PI_2); // GoToController::drive_distance(0.2, 0.4);
// GoToController::turn_degrees(90, M_PI_2);
// //
// GoToController::drive_distance(2, 0.2); // GoToController::drive_distance(0.2, 0.4);
// GoToController::turn_degrees(-90, M_PI_2); // GoToController::turn_degrees(90, M_PI_2);
// //
// GoToController::drive_distance(2, 0.2); // GoToController::drive_distance(0.2, 0.4);
// GoToController::turn_degrees(-90, M_PI_2); // GoToController::turn_degrees(90, M_PI_2);
// //
// GoToController::drive_distance(2, 0.2); // GoToController::drive_distance(0.2, 0.4);
// GoToController::turn_degrees(-90, M_PI_2); // GoToController::turn_degrees(90, M_PI_2);
// }
// ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(10, stoi(argv[1])); // ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(10, stoi(argv[1]));