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;
}
static void io_parameters_declare(parameter_namespace_t *root)
{
parameter_namespace_declare(&io_param, root, "io");
parameter_integer_declare_with_default(&uart_conn2_baud,
&io_param, "conn2_baud", 19200);
parameter_integer_declare_with_default(&uart_conn3_baud,
&io_param, "conn3_baud", SERIAL_DEFAULT_BITRATE);
parameter_integer_declare_with_default(&uart_conn4_baud,
&io_param, "conn4_baud", SERIAL_DEFAULT_BITRATE);
parameter_integer_declare_with_default(&uart_conn_i2c_baud,
&io_param, "conn_i2c_baud", SERIAL_DEFAULT_BITRATE);
}
void aseba_variables_init(parameter_namespace_t *aseba_ns)
{
/* Initializes constant variables. */
memset(&vmVariables, 0, sizeof(vmVariables));
vmVariables.productId = ASEBA_PID_UNDEFINED;
vmVariables.fwversion[0] = 0;
vmVariables.fwversion[1] = 1;
/* Registers all Aseba settings in global namespace. */
int i;
/* Must be in descending order to keep them sorted on display. */
for (i = SETTINGS_COUNT - 1; i >= 0; i--) {
sprintf(aseba_settings_name[i], "%d", i);
parameter_integer_declare_with_default(&aseba_settings[i],
aseba_ns,
aseba_settings_name[i],
0);
}
}