diff --git a/server_v2/include/ClosedLoopMotorController.hpp b/server_v2/include/ClosedLoopMotorController.hpp index ded8143..5e05ff8 100644 --- a/server_v2/include/ClosedLoopMotorController.hpp +++ b/server_v2/include/ClosedLoopMotorController.hpp @@ -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 { diff --git a/server_v2/include/Encoders.hpp b/server_v2/include/Encoders.hpp index 612419c..f2b29fc 100644 --- a/server_v2/include/Encoders.hpp +++ b/server_v2/include/Encoders.hpp @@ -19,7 +19,7 @@ public: std::array get_positions(); - std::array get_velocities_rpm(); + std::array get_velocities_ms(); private: Encoders() = default; @@ -28,7 +28,7 @@ private: Cache velocity_cache{ROBOT_ENCODER_RATE_HZ}; std::array cached_positions = {0}; - std::array cached_velocities_rpm = {0}; + std::array cached_velocities_ms = {0}; }; diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp index 28164eb..9400f0f 100644 --- a/server_v2/include/Robot.hpp +++ b/server_v2/include/Robot.hpp @@ -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) diff --git a/server_v2/src/ClosedLoopMotorController.cpp b/server_v2/src/ClosedLoopMotorController.cpp index ea06ead..4e33a88 100644 --- a/server_v2/src/ClosedLoopMotorController.cpp +++ b/server_v2/src/ClosedLoopMotorController.cpp @@ -1,4 +1,8 @@ #include + +#include +#include + #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::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::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::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::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::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; +} diff --git a/server_v2/src/Encoders.cpp b/server_v2/src/Encoders.cpp index 0d8ad5f..78aba7c 100644 --- a/server_v2/src/Encoders.cpp +++ b/server_v2/src/Encoders.cpp @@ -15,15 +15,16 @@ std::array Encoders::get_positions() { return cached_positions; } -std::array Encoders::get_velocities_rpm() { +std::array 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(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; } diff --git a/server_v2/src/communication/MessageProcessor.cpp b/server_v2/src/communication/MessageProcessor.cpp index f5e7898..a7b02e9 100644 --- a/server_v2/src/communication/MessageProcessor.cpp +++ b/server_v2/src/communication/MessageProcessor.cpp @@ -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 diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index 157dda9..8c4f330 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -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; }