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

Finish drive distance, finish turn_angle

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-05-28 01:09:34 +02:00
parent 92d42be8b8
commit 4be754c1db
10 changed files with 249 additions and 132 deletions
Download patch file Download diff file Expand all files Collapse all files

4
server_v2/CMakeLists.txt
View file

@ -50,7 +50,7 @@ set(SRC_FILES
src/communication/MessageBuilder.cpp
src/communication/UnixSocketServer.cpp
src/communication/TCPSocketServer.cpp
src/GoToGoalController.cpp)
src/GoToController.cpp)
set(HDR_FILES
include/spi.hpp
@ -69,7 +69,7 @@ set(HDR_FILES
include/communication/MessageBuilder.hpp
include/communication/UnixSocketServer.hpp
include/communication/TCPSocketServer.hpp
include/GoToGoalController.hpp)
include/GoToController.hpp)
include_directories(third_party/asio)

33
server_v2/include/GoToController.hpp Normal file
View file

@ -0,0 +1,33 @@
//
// Created by KonstantinViesure on 26.05.22.
//
#ifndef COMPLIB_SERVER_GOTOCONTROLLER_HPP
#define COMPLIB_SERVER_GOTOCONTROLLER_HPP
class GoToController {
public:
static GoToController &getInstance() {
static GoToController instance;
return instance;
}
GoToController(GoToController const &) = delete;
void operator=(GoToController const &) = delete;
static void drive_distance(double distance_m, double v_ms);
static void turn_degrees(double angle_deg, double v_rad_s);
static void go_to_point(double goal_x, double goal_y, double v_ms);
static void diff_drive_inverse_kinematics(double v_m_s, double w_rad_s);
private:
GoToController() = default;
};
#endif //COMPLIB_SERVER_GOTOCONTROLLER_HPP

29
server_v2/include/GoToGoalController.hpp
View file

@ -1,29 +0,0 @@
//
// Created by KonstantinViesure on 26.05.22.
//
#ifndef COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP
#define COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP
class GoToGoalController {
public:
static GoToGoalController &getInstance() {
static GoToGoalController instance;
return instance;
}
GoToGoalController(GoToGoalController const &) = delete;
void operator=(GoToGoalController const &) = delete;
static void go_to_point(double goal_x, double goal_y, double v_ms);
static void diff_drive_inverse_kinematics(double v_m_s, double w_rad_s);
private:
GoToGoalController() = default;
};
#endif //COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP

2
server_v2/include/OdometryController.hpp
View file

@ -3,6 +3,7 @@
#include <thread>
#include <mutex>
#include <chrono>
#include "include/Odometry.hpp"
@ -34,6 +35,7 @@ private:
OdometryController();
std::thread odometry_thread;
std::recursive_mutex odometry_mutex;
[[noreturn]] void odometry_loop();

10
server_v2/include/Robot.hpp
View file

@ -7,9 +7,9 @@
#define ROBOT_IR_RATE_HZ 250
#define ROBOT_MOTOR_COUNT 4
#define ROBOT_MOTOR_SPEED_CONTROL_KP 2.5
#define ROBOT_MOTOR_SPEED_CONTROL_KI 19.0
#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.07
#define ROBOT_MOTOR_SPEED_CONTROL_KP 8.5
#define ROBOT_MOTOR_SPEED_CONTROL_KI 31.0
#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.085
#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 4 * M_PI
#define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
@ -31,5 +31,9 @@
#define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN)
#define ROBOT_GO_TO_GOAL_K 0.99 // Between 0.5 and 1
#define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0
#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 5.0
#define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target
#define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target
#endif //COMPLIB_SERVER_ROBOT_HPP

72
server_v2/src/ClosedLoopMotorController.cpp
View file

@ -54,84 +54,98 @@ void ClosedLoopMotorController::generate_step_response_data() {
auto console = spdlog::stdout_color_mt("generate_step_response_data");
console->set_pattern("%v");
auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
auto input = 10.0;
auto input = 25.0;
speed_targets.at(port) = input;
pids.at(port) = PID(
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
pids.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = PID(
1.0, 0.0, 0.0,
1000.0, 100.0);
control_modes.at(port) = ControlMode::SPEED;
pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID(
1.0, 0.0, 0.0,
1000.0, 100.0);
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::SPEED;
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::SPEED;
std::this_thread::sleep_for(std::chrono::seconds(2));
std::this_thread::sleep_for(std::chrono::seconds(1));
auto start_time = std::chrono::steady_clock::now();
for (int i = 0; i < 500; ++i) {
for (int i = 0; i < 150; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 20.0;
speed_targets.at(port) = input;
for (int i = 0; i < 500; ++i) {
input = 35.0;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
for (int i = 0; i < 150; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
speed_targets.at(port) = 0.0;
control_modes.at(port) = ControlMode::NONE;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = 0;
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::NONE;
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::NONE;
}
void ClosedLoopMotorController::generate_tuned_step_response_data() {
auto console = spdlog::stdout_color_mt("generate_tuned_step_response_data");
console->set_pattern("%v");
auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
auto input = 2 * M_PI;
speed_targets.at(port) = input;
pids.at(port) = PID(
2.5, 19.0, 0.07,
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
pids.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = PID(
8.5, 31.0, 0.085,
4 * M_PI, 100.0);
control_modes.at(port) = ControlMode::SPEED;
pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID(
8.5, 31.0, 0.085,
4 * M_PI, 100.0);
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::SPEED;
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::SPEED;
std::this_thread::sleep_for(std::chrono::seconds(2));
auto start_time = std::chrono::steady_clock::now();
for (int i = 0; i < 500; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 4 * M_PI;
speed_targets.at(port) = input;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
for (int i = 0; i < 500; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = M_PI;
speed_targets.at(port) = input;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
for (int i = 0; i < 500; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
double delta_seconds = std::chrono::duration_cast<std::chrono::duration<double, std::micro>>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S;
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
input += 0.01;
speed_targets.at(port) = input;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
}
speed_targets.at(port) = 0.0;
control_modes.at(port) = ControlMode::NONE;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = 0;
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::NONE;
control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::NONE;
}

143
server_v2/src/GoToController.cpp Normal file
View file

@ -0,0 +1,143 @@
#include <spdlog/spdlog.h>
#include "include/GoToController.hpp"
#include "include/Robot.hpp"
#include "include/ClosedLoopMotorController.hpp"
#include "include/OdometryController.hpp"
#include "include/mathUtils.hpp"
#define MS_IN_S 1000
#define US_IN_S (1000 * MS_IN_S)
// 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
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 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_LEFT_PORT, vl_rad_s);
}
void GoToController::go_to_point(double goal_x, double goal_y, double v_ms) {
auto was_enabled = OdometryController::getInstance().is_enabled();
OdometryController::getInstance().enable();
auto start_pos = OdometryController::getInstance().get();
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
auto abs_distance = fabs(sqrt(
pow(goal_x - start_pos.get_x_position(), 2) + pow(goal_y - start_pos.get_y_position(), 2)
));
while (abs_distance > 0.05) {
auto current_pos = OdometryController::getInstance().get();
auto alpha = mathUtils::wrap_angle_to_pi(
(atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()) - (current_pos.get_angular_orientation() + M_PI_2))
);
auto vr_ms = v_ms * (cos(alpha) - ROBOT_GO_TO_GOAL_K * sin(alpha));
auto vl_ms = v_ms * (cos(alpha) + ROBOT_GO_TO_GOAL_K * sin(alpha));
auto vr_rad_s = vr_ms / ROBOT_WHEEL_RADIUS_M * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
auto vl_rad_s = vl_ms / 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_LEFT_PORT, vl_rad_s);
abs_distance = fabs(sqrt(
pow(goal_x - current_pos.get_x_position(), 2) + pow(goal_y - current_pos.get_y_position(), 2)
));
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}",
current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position(),
current_pos.get_angular_orientation() + M_PI_2, atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()), alpha,
vr_rad_s, vl_rad_s);
std::this_thread::sleep_for(sleep_duration);
}
ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
if (!was_enabled) {
OdometryController::getInstance().disable();
}
}
void GoToController::drive_distance(double distance_m, double v_m_s) {
if (distance_m < 0 && v_m_s > 0) {
distance_m = fabs(distance_m);
v_m_s *= -1;
}
auto was_enabled = OdometryController::getInstance().is_enabled();
OdometryController::getInstance().enable();
auto start_pos = OdometryController::getInstance().get();
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
auto abs_distance_to_target = distance_m;
while (abs_distance_to_target > ROBOT_GO_TO_DISTANCE_END_M) {
std::this_thread::sleep_for(sleep_duration);
auto current_pos = OdometryController::getInstance().get();
auto distance_driven = fabs(current_pos.get_x_position() - start_pos.get_x_position());
abs_distance_to_target = fabs(distance_m - distance_driven);
auto alpha = -mathUtils::wrap_angle_to_pi(current_pos.get_angular_orientation());
auto vel_mult = fmax(0, fmin(1, abs_distance_to_target * ROBOT_GO_TO_DISTANCE_VELOCITY_P));
diff_drive_inverse_kinematics(v_m_s * vel_mult, alpha * ROBOT_GO_TO_DISTANCE_ALPHA_P);
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}",
current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
distance_driven, abs_distance_to_target,
alpha, vel_mult
);
}
ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
if (!was_enabled) {
OdometryController::getInstance().disable();
}
}
void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
if (angle_deg < 0 && v_rad_s > 0) {
v_rad_s *= -1;
}
auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0));
auto was_enabled = OdometryController::getInstance().is_enabled();
OdometryController::getInstance().enable();
auto start_pos = OdometryController::getInstance().get();
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
auto angle_to_target = angle_rad;
while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD) {
std::this_thread::sleep_for(sleep_duration);
auto current_pos = OdometryController::getInstance().get();
auto angle_turned = current_pos.get_angular_orientation() - start_pos.get_angular_orientation();
angle_to_target = mathUtils::wrap_angle_to_pi(angle_rad - angle_turned);
diff_drive_inverse_kinematics(0.0, v_rad_s);
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};",
current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
angle_turned, angle_to_target
);
}
ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
if (!was_enabled) {
OdometryController::getInstance().disable();
}
}

64
server_v2/src/GoToGoalController.cpp
View file

@ -1,64 +0,0 @@
#include <spdlog/spdlog.h>
#include "include/GoToGoalController.hpp"
#include "include/Robot.hpp"
#include "include/ClosedLoopMotorController.hpp"
#include "include/OdometryController.hpp"
#include "include/mathUtils.hpp"
#define MS_IN_S 1000
#define US_IN_S (1000 * MS_IN_S)
// 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
void GoToGoalController::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 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_LEFT_PORT, vl_rad_s);
}
void GoToGoalController::go_to_point(double goal_x, double goal_y, double v_ms) {
auto was_enabled = OdometryController::getInstance().is_enabled();
OdometryController::getInstance().enable();
auto start_pos = OdometryController::getInstance().get();
auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ);
auto abs_distance = fabs(sqrt(
pow(goal_x - start_pos.get_x_position(), 2) + pow(goal_y - start_pos.get_y_position(), 2)
));
while (abs_distance > 0.05) {
auto current_pos = OdometryController::getInstance().get();
auto alpha = mathUtils::wrap_angle_to_pi(
(atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()) - (current_pos.get_angular_orientation() + M_PI_2))
);
auto vr_ms = v_ms * (cos(alpha) - ROBOT_GO_TO_GOAL_K * sin(alpha));
auto vl_ms = v_ms * (cos(alpha) + ROBOT_GO_TO_GOAL_K * sin(alpha));
auto vr_rad_s = vr_ms / ROBOT_WHEEL_RADIUS_M * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT;
auto vl_rad_s = vl_ms / 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_LEFT_PORT, vl_rad_s);
abs_distance = fabs(sqrt(
pow(goal_x - current_pos.get_x_position(), 2) + pow(goal_y - current_pos.get_y_position(), 2)
));
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}",
current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(),
goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position(), atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()),
current_pos.get_angular_orientation() + M_PI_2, alpha,
vr_rad_s, vl_rad_s);
std::this_thread::sleep_for(sleep_duration);
}
ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0);
ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0);
if (!was_enabled) {
OdometryController::getInstance().disable();
}
}

6
server_v2/src/OdometryController.cpp
View file

@ -19,14 +19,14 @@ void OdometryController::enable() {
}
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_position_left = 0;
last_position_right = 0;
last_run = clock::now();
}
@ -44,7 +44,7 @@ OdometryController::OdometryController() {
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();

18
server_v2/src/main.cpp
View file

@ -6,7 +6,7 @@
#include "include/ClosedLoopMotorController.hpp"
#include "include/communication/UnixSocketServer.hpp"
#include "include/communication/TCPSocketServer.hpp"
#include "include/GoToGoalController.hpp"
#include "include/GoToController.hpp"
using namespace std;
@ -36,7 +36,21 @@ int main() {
// spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
// }
GoToGoalController::go_to_point(2, 0, 0.2);
// GoToGoalController::go_to_point(2, 0, 0.2);
// GoToController::drive_distance(-4, 0.2);
// GoToController::turn_degrees(-90, M_PI_2);
GoToController::drive_distance(2, 0.2);
GoToController::turn_degrees(-90, M_PI_2);
GoToController::drive_distance(2, 0.2);
GoToController::turn_degrees(-90, M_PI_2);
GoToController::drive_distance(2, 0.2);
GoToController::turn_degrees(-90, M_PI_2);
GoToController::drive_distance(2, 0.2);
GoToController::turn_degrees(-90, M_PI_2);
// ClosedLoopMotorController::getInstance().generate_step_response_data();
// ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();