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

Change RPM to M/S

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-05-26 18:55:57 +02:00
parent a484bc2137
commit 6a1ac72912
7 changed files with 111 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

6
server_v2/include/ClosedLoopMotorController.hpp
View file

@ -22,7 +22,11 @@ public:
void set_power(uint8_t port, double power);
void set_speed(uint8_t port, double speed_rpm);
void set_speed(uint8_t port, double speed_ms);
void generate_step_response_data();
void generate_tuned_step_response_data();
private:
enum ControlMode : uint8_t {

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_rpm();
std::array<double, ROBOT_MOTOR_COUNT> get_velocities_ms();
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_rpm = {0};
std::array<double, ROBOT_MOTOR_COUNT> cached_velocities_ms = {0};
};

9
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 0.5
#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0
#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.025
#define ROBOT_MOTOR_SPEED_CONTROL_RAMP 100.0
#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_RATE_HZ 250
#define ROBOT_ODOMETRY_CONTROLLER_RATE_HZ 250
@ -22,6 +22,7 @@
#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_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)

96
server_v2/src/ClosedLoopMotorController.cpp
View file

@ -1,4 +1,8 @@
#include <algorithm>
#include <spdlog/spdlog.h>
#include <spdlog/sinks/stdout_color_sinks.h>
#include "include/ClosedLoopMotorController.hpp"
#include "include/Motors.hpp"
#include "include/Encoders.hpp"
@ -11,8 +15,8 @@ void ClosedLoopMotorController::set_power(uint8_t port, double power) {
Motors::set_power(port, power);
}
void ClosedLoopMotorController::set_speed(uint8_t port, double speed_rpm) {
speed_targets.at(port) = speed_rpm;
void ClosedLoopMotorController::set_speed(uint8_t port, double speed_ms) {
speed_targets.at(port) = speed_ms;
if (control_modes.at(port) != ControlMode::SPEED) {
pids.at(port) = PID(
ROBOT_MOTOR_SPEED_CONTROL_KP, ROBOT_MOTOR_SPEED_CONTROL_KI, ROBOT_MOTOR_SPEED_CONTROL_KD,
@ -36,14 +40,98 @@ ClosedLoopMotorController::ClosedLoopMotorController() {
continue;
}
auto speeds = Encoders::getInstance().get_velocities_rpm();
auto speeds_ms = Encoders::getInstance().get_velocities_ms();
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.at(i));
double power = pids.at(i)(speed_targets.at(i), speeds_ms.at(i));
Motors::set_power(i, power);
}
}
}
}
void ClosedLoopMotorController::generate_step_response_data() {
auto console = spdlog::stdout_color_mt("generate_step_response_data");
console->set_pattern("%v");
auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
auto input = 25.0;
speed_targets.at(port) = input;
pids.at(port) = PID(
1.0, 0.0, 0.0,
1000.0, 100.0);
control_modes.at(port) = ControlMode::SPEED;
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));
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);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 50.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);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
speed_targets.at(port) = 0.0;
control_modes.at(port) = ControlMode::NONE;
}
void ClosedLoopMotorController::generate_tuned_step_response_data() {
auto console = spdlog::stdout_color_mt("generate_tuned_step_response_data");
console->set_pattern("%v");
auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT;
auto input = 0.25;
speed_targets.at(port) = input;
pids.at(port) = PID(
221.0, 2110.0, 2.21,
0.2, 100.0);
control_modes.at(port) = ControlMode::SPEED;
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));
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);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 0.35;
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);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
}
input = 0.1;
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);
console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output);
input += 0.001;
speed_targets.at(port) = input;
}
speed_targets.at(port) = 0.0;
control_modes.at(port) = ControlMode::NONE;
}

7
server_v2/src/Encoders.cpp
View file

@ -15,15 +15,16 @@ std::array<int32_t, ROBOT_MOTOR_COUNT> Encoders::get_positions() {
return cached_positions;
}
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_rpm() {
std::array<double, ROBOT_MOTOR_COUNT> Encoders::get_velocities_ms() {
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);
cached_velocities_rpm.at(i) = velocity_tps * 60.0 / ROBOT_TICKS_PER_TURN;
auto velocity_rps = velocity_tps / ROBOT_TICKS_PER_TURN;
cached_velocities_ms.at(i) = velocity_rps * ROBOT_WHEEL_CIRCUMFERENCE_M;
}
velocity_cache.update();
}
return cached_velocities_rpm;
return cached_velocities_ms;
}

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_rpm());
return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_ms());
}
// IR Sensors

2
server_v2/src/main.cpp
View file

@ -26,6 +26,8 @@ int main() {
// spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
// usleep(1000);
// }
ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();
std::this_thread::sleep_for(std::chrono::hours(12));
return 0;
}