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

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