static void pid_register(struct pid_parameter_s *pid, parameter_namespace_t *parent, const char *name) { parameter_namespace_declare(&pid->root, parent, name); parameter_scalar_declare_with_default(&pid->kp, &pid->root, "kp", 0); parameter_scalar_declare_with_default(&pid->ki, &pid->root, "ki", 0); parameter_scalar_declare_with_default(&pid->kd, &pid->root, "kd", 0); parameter_scalar_declare_with_default(&pid->ilimit, &pid->root, "ilimit", 0); }
void motor_driver_init(motor_driver_t *d, const char *actuator_id, parameter_namespace_t *ns, memory_pool_t *traj_buffer_pool, memory_pool_t *traj_buffer_points_pool, int traj_buffer_nb_points) { chBSemObjectInit(&d->lock, false); d->traj_buffer_pool = traj_buffer_pool; d->traj_buffer_points_pool = traj_buffer_points_pool; d->traj_buffer_nb_points = traj_buffer_nb_points; strncpy(d->id, actuator_id, MOTOR_ID_MAX_LEN); d->id[MOTOR_ID_MAX_LEN] = '\0'; d->can_id = CAN_ID_NOT_SET; d->control_mode = MOTOR_CONTROL_MODE_DISABLED; d->update_period = 1; d->can_driver = NULL; parameter_namespace_declare(&d->config.root, ns, d->id); parameter_namespace_declare(&d->config.control, &d->config.root, "control"); pid_register(&d->config.position_pid, &d->config.control, "position"); pid_register(&d->config.velocity_pid, &d->config.control, "velocity"); pid_register(&d->config.current_pid, &d->config.control, "current"); parameter_scalar_declare_with_default(&d->config.torque_limit, &d->config.control, "torque_limit", 0); parameter_scalar_declare_with_default(&d->config.velocity_limit, &d->config.control, "velocity_limit", 0); parameter_scalar_declare_with_default(&d->config.acceleration_limit, &d->config.control, "acceleration_limit", 0); parameter_scalar_declare_with_default(&d->config.low_batt_th, &d->config.control, "low_batt_th", 12); parameter_namespace_declare(&d->config.thermal, &d->config.root, "thermal"); parameter_scalar_declare(&d->config.thermal_capacity, &d->config.thermal, "capacity"); parameter_scalar_declare(&d->config.thermal_resistance, &d->config.thermal, "resistance"); parameter_scalar_declare(&d->config.thermal_current_gain, &d->config.thermal, "current_gain"); parameter_scalar_declare(&d->config.max_temperature, &d->config.thermal, "max_temperature"); parameter_namespace_declare(&d->config.motor, &d->config.root, "motor"); parameter_scalar_declare_with_default(&d->config.torque_constant, &d->config.motor, "torque_constant", 1); parameter_integer_declare_with_default(&d->config.transmission_ratio_p, &d->config.motor, "transmission_ratio_p", 1); parameter_integer_declare_with_default(&d->config.transmission_ratio_q, &d->config.motor, "transmission_ratio_q", 1); parameter_integer_declare_with_default(&d->config.motor_encoder_steps_per_revolution, &d->config.motor, "motor_encoder_steps_per_revolution", 4096); parameter_integer_declare_with_default(&d->config.second_encoder_steps_per_revolution, &d->config.motor, "second_encoder_steps_per_revolution", 4096); parameter_scalar_declare_with_default(&d->config.potentiometer_gain, &d->config.motor, "potentiometer_gain", 1); parameter_integer_declare_with_default(&d->config.mode, &d->config.motor, "mode", 4); // todo parameter_namespace_declare(&d->config.stream, &d->config.root, "stream"); parameter_scalar_declare(&d->config.current_pid_stream, &d->config.stream, "current_pid"); parameter_scalar_declare(&d->config.velocity_pid_stream, &d->config.stream, "velocity_pid"); parameter_scalar_declare(&d->config.position_pid_stream, &d->config.stream, "position_pid"); parameter_scalar_declare(&d->config.index_stream, &d->config.stream, "index"); parameter_scalar_declare(&d->config.encoder_pos_stream, &d->config.stream, "encoder_pos"); parameter_scalar_declare(&d->config.motor_pos_stream, &d->config.stream, "motor_pos"); parameter_scalar_declare(&d->config.motor_torque_stream, &d->config.stream, "motor_torque"); d->stream.change_status = 0; }
void config_init(void) { parameter_namespace_declare(&global_config, NULL, NULL); parameter_namespace_declare(&actuator_config, &global_config, "actuator"); parameter_namespace_declare(&master_config, &global_config, "master"); parameter_namespace_declare(&odometry_config, &master_config, "odometry"); parameter_scalar_declare_with_default(&odometry_wheel_base, &odometry_config, "wheelbase", ROBOT_EXTERNAL_WHEELBASE); parameter_scalar_declare_with_default(&odometry_right_radius, &odometry_config, "radius_right", ROBOT_RIGHT_EXTERNAL_WHEEL_RADIUS); parameter_scalar_declare_with_default(&odometry_left_radius, &odometry_config, "radius_left", ROBOT_LEFT_EXTERNAL_WHEEL_RADIUS); parameter_namespace_declare(&differential_base_config, &master_config, "differential_base"); parameter_scalar_declare_with_default(&differential_base_wheel_base, &differential_base_config, "wheelbase", ROBOT_EXTERNAL_WHEELBASE); parameter_scalar_declare_with_default(&differential_base_right_radius, &differential_base_config, "radius_right", ROBOT_RIGHT_MOTOR_WHEEL_RADIUS); parameter_scalar_declare_with_default(&differential_base_left_radius, &differential_base_config, "radius_left", ROBOT_LEFT_MOTOR_WHEEL_RADIUS); parameter_namespace_declare(&tracy_config, &master_config, "tracy"); parameter_scalar_declare_with_default(&tracy_g, &tracy_config, "g", DEFAULT_PARAM_G); parameter_scalar_declare_with_default(&tracy_damping_coef, &tracy_config, "damping", DEFAULT_PARAM_DAMPING_COEFF); parameter_scalar_declare(&foo, &master_config, "foo"); }
void rate_limiter_init(rate_limiter_t *rl, const char *name, parameter_namespace_t *ns) { parameter_scalar_declare_with_default(&rl->rate, ns, name, 0); rl->last_update = 0; }