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

Continue on go to goal behaviour

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-05-27 02:44:52 +02:00
parent 6a1ac72912
commit 92d42be8b8
13 changed files with 161 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

6
server_v2/CMakeLists.txt
View file

@ -49,7 +49,8 @@ set(SRC_FILES
src/communication/MessageProcessor.cpp
src/communication/MessageBuilder.cpp
src/communication/UnixSocketServer.cpp
src/communication/TCPSocketServer.cpp)
src/communication/TCPSocketServer.cpp
src/GoToGoalController.cpp)
set(HDR_FILES
include/spi.hpp
@ -67,7 +68,8 @@ set(HDR_FILES
include/communication/MessageProcessor.hpp
include/communication/MessageBuilder.hpp
include/communication/UnixSocketServer.hpp
include/communication/TCPSocketServer.hpp)
include/communication/TCPSocketServer.hpp
include/GoToGoalController.hpp)
include_directories(third_party/asio)

2
server_v2/include/ClosedLoopMotorController.hpp
View file

@ -22,7 +22,7 @@ public:
void set_power(uint8_t port, double power);
void set_speed(uint8_t port, double speed_ms);
void set_speed(uint8_t port, double speed_rad_s);
void generate_step_response_data();

4
server_v2/include/Encoders.hpp
View file

@ -19,7 +19,7 @@ public:
std::array<int32_t, ROBOT_MOTOR_COUNT> get_positions();
std::array<double, ROBOT_MOTOR_COUNT> get_velocities_ms();
std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rad_s();
private:
Encoders() = default;
@ -28,7 +28,7 @@ private:
Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
std::array<int32_t, ROBOT_MOTOR_COUNT> cached_positions = {0};
std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_ms = {0};
std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rad_s = {0};
};

29
server_v2/include/GoToGoalController.hpp Normal file
View file

@ -0,0 +1,29 @@
//
// 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

3
server_v2/include/OdometryController.hpp
View file

@ -26,6 +26,8 @@ public:
Odometry get();
[[nodiscard]] bool is_enabled() const;
private:
typedef std::chrono::steady_clock clock;
@ -40,6 +42,7 @@ private:
double last_position_left{0};
double last_position_right{0};
std::chrono::time_point<clock> last_run;
};

15
server_v2/include/Robot.hpp
View file

@ -7,10 +7,10 @@
#define ROBOT_IR_RATE_HZ 250
#define ROBOT_MOTOR_COUNT 4
#define ROBOT_MOTOR_SPEED_CONTROL_KP 222.0
#define ROBOT_MOTOR_SPEED_CONTROL_KI 2110.0
#define ROBOT_MOTOR_SPEED_CONTROL_KD 2.22
#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 0.2
#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_RAMP 4 * M_PI
#define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
#define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250
@ -21,12 +21,15 @@
#define ROBOT_ENCODER_RATE_HZ 250
#define ROBOT_WHEEL_CIRCUMFERENCE_MM (71.0 * M_PI)
#define ROBOT_WHEEL_CIRCUMFERENCE_M (ROBOT_WHEEL_CIRCUMFERENCE_MM / 1000.0)
#define ROBOT_WHEEL_RADIUS_MM 35.5
#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0)
#define ROBOT_WHEEL_CIRCUMFERENCE_MM (2 * 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_ARBOR_LENGTH_MM 139.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
#endif //COMPLIB_SERVER_ROBOT_HPP

6
server_v2/include/mathUtils.hpp
View file

@ -2,6 +2,7 @@
#define COMPLIB_SERVER_MATHUTILS_HPP
#include <cstdint>
#include <cmath>
namespace mathUtils {
inline int int_from_bytes(uint8_t *data, int length) {
@ -34,6 +35,11 @@ namespace mathUtils {
i++;
}
}
template<class T>
inline T wrap_angle_to_pi(T angle) {
return atan2(sin(angle), cos(angle));
}
}
#endif // COMPLIB_SERVER_MATHUTILS_HPP

28
server_v2/src/ClosedLoopMotorController.cpp
View file

@ -40,7 +40,7 @@ ClosedLoopMotorController::ClosedLoopMotorController() {
continue;
}
auto speeds_ms = Encoders::getInstance().get_velocities_ms();
auto speeds_ms = Encoders::getInstance().get_velocities_rad_s();
for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) {
if (control_modes.at(i) == ControlMode::SPEED) {
double power = pids.at(i)(speed_targets.at(i), speeds_ms.at(i));
@ -55,7 +55,7 @@ void ClosedLoopMotorController::generate_step_response_data() {
console->set_pattern("%v");
auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
auto input = 25.0;
auto input = 10.0;
speed_targets.at(port) = input;
pids.at(port) = PID(
@ -70,16 +70,16 @@ void ClosedLoopMotorController::generate_step_response_data() {
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_ms().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 50.0;
input = 20.0;
speed_targets.at(port) = input;
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_ms().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
@ -92,12 +92,12 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
console->set_pattern("%v");
auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
auto input = 0.25;
auto input = 2 * M_PI;
speed_targets.at(port) = input;
pids.at(port) = PID(
221.0, 2110.0, 2.21,
0.2, 100.0);
2.5, 19.0, 0.07,
4 * M_PI, 100.0);
control_modes.at(port) = ControlMode::SPEED;
std::this_thread::sleep_for(std::chrono::seconds(2));
@ -107,28 +107,28 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
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_ms().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 0.35;
input = 4 * M_PI;
speed_targets.at(port) = input;
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_ms().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 0.1;
input = M_PI;
speed_targets.at(port) = input;
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_ms().at(port);
auto output = Encoders::getInstance().get_velocities_rad_s().at(port);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
input += 0.001;
input += 0.01;
speed_targets.at(port) = input;
}

11
server_v2/src/Encoders.cpp
View file

@ -15,16 +15,17 @@ std::array<int32_t, ROBOT_MOTOR_COUNT> Encoders::get_positions() {
return cached_positions;
}
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_ms() {
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rad_s() {
if (velocity_cache.is_expired()) {
uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0};
Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data);
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
auto velocity_tps = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
auto velocity_rps = velocity_tps / ROBOT_TICKS_PER_TURN;
cached_velocities_ms.at(i) = velocity_rps * ROBOT_WHEEL_CIRCUMFERENCE_M;
auto velocity_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
auto velocity_rot_s = velocity_ticks_s / ROBOT_TICKS_PER_TURN;
auto velocity_rad_s = velocity_rot_s * 2 * M_PI;
cached_velocities_rad_s.at(i) = velocity_rad_s;
}
velocity_cache.update();
}
return cached_velocities_ms;
return cached_velocities_rad_s;
}

64
server_v2/src/GoToGoalController.cpp Normal file
View file

@ -0,0 +1,64 @@
#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();
}
}

8
server_v2/src/OdometryController.cpp
View file

@ -4,10 +4,15 @@
#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;
@ -15,6 +20,7 @@ void OdometryController::enable() {
void OdometryController::disable() {
enabled = false;
OdometryController::reset();
}
void OdometryController::reset() {
@ -70,7 +76,7 @@ OdometryController::OdometryController() {
cos(current_orientation) * distance_y);
}
if (theta != 0) {
new_angular_orientation = current_odometry.get_angular_orientation() + theta;
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);

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

@ -20,7 +20,7 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
if (messageTypeName == EncoderReadPositionsRequest::GetDescriptor()->full_name()) {
return MessageBuilder::encoder_read_positions_response(Encoders::getInstance().get_positions());
} else if (messageTypeName == EncoderReadVelocitiesRequest::GetDescriptor()->full_name()) {
return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_ms());
return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_rad_s());
}
// IR Sensors

18
server_v2/src/main.cpp
View file

@ -6,6 +6,7 @@
#include "include/ClosedLoopMotorController.hpp"
#include "include/communication/UnixSocketServer.hpp"
#include "include/communication/TCPSocketServer.hpp"
#include "include/GoToGoalController.hpp"
using namespace std;
@ -17,8 +18,8 @@ int main() {
UnixSocketServer unixSocketServer;
TCPSocketServer tcpSocketServer;
// ClosedLoopMotorController::getInstance().set_speed(0, -125);
// ClosedLoopMotorController::getInstance().set_speed(3, 125);
// ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, M_PI * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT);
// ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, M_PI_4 * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT);
// OdometryController::getInstance().enable();
// for (int i = 0; i < 10000; ++i) {
@ -27,7 +28,18 @@ int main() {
// usleep(1000);
// }
ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
// OdometryController::getInstance().enable();
// GoToGoalController::diff_drive_inverse_kinematics(0.2, -M_PI_4);
// for (int i = 0; i < 800; ++i) {
// std::this_thread::sleep_for(std::chrono::milliseconds(10));
// auto odom = OdometryController::getInstance().get();
// 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);
// ClosedLoopMotorController::getInstance().generate_step_response_data();
// ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
std::this_thread::sleep_for(std::chrono::hours(12));
return 0;
}