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 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);
}
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);
}
static void service_parameters_declare(parameter_namespace_t *root)
{
parameter_namespace_declare(&service_param, root, "service");
parameter_string_declare_with_default(&shell_port,
&service_param, "shell_port", shell_port_buf,
sizeof(shell_port_buf), "CONN1");
parameter_string_declare_with_default(&sumd_in_uart,
&service_param, "sumd_input", sumd_in_uart_buf,
sizeof(sumd_in_uart_buf), "OFF");
parameter_string_declare_with_default(&hott_tm_uart,
&service_param, "hott_tm", hott_tm_uart_buf,
sizeof(hott_tm_uart_buf), "CONN2");
parameter_string_declare_with_default(&datagram_message_port,
&service_param, "datagram_message_port", datagram_message_port_buf,
sizeof(datagram_message_port_buf), "OFF");
}
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");
}
int main(void)
{
halInit();
chSysInit();
timestamp_stm32_init();
board_io_pwr_en(true);
board_sensor_pwr_en(true);
error_init();
// standard output
sdStart(&UART_CONN1, NULL);
stdout = (BaseSequentialStream*)&UART_CONN1;
chprintf(stdout, "\n\n\n");
log_init();
log_handler_register(&log_handler_stdout, LOG_LVL_DEBUG, log_handler_stdout_cb);
log_info("=== boot ===");
led_start();
// mount SD card
board_power_cycle_sdcard();
sdcStart(&SDCD1, NULL);
sdcard_mount();
static char logdir[100];
if (sdcard_find_next_file_name_with_prefix("/", "log_", logdir, sizeof(logdir)) < 0) {
log_error("could not determine log file directory");
}
FRESULT res = f_mkdir(logdir);
if (!(res == FR_OK || res == FR_EXIST)) {
log_warning("could not create log directory %s", logdir);
}
size_t logdir_strlen = strlen(logdir);
if (sdcard_is_mounted()) {
// add .txt to logdir
strncpy(&logdir[logdir_strlen], "/log.txt", sizeof(logdir) - logdir_strlen);
logdir[sizeof(logdir)-1] = '\0';
sdcard_log_handler_init(logdir, LOG_LVL_INFO);
logdir[logdir_strlen] = '\0'; // reset log dir string
}
log_boot_message();
// initialization
parameter_namespace_declare(¶meters, NULL, NULL); // root namespace
parameter_string_declare_with_default(&board_name,
¶meters,
"name",
board_name_p_buf,
sizeof(board_name_p_buf),
"ins-board");
msgbus_init(&bus);
services_init();
// load parameters from SD card
log_info("loading parameters from sd card");
sdcard_read_parameter(¶meters, "/config.msgpack");
chprintf(stdout, "current parameters:");
parameter_print(¶meters, (parameter_printfn_t)chprintf, stdout);
// UART driver
io_setup();
// USB serial driver
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(100);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
// start all services
services_start(logdir);
// shellInit();
// char buf[STREAM_DEV_STR_SIZE];
// parameter_string_get(&shell_port, buf, sizeof(buf));
// BaseSequentialStream* shell_dev = get_base_seq_stream_device_from_str(buf);
// static thread_t *shelltp = NULL;
// static ShellConfig shell_cfg;
// shell_cfg.sc_channel = shell_dev;
// shell_cfg.sc_commands = shell_commands;
chThdSleepMilliseconds(500);
while (true) {
// if (shelltp == NULL && shell_dev != NULL) {
// static THD_WORKING_AREA(shell_wa, 2048);
// shelltp = shellCreateStatic(&shell_cfg, shell_wa, sizeof(shell_wa), THD_PRIO_SHELL);
// } else if (shelltp != NULL && chThdTerminatedX(shelltp)) {
// shelltp = NULL;
// }
log_debug("VCC %f", analog_get_vcc());
log_debug("Temp %f", analog_get_cpu_temp());
log_debug("V_DC %f", analog_get_vdc());
log_debug("CONN2_TX %f", analog_get_voltage(ANALOG_CH_CONN2_TX));
log_debug("CONN2_RX %f", analog_get_voltage(ANALOG_CH_CONN2_RX));
log_debug("CONN3_TX %f", analog_get_voltage(ANALOG_CH_CONN3_TX));
log_debug("CONN3_RX %f", analog_get_voltage(ANALOG_CH_CONN3_RX));
chThdSleepMilliseconds(500);
}
}
