From 551c4d2db9a43a1bc359a54edee8509989964305 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 15 Nov 2023 14:46:59 +0100 Subject: [PATCH] Update parameters --- Src/firmware/parameters.cpp | 104 ++++++++++++++---------------------- 1 file changed, 40 insertions(+), 64 deletions(-) diff --git a/Src/firmware/parameters.cpp b/Src/firmware/parameters.cpp index 300d0df..439b7c7 100644 --- a/Src/firmware/parameters.cpp +++ b/Src/firmware/parameters.cpp @@ -9,20 +9,16 @@ constexpr const char* wheel_pid_i_param_name = "wheels/pid/i"; constexpr const char* wheel_pid_d_param_name = "wheels/pid/d"; constexpr const char* wheel_pwm_duty_limit_param_name = "wheels/pwm_duty_limit"; constexpr const char* mecanum_wheels_param_name = "mecanum_wheels"; -constexpr const char* dd_wheel_radius_param_name = "diff_drive/wheel_radius"; -constexpr const char* dd_wheel_separation_param_name = - "diff_drive/wheel_separation"; -constexpr const char* dd_angular_velocity_multiplier_param_name = - "diff_drive/angular_velocity_multiplier"; -constexpr const char* dd_input_timeout_param_name = "diff_drive/input_timeout"; -// constexpr const char* md_wheel_radius_param_name = "mecanum_drive/wheel_radius"; -// constexpr const char* md_wheel_separation_param_name = - // "mecanum_drive/wheel_separation"; -// constexpr const char* md_wheel_base_param_name = "mecanum_drive/wheel_base"; -// constexpr const char* md_angular_velocity_multiplier_param_name = -// "mecanum_drive/angular_velocity_multiplier"; -// constexpr const char* md_input_timeout_param_name = "mecanum_drive/input_timeout"; - +constexpr const char* controller_wheel_radius_param_name = + "controller/wheel_radius"; +constexpr const char* controller_wheel_separation_param_name = + "controller/wheel_separation"; +constexpr const char* controller_wheel_base_param_name = + "controller/wheel_base"; +constexpr const char* controller_angular_velocity_multiplier_param_name = + "controller/angular_velocity_multiplier"; +constexpr const char* controller_input_timeout_param_name = + "controller/input_timeout"; constexpr const char* battery_min_voltage_param_name = "battery_min_voltage"; inline rcl_ret_t init_parameter_double(rclc_parameter_server_t* param_server, @@ -74,28 +70,18 @@ bool Parameters::init(rclc_parameter_server_t* param_server) { wheel_pwm_duty_limit)) RCCHECK(init_parameter_bool(param_server, mecanum_wheels_param_name, mecanum_wheels)) - // diff dirve params - RCCHECK(init_parameter_double(param_server, dd_wheel_radius_param_name, - robot_wheel_radius)) - RCCHECK(init_parameter_double(param_server, dd_wheel_separation_param_name, - robot_wheel_separation)) + RCCHECK(init_parameter_double( + param_server, controller_wheel_radius_param_name, robot_wheel_radius)) RCCHECK(init_parameter_double(param_server, - dd_angular_velocity_multiplier_param_name, - robot_angular_velocity_multiplier)) - RCCHECK(init_parameter_int(param_server, dd_input_timeout_param_name, + controller_wheel_separation_param_name, + robot_wheel_separation)) + RCCHECK(init_parameter_double(param_server, controller_wheel_base_param_name, + robot_wheel_base)) + RCCHECK(init_parameter_double( + param_server, controller_angular_velocity_multiplier_param_name, + robot_angular_velocity_multiplier)) + RCCHECK(init_parameter_int(param_server, controller_input_timeout_param_name, robot_input_timeout)) - // mecanum params - // RCCHECK(init_parameter_double(param_server, md_wheel_radius_param_name, - // robot_wheel_radius)) - // RCCHECK(init_parameter_double(param_server, md_wheel_separation_param_name, - // robot_wheel_separation)) - // RCCHECK(init_parameter_double(param_server, md_wheel_base_param_name, - // robot_wheel_base)) - // RCCHECK(init_parameter_double(param_server, - // md_angular_velocity_multiplier_param_name, - // robot_angular_velocity_multiplier)) - // RCCHECK(init_parameter_int(param_server, md_input_timeout_param_name, - // robot_input_timeout)) RCCHECK(init_parameter_double(param_server, battery_min_voltage_param_name, battery_min_voltage)) return true; @@ -108,36 +94,14 @@ inline void get_parameter_double(rclc_parameter_server_t* param_server, *output = static_cast(tmp); } -void Parameters::update(rclc_parameter_server_t* param_server) { - rclc_parameter_get_bool(param_server, mecanum_wheels_param_name, - &mecanum_wheels); - - int64_t input_timeout; - // if (mecanum_wheels) { - // get_parameter_double(param_server, md_wheel_radius_param_name, - // &robot_wheel_radius); - // get_parameter_double(param_server, md_wheel_separation_param_name, - // &robot_wheel_separation); - // get_parameter_double(param_server, md_wheel_base_param_name, - // &robot_wheel_base); - // get_parameter_double(param_server, - // md_angular_velocity_multiplier_param_name, - // &robot_angular_velocity_multiplier); - // rclc_parameter_get_int(param_server, md_input_timeout_param_name, - // &input_timeout); - // } else { - get_parameter_double(param_server, dd_wheel_radius_param_name, - &robot_wheel_radius); - get_parameter_double(param_server, dd_wheel_separation_param_name, - &robot_wheel_separation); - get_parameter_double(param_server, - dd_angular_velocity_multiplier_param_name, - &robot_angular_velocity_multiplier); - rclc_parameter_get_int(param_server, dd_input_timeout_param_name, - &input_timeout); - // } - robot_input_timeout = static_cast(input_timeout); +inline void get_parameter_int(rclc_parameter_server_t* param_server, + const char* param_name, int* output) { + int64_t tmp; + rclc_parameter_get_int(param_server, param_name, &tmp); + *output = static_cast(tmp); +} +void Parameters::update(rclc_parameter_server_t* param_server) { get_parameter_double(param_server, wheel_encoder_resolution_param_name, &wheel_encoder_resolution); get_parameter_double(param_server, wheel_torque_constant_param_name, @@ -147,7 +111,19 @@ void Parameters::update(rclc_parameter_server_t* param_server) { get_parameter_double(param_server, wheel_pid_d_param_name, &wheel_pid_d); get_parameter_double(param_server, wheel_pwm_duty_limit_param_name, &wheel_pwm_duty_limit); - + rclc_parameter_get_bool(param_server, mecanum_wheels_param_name, + &mecanum_wheels); + get_parameter_double(param_server, controller_wheel_radius_param_name, + &robot_wheel_radius); + get_parameter_double(param_server, controller_wheel_separation_param_name, + &robot_wheel_separation); + get_parameter_double(param_server, controller_wheel_base_param_name, + &robot_wheel_base); + get_parameter_double(param_server, + controller_angular_velocity_multiplier_param_name, + &robot_angular_velocity_multiplier); + get_parameter_int(param_server, controller_input_timeout_param_name, + &robot_input_timeout); get_parameter_double(param_server, battery_min_voltage_param_name, &battery_min_voltage); } \ No newline at end of file