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

Add goToDistance and goToAngle

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-06-07 22:53:49 +02:00
parent 4ee0b8045f
commit 37447298a4
8 changed files with 104 additions and 107 deletions
Download patch file Download diff file Expand all files Collapse all files

15
server_v2/include/GoToController.hpp
View file

@ -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:

13
server_v2/include/Robot.hpp
View file

@ -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

17
server_v2/protos/CompLib.proto
View file

@ -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;
}

10
server_v2/src/ClosedLoopMotorController.cpp
View file

@ -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);

121
server_v2/src/GoToController.cpp
View file

@ -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);

2
server_v2/src/OdometryController.cpp
View file

@ -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);

19
server_v2/src/communication/MessageProcessor.cpp
View file

@ -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());

14
server_v2/src/main.cpp
View file

@ -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();