Skip to content

Commit

Permalink
Update parameters
Browse files Browse the repository at this point in the history
  • Loading branch information
@bjsowa
bjsowa committed Nov 15, 2023
1 parent 9a986ab commit 551c4d2
Showing 1 changed file with 40 additions and 64 deletions.
104 changes: 40 additions & 64 deletions Src/firmware/parameters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand All @@ -108,36 +94,14 @@ inline void get_parameter_double(rclc_parameter_server_t* param_server,
*output = static_cast<float>(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<int>(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<int>(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,
Expand All @@ -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);
}

0 comments on commit 551c4d2

Please sign in to comment.