diff --git a/server_v2/include/GoToController.hpp b/server_v2/include/GoToController.hpp index 4eb7236..bec969b 100644 --- a/server_v2/include/GoToController.hpp +++ b/server_v2/include/GoToController.hpp @@ -1,28 +1,13 @@ -// -// 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: diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp index ed1fd88..0ff17f0 100644 --- a/server_v2/include/Robot.hpp +++ b/server_v2/include/Robot.hpp @@ -19,25 +19,28 @@ #define ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT 1.0 #define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0 -#define ROBOT_ENCODER_RATE_HZ 250 +#define ROBOT_ENCODER_RATE_HZ 500 #define ROBOT_WHEEL_RADIUS_MM 35.5 #define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0) #define ROBOT_WHEEL_CIRCUMFERENCE_MM (2.0 * ROBOT_WHEEL_RADIUS_MM * M_PI) #define ROBOT_WHEEL_CIRCUMFERENCE_M (2 * ROBOT_WHEEL_RADIUS_M * M_PI) -#define ROBOT_TICKS_PER_TURN (27.7 * 100.0) +#define ROBOT_TICKS_PER_TURN 2770.0 #define ROBOT_ARBOR_LENGTH_MM 140.0 #define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0) #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_RATE_HZ 200 #define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0 -#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 5.0 +#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 4.0 #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_ANGLE_END_RAD (0.1 * (M_PI / 180.0)) // stop 0.1 deg before target +#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_ALPHA_P 0.5 -#define ROBOT_GO_TO_ANGLE_MIN_VEL (M_PI / 3.0) +#define ROBOT_GO_TO_ANGLE_MIN_VEL (M_PI / 2.0) #endif //COMPLIB_SERVER_ROBOT_HPP diff --git a/server_v2/protos/CompLib.proto b/server_v2/protos/CompLib.proto index 1bb1bd0..8185499 100644 --- a/server_v2/protos/CompLib.proto +++ b/server_v2/protos/CompLib.proto @@ -82,3 +82,20 @@ message OdometryReadResponse { double orientation = 5; } +message DriveDistanceRequest { + Header header = 1; + double distance_m = 2; + double velocity_m_s = 3; +} + +message TurnDegreesRequest { + Header header = 1; + double angle_degrees = 2; + double velocity_rad_s = 3; +} + +message DriveRequest { + Header header = 1; + double linear_velocity_m_s = 2; + double angular_velocity_rad_s = 3; +} \ No newline at end of file diff --git a/server_v2/src/ClosedLoopMotorController.cpp b/server_v2/src/ClosedLoopMotorController.cpp index bafa082..2f34fcc 100644 --- a/server_v2/src/ClosedLoopMotorController.cpp +++ b/server_v2/src/ClosedLoopMotorController.cpp @@ -152,15 +152,13 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() { void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks_per_turn) { auto k_p = 0.0001; - auto power = 40.0; + auto power = 25.0; auto min_power = 7.5; - auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT; + auto port = ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT; auto ticks = 0.0; auto target_ticks = ticks_per_turn * turns; while (ticks != target_ticks) { - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - auto power_mult = fmax(-1.0, fmin(1.0, (target_ticks - ticks) * k_p)); auto adjusted_power = power * power_mult; if (adjusted_power < 0 && adjusted_power > -min_power) { @@ -171,9 +169,11 @@ void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks set_power(port, adjusted_power); - ticks = Encoders::getInstance().get_positions().at(port); spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};", ticks, target_ticks, adjusted_power); + + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + ticks = Encoders::getInstance().get_positions().at(port); } set_power(port, 0); diff --git a/server_v2/src/GoToController.cpp b/server_v2/src/GoToController.cpp index 53c8e8d..da32bb9 100644 --- a/server_v2/src/GoToController.cpp +++ b/server_v2/src/GoToController.cpp @@ -19,83 +19,45 @@ void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_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(); - } -} - +// +dist, +vms -> forward +// -dist, +vms -> backward +// +dist, -vms -> backward +// -dist, -vms -> backward void GoToController::drive_distance(double distance_m, double v_m_s) { + auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_DISTANCE_RATE_HZ); + 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 start_x = OdometryController::getInstance().get().get_x_position(); + auto end_x = start_x + distance_m; - 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) { + auto distance_to_target = distance_m; + while (fabs(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); - + distance_to_target = end_x - current_pos.get_x_position(); 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); + auto vel_mult = mathUtils::limit_max(distance_to_target * ROBOT_GO_TO_DISTANCE_VELOCITY_P, -1.0, 1.0); + auto adjusted_speed = mathUtils::limit_min(fabs(v_m_s) * vel_mult, ROBOT_GO_TO_DISTANCE_MIN_VEL); + diff_drive_inverse_kinematics(adjusted_speed, alpha * ROBOT_GO_TO_DISTANCE_ALPHA_P); - spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}", + 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 + distance_to_target, + alpha, vel_mult, adjusted_speed ); } + 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_LEFT_PORT, 0); @@ -104,8 +66,13 @@ void GoToController::drive_distance(double distance_m, double v_m_s) { } } +// +ang, +rad -> cw +// -ang, +rad -> ccw +// +ang, -rad -> ccw +// -ang, -rad -> ccw void GoToController::turn_degrees(double angle_deg, double v_rad_s) { auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0)); + auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_ANGLE_RATE_HZ); if (angle_rad < 0 && v_rad_s > 0) { v_rad_s *= -1; @@ -113,28 +80,24 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) { 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; - auto angle_turned = 0.0; - while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD && angle_turned < angle_rad) { + auto start_angle = OdometryController::getInstance().get().get_angular_orientation(); + auto end_angle = mathUtils::wrap_angle_to_pi(start_angle + angle_rad); + auto angle_to_target = mathUtils::wrap_angle_to_pi(end_angle - start_angle); + while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD) { std::this_thread::sleep_for(sleep_duration); + auto current_angle = OdometryController::getInstance().get().get_angular_orientation(); + angle_to_target = mathUtils::wrap_angle_to_pi(end_angle - current_angle); - auto current_pos = OdometryController::getInstance().get(); - - angle_turned = current_pos.get_angular_orientation() - start_pos.get_angular_orientation(); - angle_to_target = mathUtils::wrap_angle_to_pi(angle_rad - angle_turned); - - auto speed_mult = mathUtils::limit_max(fabs(angle_to_target) * ROBOT_GO_TO_ANGLE_ALPHA_P, 0.0, 1.0); - auto adjusted_speed = mathUtils::limit_min(v_rad_s * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL); - diff_drive_inverse_kinematics(0.0, adjusted_speed); - - spdlog::debug("{: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(), - angle_turned, angle_to_target, adjusted_speed - ); + if (fabs(angle_to_target) > M_PI_2) { + diff_drive_inverse_kinematics(0.0, v_rad_s); + spdlog::debug("{:03.4f}; {:03.4f};", current_angle, angle_to_target); + } else { + auto speed_mult = mathUtils::limit_max(angle_to_target * ROBOT_GO_TO_ANGLE_ALPHA_P, -1.0, 1.0); + auto adjusted_speed = mathUtils::limit_min(fabs(v_rad_s) * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL); + diff_drive_inverse_kinematics(0.0, adjusted_speed); + spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed); + } } ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp index 38932cf..28419c5 100644 --- a/server_v2/src/OdometryController.cpp +++ b/server_v2/src/OdometryController.cpp @@ -80,7 +80,7 @@ OdometryController::OdometryController() { } current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation); - spdlog::info("{} {} {} {} {} {}", + spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", current_position_left, current_position_right, distance_left, distance_right, distance, theta); diff --git a/server_v2/src/communication/MessageProcessor.cpp b/server_v2/src/communication/MessageProcessor.cpp index 0cbc171..283177e 100644 --- a/server_v2/src/communication/MessageProcessor.cpp +++ b/server_v2/src/communication/MessageProcessor.cpp @@ -6,6 +6,7 @@ #include "include/Encoders.hpp" #include "include/IRSensors.hpp" #include "include/ClosedLoopMotorController.hpp" +#include "include/GoToController.hpp" using namespace CompLib; @@ -46,6 +47,24 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string & ClosedLoopMotorController::getInstance().set_speed(request.port(), request.speed()); return MessageBuilder::default_successful_generic_response(); } + + // Drive + if (messageTypeName == DriveDistanceRequest::GetDescriptor()->full_name()) { + DriveDistanceRequest request; + request.ParseFromString(serializedMessage); + GoToController::drive_distance(request.distance_m(), request.velocity_m_s()); + return MessageBuilder::default_successful_generic_response(); + } else if (messageTypeName == TurnDegreesRequest::GetDescriptor()->full_name()) { + TurnDegreesRequest request; + request.ParseFromString(serializedMessage); + GoToController::turn_degrees(request.angle_degrees(), request.velocity_rad_s()); + return MessageBuilder::default_successful_generic_response(); + } else if (messageTypeName == DriveRequest::GetDescriptor()->full_name()) { + DriveRequest request; + request.ParseFromString(serializedMessage); + GoToController::diff_drive_inverse_kinematics(request.linear_velocity_m_s(), request.angular_velocity_rad_s()); + return MessageBuilder::default_successful_generic_response(); + } } catch (const std::exception &ex) { spdlog::error("Error when processing message with header {}: {}", messageTypeName, ex.what()); google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ex.what()); diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index d64c710..9948800 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -7,6 +7,7 @@ #include "include/communication/UnixSocketServer.hpp" #include "include/communication/TCPSocketServer.hpp" #include "include/GoToController.hpp" +#include "include/Encoders.hpp" using namespace std; @@ -38,7 +39,16 @@ int main(int argc, char *argv[]) { // GoToGoalController::go_to_point(2, 0, 0.2); // GoToController::drive_distance(-4, 0.2); - GoToController::turn_degrees(180, M_PI); +// GoToController::turn_degrees(90, M_PI); +// while (true) { +// this_thread::sleep_for(std::chrono::milliseconds(10)); +// ClosedLoopMotorController::getInstance().set_power(0, 100); +// spdlog::debug("{}", Encoders::getInstance().get_velocities_rad_s().at(0)); +// } + +// GoToController::drive_distance(2, 0.3); +// std::this_thread::sleep_for(std::chrono::seconds(2)); +// GoToController::drive_distance(-2, 0.3); // GoToController::drive_distance(2, 0.2); // GoToController::turn_degrees(-90, M_PI_2); @@ -52,7 +62,7 @@ int main(int argc, char *argv[]) { // GoToController::drive_distance(2, 0.2); // GoToController::turn_degrees(-90, M_PI_2); -// ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(5, stoi(argv[1])); +// ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(10, stoi(argv[1])); // ClosedLoopMotorController::getInstance().generate_step_response_data(); // ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();