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;
}
Beispiel #2
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);
    }
}