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

Add movement class to python

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-06-11 23:52:57 +02:00
parent 2c5f283a46
commit d4c49a0a51
18 changed files with 240 additions and 53 deletions
Download patch file Download diff file Expand all files Collapse all files

2
server_v2/include/ClosedLoopMotorController.hpp
View file

@ -5,6 +5,7 @@
#include <cstdint>
#include <array>
#include <thread>
#include <mutex>
#include "include/PID.hpp"
#include "include/Robot.hpp"
@ -46,6 +47,7 @@ private:
std::array<double, ROBOT_MOTOR_COUNT> speed_targets{0};
std::thread speed_control_thread;
std::recursive_mutex speed_control_mutex;
};

4
server_v2/include/Encoders.hpp
View file

@ -21,14 +21,18 @@ public:
std::array<double, ROBOT_MOTOR_COUNT> get_velocities_rad_s();
std::array<double, ROBOT_MOTOR_COUNT> get_velocities_ticks_s();
private:
Encoders() = default;
Cache position_cache{ROBOT_ENCODER_RATE_HZ};
Cache velocity_cache{ROBOT_ENCODER_RATE_HZ};
Cache ticks_cache{ROBOT_ENCODER_RATE_HZ};
std::array<int32_t, ROBOT_MOTOR_COUNT> cached_positions = {0};
std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_rad_s = {0};
std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_ticks_s = {0};
};

13
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 8.5
#define ROBOT_MOTOR_SPEED_CONTROL_KI 31.0
#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.085
#define ROBOT_MOTOR_SPEED_CONTROL_KP 15.0
#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0
#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.0
#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 4 * M_PI
#define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250
@ -20,6 +20,7 @@
#define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0
#define ROBOT_ENCODER_RATE_HZ 500
#define ROBOT_ENCODER_MAX_CHANGE_RAD_S M_PI_4
#define ROBOT_WHEEL_RADIUS_MM 35.5
#define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0)
@ -34,13 +35,13 @@
#define ROBOT_GO_TO_DISTANCE_RATE_HZ 200
#define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0
#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 10.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_DISTANCE_MIN_VEL 0.075
#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 / 2.0)
#define ROBOT_GO_TO_ANGLE_ALPHA_P 0.9
#define ROBOT_GO_TO_ANGLE_MIN_VEL M_PI_2
#endif //COMPLIB_SERVER_ROBOT_HPP

50
server_v2/src/ClosedLoopMotorController.cpp
View file

@ -11,11 +11,13 @@
#define US_IN_S (1000 * MS_IN_S)
void ClosedLoopMotorController::set_power(uint8_t port, double power) {
std::lock_guard<std::recursive_mutex> lock(speed_control_mutex);
control_modes.at(port) = ControlMode::POWER;
Motors::set_power(port, power);
}
void ClosedLoopMotorController::set_speed(uint8_t port, double speed_ms) {
std::lock_guard<std::recursive_mutex> lock(speed_control_mutex);
speed_targets.at(port) = speed_ms;
if (control_modes.at(port) != ControlMode::SPEED) {
pids.at(port) = PID(
@ -35,15 +37,17 @@ ClosedLoopMotorController::ClosedLoopMotorController() {
while (true) {
std::this_thread::sleep_for(sleep_duration);
std::lock_guard<std::recursive_mutex> lock(speed_control_mutex);
bool should_control_speed = std::find(control_modes.begin(), control_modes.end(), ControlMode::SPEED) != control_modes.end();
if (!should_control_speed) {
continue;
}
auto speeds_ms = Encoders::getInstance().get_velocities_rad_s();
auto speeds_rad_s = 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));
spdlog::debug("CLMC: {}", i);
double power = pids.at(i)(speed_targets.at(i), speeds_rad_s.at(i));
Motors::set_power(i, power);
}
}
@ -54,7 +58,7 @@ void ClosedLoopMotorController::generate_step_response_data() {
auto console = spdlog::stdout_color_mt("generate_step_response_data");
console->set_pattern("%v");
auto input = 25.0;
auto input = 30.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;
@ -70,21 +74,21 @@ void ClosedLoopMotorController::generate_step_response_data() {
std::this_thread::sleep_for(std::chrono::seconds(1));
auto start_time = std::chrono::steady_clock::now();
for (int i = 0; i < 150; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
for (int i = 0; i < 1000; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(3));
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(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 35.0;
input = 40.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));
for (int i = 0; i < 1000; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(3));
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(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0;
@ -102,10 +106,10 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
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,
5.5, 50.0, 0.0,
4 * M_PI, 100.0);
pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID(
8.5, 31.0, 0.085,
5.5, 50.0, 0.0,
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;
@ -113,31 +117,31 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() {
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));
for (int i = 0; i < 1500; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(3));
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(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 4 * M_PI;
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));
for (int i = 0; i < 1500; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(3));
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(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = M_PI;
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));
for (int i = 0; i < 1500; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(3));
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(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
input += 0.01;
speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT;
@ -169,8 +173,8 @@ void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks
set_power(port, adjusted_power);
spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};",
ticks, target_ticks, adjusted_power);
spdlog::info("{: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);

18
server_v2/src/Encoders.cpp
View file

@ -23,9 +23,25 @@ std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rad_s() {
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;
auto change = velocity_rad_s - cached_velocities_rad_s.at(i);
change = mathUtils::limit_max(change, -ROBOT_ENCODER_MAX_CHANGE_RAD_S, ROBOT_ENCODER_MAX_CHANGE_RAD_S);
cached_velocities_rad_s.at(i) = cached_velocities_rad_s.at(i) + change;
}
velocity_cache.update();
}
return cached_velocities_rad_s;
}
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_ticks_s() {
if (ticks_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_ticks_s = mathUtils::from_bytes<int16_t>(data + i * 2, 2);
cached_velocities_ticks_s.at(i) = velocity_ticks_s;
}
ticks_cache.update();
}
return cached_velocities_ticks_s;
}

14
server_v2/src/GoToController.cpp
View file

@ -26,8 +26,9 @@ void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s)
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) {
v_m_s *= -1;
if (distance_m < 0 || v_m_s < 0) {
distance_m = -fabs(distance_m);
v_m_s = -fabs(v_m_s);
}
auto was_enabled = OdometryController::getInstance().is_enabled();
@ -74,8 +75,9 @@ 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;
if (angle_rad < 0 || v_rad_s < 0) {
angle_rad = -fabs(angle_rad);
v_rad_s = -fabs(v_rad_s);
}
auto was_enabled = OdometryController::getInstance().is_enabled();
@ -91,12 +93,12 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) {
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);
spdlog::debug("TURN: {: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);
spdlog::debug("TURN: {:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed);
}
}

8
server_v2/src/OdometryController.cpp
View file

@ -80,10 +80,10 @@ OdometryController::OdometryController() {
current_odometry = Odometry(new_x, new_y, new_theta);
spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}",
current_position_left, current_position_right,
distance_left, distance_right,
v, w);
spdlog::debug("ODOM: {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}",
current_position_left, current_position_right,
distance_left, distance_right,
v, w);
}
}
}

8
server_v2/src/PID.cpp
View file

@ -1,6 +1,7 @@
#include "include/PID.hpp"
#include <chrono>
#include <algorithm>
#include <spdlog/spdlog.h>
#define MS_IN_S 1000.0
#define US_IN_S (1000.0 * MS_IN_S)
@ -35,12 +36,15 @@ double PID::operator()(double setpoint, double process_variable) {
// Tustin transform of the integral part
double integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev);
double derivative = D * ((error - error_prev) / delta_seconds);
integral = std::clamp(integral, -limit, limit);
double output = proportional + integral + derivative;
// anti-windup - limit the output variable
output = std::min(std::max(-limit, output), limit);
// spdlog::info("E{} P{} I{} D{} O{} EP{} DS{}", error, proportional, integral, derivative, output, error_prev, delta_seconds);
output = std::clamp(output, -limit, limit);
spdlog::debug("PID: E{:03.4f} P{:03.4f} I{:03.4f} D{:03.4f} O{:03.4f} EP{:03.4f} DS{:03.4f}, SP{:03.4f} PV{:03.4f}",
error, proportional, integral, derivative, output, error_prev, delta_seconds, setpoint, process_variable
);
integral_prev = integral;
setpoint_prev = setpoint;

9
server_v2/src/main.cpp
View file

@ -14,7 +14,7 @@ using namespace std;
int main(int argc, char *argv[]) {
Reset::reset_robot();
spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
spdlog::set_level(spdlog::level::debug);
spdlog::set_level(spdlog::level::info);
UnixSocketServer unixSocketServer;
TCPSocketServer tcpSocketServer;
@ -39,7 +39,11 @@ 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);
// this_thread::sleep_for(std::chrono::milliseconds(100));
// GoToController::turn_degrees(-90, M_PI);
// while (true) {
// this_thread::sleep_for(std::chrono::milliseconds(10));
// ClosedLoopMotorController::getInstance().set_power(0, 100);
@ -69,6 +73,7 @@ int main(int argc, char *argv[]) {
// ClosedLoopMotorController::getInstance().generate_step_response_data();
// ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
// this_thread::sleep_for(std::chrono::milliseconds(1000));
std::this_thread::sleep_for(std::chrono::hours(12));
return 0;
}