From 9000316350b72b9850bdcd263c4447e6bf31c98f Mon Sep 17 00:00:00 2001 From: Konstantin Lampalzer Date: Sat, 28 May 2022 22:25:01 +0200 Subject: [PATCH] add wheel tick calib --- .../include/ClosedLoopMotorController.hpp | 2 ++ server_v2/src/ClosedLoopMotorController.cpp | 31 +++++++++++++++++++ server_v2/src/main.cpp | 26 +++++++++------- 3 files changed, 47 insertions(+), 12 deletions(-) diff --git a/server_v2/include/ClosedLoopMotorController.hpp b/server_v2/include/ClosedLoopMotorController.hpp index b5a5805..08ee94d 100644 --- a/server_v2/include/ClosedLoopMotorController.hpp +++ b/server_v2/include/ClosedLoopMotorController.hpp @@ -28,6 +28,8 @@ public: void generate_tuned_step_response_data(); + void calibrate_wheel_ticks(double turns, double ticks_per_turn); + private: enum ControlMode : uint8_t { NONE = 0, diff --git a/server_v2/src/ClosedLoopMotorController.cpp b/server_v2/src/ClosedLoopMotorController.cpp index a2ef426..bafa082 100644 --- a/server_v2/src/ClosedLoopMotorController.cpp +++ b/server_v2/src/ClosedLoopMotorController.cpp @@ -149,3 +149,34 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() { control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::NONE; control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::NONE; } + +void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks_per_turn) { + auto k_p = 0.0001; + auto power = 40.0; + auto min_power = 7.5; + auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT; + + auto ticks = 0.0; + auto target_ticks = ticks_per_turn * turns; + while (ticks != target_ticks) { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + auto power_mult = fmax(-1.0, fmin(1.0, (target_ticks - ticks) * k_p)); + auto adjusted_power = power * power_mult; + if (adjusted_power < 0 && adjusted_power > -min_power) { + adjusted_power = -min_power; + } else if (adjusted_power > 0 && adjusted_power < min_power) { + adjusted_power = min_power; + } + + set_power(port, adjusted_power); + + ticks = Encoders::getInstance().get_positions().at(port); + spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};", + ticks, target_ticks, adjusted_power); + } + + set_power(port, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + +} diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index 7eeca54..0da449e 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -10,7 +10,7 @@ using namespace std; -int main() { +int main(int argc, char *argv[]) { Reset::reset_robot(); spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v"); spdlog::set_level(spdlog::level::debug); @@ -40,20 +40,22 @@ int main() { // GoToController::drive_distance(-4, 0.2); // GoToController::turn_degrees(-90, M_PI_2); - GoToController::drive_distance(2, 0.2); - GoToController::turn_degrees(-90, M_PI_2); +// GoToController::drive_distance(2, 0.2); +// GoToController::turn_degrees(-90, M_PI_2); +// +// GoToController::drive_distance(2, 0.2); +// GoToController::turn_degrees(-90, M_PI_2); +// +// GoToController::drive_distance(2, 0.2); +// GoToController::turn_degrees(-90, M_PI_2); +// +// GoToController::drive_distance(2, 0.2); +// GoToController::turn_degrees(-90, M_PI_2); - GoToController::drive_distance(2, 0.2); - GoToController::turn_degrees(-90, M_PI_2); - - GoToController::drive_distance(2, 0.2); - GoToController::turn_degrees(-90, M_PI_2); - - GoToController::drive_distance(2, 0.2); - GoToController::turn_degrees(-90, M_PI_2); + ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(5, stoi(argv[1])); // ClosedLoopMotorController::getInstance().generate_step_response_data(); // ClosedLoopMotorController::getInstance().generate_tuned_step_response_data(); - std::this_thread::sleep_for(std::chrono::hours(12)); +// std::this_thread::sleep_for(std::chrono::hours(12)); return 0; }