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

add wheel tick calib

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-05-28 22:25:01 +02:00
parent 4be754c1db
commit 9000316350
3 changed files with 47 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

2
server_v2/include/ClosedLoopMotorController.hpp
View file

@ -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,

31
server_v2/src/ClosedLoopMotorController.cpp
View file

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

26
server_v2/src/main.cpp
View file

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