diff --git a/scripts/connect_bluetooth.py b/scripts/connect_bluetooth.py index 81c33f41..f7d5b145 100644 --- a/scripts/connect_bluetooth.py +++ b/scripts/connect_bluetooth.py @@ -182,7 +182,10 @@ def last_log_time(self): def get_battery_voltage(self): self.filter_next(function='log_battery_voltage') self.send_bt('battery\0') - return float(self.wait_filtered()[-1]) + result = self.wait_filtered() + if result is None: + return 0. + return float(result[-1]) def get_configuration_variables(self): self.filter_next(function='log_configuration_variables') @@ -197,7 +200,7 @@ class Bulebule(cmd.Cmd): prompt = '>>> ' LOG_SUBCOMMANDS = ['all', 'clear', 'save'] PLOT_SUBCOMMANDS = ['linear_speed_profile', 'angular_speed_profile'] - MOVE_SUBCOMMANDS = list('OFLRBMHElrbkj') + MOVE_SUBCOMMANDS = list('OFLRBMHElrbskj') RUN_SUBCOMMANDS = [ 'angular_speed_profile', 'linear_speed_profile', diff --git a/scripts/notebooks/front_sensors_calibration.ipynb b/scripts/notebooks/front_sensors_calibration.ipynb index 272f704f..7f86155d 100644 --- a/scripts/notebooks/front_sensors_calibration.ipynb +++ b/scripts/notebooks/front_sensors_calibration.ipynb @@ -19,6 +19,7 @@ "import pandas\n", "from pandas import DataFrame\n", "from scipy.optimize import curve_fit\n", + "import yaml\n", "\n", "\n", "# Physical dimensions of the mouse\n", @@ -44,15 +45,24 @@ "\n", "\n", "df = log_as_dataframe(pickle.load(open('../log.pkl', 'rb')))\n", - "df = df[df['function'] == 'log_front_sensors_calibration']\n", + "df = df[df['level'] == 'DATA']\n", + "df.head()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ "data = []\n", "for value in df['data'].values:\n", - " try:\n", - " value = json.loads(value.replace('nan', 'NaN'))\n", - " data.append(value)\n", - " except json.JSONDecodeError:\n", - " pass\n", + " value = yaml.load(value)\n", + " data.append(value)\n", "df = DataFrame(data)\n", + "df.columns = ['micrometers', 'left_raw_on', 'left_raw_off', 'right_raw_on', 'right_raw_off',\n", + " 'left_distance', 'right_distance']\n", + "\n", "df['left_raw'] = df['left_raw_on'] - df['left_raw_off']\n", "df['right_raw'] = df['right_raw_on'] - df['right_raw_off']\n", "df['micrometers'] = df['micrometers'] - df['micrometers'][0]\n", diff --git a/scripts/notebooks/show_data.ipynb b/scripts/notebooks/show_data.ipynb new file mode 100644 index 00000000..f26e723c --- /dev/null +++ b/scripts/notebooks/show_data.ipynb @@ -0,0 +1,139 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from pathlib import Path\n", + "import pickle\n", + "\n", + "import pandas\n", + "from pandas import DataFrame\n", + "import yaml\n", + "\n", + "\n", + "def log_as_dataframe(log):\n", + " columns = ['timestamp', 'level', 'source', 'function', 'data']\n", + " df = pandas.DataFrame(log, columns=columns)\n", + " return df\n", + "\n", + "\n", + "df = log_as_dataframe(pickle.load(open('../log.pkl', 'rb')))\n", + "df = df[df['level'] == 'DATA']\n", + "df.head()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "data = []\n", + "for row in df.itertuples():\n", + " try:\n", + " value = yaml.load(row.data)\n", + " except yaml.error.YAMLError:\n", + " continue\n", + " if len(value) != 10:\n", + " print(value)\n", + " continue\n", + " data.append([row.timestamp] + value)\n", + "df = DataFrame(data)\n", + "df.columns = ['timestamp', 'front_left', 'front_right', 'side_left', 'side_right',\n", + " 'linear_ideal', 'linear_measured', 'angular_ideal', 'angular_measured',\n", + " 'pwm_left', 'pwm_right']\n", + "df['timestamp'] /= 1000\n", + "df = df.set_index('timestamp')\n", + "df = df.sort_index()\n", + "df.head()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from bokeh.io import output_notebook\n", + "from bokeh.io import show\n", + "from bokeh.layouts import gridplot\n", + "from bokeh.models import ColumnDataSource\n", + "from bokeh.plotting import figure\n", + "\n", + "\n", + "output_notebook()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def show_data(df):\n", + " sensors = figure(width=800, height=400,\n", + " title='Front sensors log',\n", + " x_axis_label='Time (s)',\n", + " y_axis_label='Distance (m)',\n", + " y_range=(0., 0.4))\n", + " sensors.line(x=df.index, y=df['front_left'], color='orange', legend='Front-left')\n", + " sensors.line(x=df.index, y=df['front_right'], legend='Front-right')\n", + "\n", + " linear = figure(width=800, height=400,\n", + " title='Linear speed',\n", + " x_axis_label='Time (s)',\n", + " y_axis_label='Speed (m/s)',\n", + " x_range=sensors.x_range)\n", + " linear.line(x=df.index, y=df['linear_ideal'], color='orange', legend='Ideal')\n", + " linear.line(x=df.index, y=df['linear_measured'], legend='Measured')\n", + "\n", + " angular = figure(width=800, height=400,\n", + " title='Angular speed',\n", + " x_axis_label='Time (s)',\n", + " y_axis_label='Speed (rad/s)',\n", + " x_range=sensors.x_range)\n", + " angular.line(x=df.index, y=df['angular_ideal'], color='orange', legend='Ideal')\n", + " angular.line(x=df.index, y=df['angular_measured'], legend='Measured')\n", + "\n", + " pwm = figure(width=800, height=400,\n", + " title='Output PWM',\n", + " x_axis_label='Time (s)',\n", + " y_axis_label='Duty',\n", + " x_range=sensors.x_range)\n", + " pwm.line(x=df.index, y=df['pwm_left'], color='orange', legend='Left')\n", + " pwm.line(x=df.index, y=df['pwm_right'], legend='Right')\n", + "\n", + " grid = gridplot([sensors, linear, angular, pwm], ncols=1)\n", + "\n", + " show(grid)\n", + "\n", + "\n", + "show_data(df)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.7" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/src/battery.h b/src/battery.h index c1dd0274..5364371c 100644 --- a/src/battery.h +++ b/src/battery.h @@ -6,7 +6,6 @@ #include #include "common.h" -#include "formatting.h" #include "hmi.h" #include "setup.h" diff --git a/src/calibration.c b/src/calibration.c index f0621ec6..8f94cbba 100644 --- a/src/calibration.c +++ b/src/calibration.c @@ -106,6 +106,7 @@ void run_static_turn_right_profile(void) * - 'l': to turn left (in place). * - 'r': to turn right (in place). * - 'b': to turn back (in place). + * - 's': to stop now. * - 'k': keep half cell front distance. * - 'j': keep one cell front distance. */ @@ -156,6 +157,10 @@ void run_movement_sequence(const char *sequence) case 'b': turn_back(speed); break; + case 's': + set_target_linear_speed(0.); + sleep_seconds(1); + break; case 'k': keep_front_wall_distance(CELL_DIMENSION / 2.); break; @@ -181,40 +186,22 @@ void run_movement_sequence(const char *sequence) */ void run_front_sensors_calibration(void) { - float calibration_linear_speed = .3; float linear_acceleration = get_linear_acceleration(); float linear_deceleration = get_linear_deceleration(); - float required_deceleration; - int32_t target_micrometers; - int32_t micrometers_to_stop; - uint32_t ticks_to_stop; + float distance; calibrate(); disable_walls_control(); enable_motor_control(); - set_linear_acceleration(3.); + set_linear_acceleration(2.); + set_linear_deceleration(2.); - target_micrometers = get_encoder_average_micrometers() + - (2 * CELL_DIMENSION - MOUSE_LENGTH - WALL_WIDTH) * - MICROMETERS_PER_METER; - set_target_angular_speed(0.); - set_target_linear_speed(calibration_linear_speed); - micrometers_to_stop = 15000; - while (get_encoder_average_micrometers() < - target_micrometers - micrometers_to_stop) { - log_front_sensors_calibration(); - sleep_us(1000); - } - required_deceleration = - (calibration_linear_speed * calibration_linear_speed) / - (2 * - (float)(target_micrometers - get_encoder_average_micrometers()) / - MICROMETERS_PER_METER); - set_linear_deceleration(required_deceleration); - ticks_to_stop = (uint32_t)(required_time_to_speed(0.) * 1000); - set_target_linear_speed(0.); - each(2, log_front_sensors_calibration, ticks_to_stop); + distance = 2 * CELL_DIMENSION - MOUSE_LENGTH - WALL_WIDTH; + + start_data_logging(log_data_front_sensors_calibration); + target_straight(get_encoder_average_micrometers(), distance, 0.); + stop_data_logging(); reset_motion(); diff --git a/src/clock.c b/src/clock.c index 09f53d64..085d2015 100644 --- a/src/clock.c +++ b/src/clock.c @@ -131,3 +131,23 @@ void each(uint32_t period, void (*function)(void), uint32_t during) ticks_current = get_clock_ticks(); } } + +/** + * @brief Wait until the given function returns true, or timeout. + * + * @param[in] timeout Timeout duration, in ticks. + * @param[in] function Function to execute. + * + * @return False if the timeout occurred before the function returned true. + */ +bool wait_until(bool (*function)(void), uint32_t timeout) +{ + uint32_t ticks_initial; + + ticks_initial = get_clock_ticks(); + while (get_clock_ticks() - ticks_initial < timeout) { + if (function()) + return true; + } + return false; +} diff --git a/src/clock.h b/src/clock.h index 478933be..b4900a60 100644 --- a/src/clock.h +++ b/src/clock.h @@ -8,6 +8,7 @@ #include "setup.h" void each(uint32_t period, void (*function)(void), uint32_t during); +bool wait_until(bool (*function)(void), uint32_t timeout); uint32_t get_clock_ticks(void); uint32_t read_cycle_counter(void); void sleep_ticks(uint32_t ticks); diff --git a/src/control.c b/src/control.c index 78f6d78d..e9a45a46 100644 --- a/src/control.c +++ b/src/control.c @@ -219,7 +219,7 @@ void disable_motor_control(void) /** * @brief Return the current PWM duty for the left motor. */ -float get_left_pwm(void) +int32_t get_left_pwm(void) { return pwm_left; } @@ -227,7 +227,7 @@ float get_left_pwm(void) /** * @brief Return the current PWM duty for the right motor. */ -float get_right_pwm(void) +int32_t get_right_pwm(void) { return pwm_right; } @@ -241,7 +241,7 @@ float get_target_linear_speed(void) } /** - * @brief Return the current target angular speed in degrees per second. + * @brief Return the current target angular speed in radians per second. */ float get_target_angular_speed(void) { @@ -257,13 +257,29 @@ float get_ideal_linear_speed(void) } /** - * @brief Return the current ideal angular speed in degrees per second. + * @brief Return the current ideal angular speed in radians per second. */ float get_ideal_angular_speed(void) { return ideal_angular_speed; } +/** + * @brief Return the current measured linear speed in meters per second. + */ +float get_measured_linear_speed(void) +{ + return (get_encoder_left_speed() + get_encoder_right_speed()) / 2.; +} + +/** + * @brief Return the current measured angular speed in radians per second. + */ +float get_measured_angular_speed(void) +{ + return -get_gyro_z_radps(); +} + /** * @brief Set target linear speed in meters per second. */ @@ -323,10 +339,6 @@ void update_ideal_speed(void) */ void motor_control(void) { - float left_speed; - float right_speed; - float encoder_feedback_linear; - float gyro_feedback_angular; float linear_pwm; float angular_pwm; float side_sensors_feedback; @@ -335,11 +347,6 @@ void motor_control(void) if (!motor_control_enabled_signal) return; - left_speed = get_encoder_left_speed(); - right_speed = get_encoder_right_speed(); - encoder_feedback_linear = (left_speed + right_speed) / 2.; - gyro_feedback_angular = -get_gyro_z_radps(); - if (side_sensors_control_enabled) { side_sensors_feedback = get_side_sensors_error(); side_sensors_integral += side_sensors_feedback; @@ -355,8 +362,8 @@ void motor_control(void) front_sensors_feedback = 0; } - linear_error += ideal_linear_speed - encoder_feedback_linear; - angular_error += ideal_angular_speed - gyro_feedback_angular; + linear_error += ideal_linear_speed - get_measured_linear_speed(); + angular_error += ideal_angular_speed - get_measured_angular_speed(); linear_pwm = kp_linear * linear_error + kd_linear * (linear_error - last_linear_error); diff --git a/src/control.h b/src/control.h index 10fe0469..adafe8a6 100644 --- a/src/control.h +++ b/src/control.h @@ -33,12 +33,14 @@ void reset_control_speed(void); void reset_control_all(void); void enable_motor_control(void); void disable_motor_control(void); -float get_left_pwm(void); -float get_right_pwm(void); +int32_t get_left_pwm(void); +int32_t get_right_pwm(void); float get_target_linear_speed(void); float get_target_angular_speed(void); float get_ideal_linear_speed(void); float get_ideal_angular_speed(void); +float get_measured_linear_speed(void); +float get_measured_angular_speed(void); void motor_control(void); void set_target_linear_speed(float speed); void set_target_angular_speed(float speed); diff --git a/src/eeprom.c b/src/eeprom.c index 5ce2e126..192442ed 100644 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -1,5 +1,4 @@ #include "eeprom.h" -#include "formatting.h" #define BYTES_PER_WORD 4 diff --git a/src/formatting.c b/src/formatting.c deleted file mode 100644 index fe3966e8..00000000 --- a/src/formatting.c +++ /dev/null @@ -1,20 +0,0 @@ -#include "formatting.h" - -/** - * @brief Make `printf` send characters through the USART. - * - * Both standard output and standard error are redirected through the USART. - */ -int _write(int file, char *ptr, int len) -{ - int i; - - if (file == STDOUT_FILENO || file == STDERR_FILENO) { - for (i = 0; i < len; i++) - usart_send_blocking(USART3, ptr[i]); - return i; - } - - errno = EIO; - return -1; -} diff --git a/src/formatting.h b/src/formatting.h deleted file mode 100644 index 44baa6b9..00000000 --- a/src/formatting.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef __FORMATTING_H -#define __FORMATTING_H - -#include -#include -#include -#include - -#include - -#include "clock.h" - -enum { LOG_LEVEL_DEBUG, LOG_LEVEL_INFO, LOG_LEVEL_WARNING, LOG_LEVEL_ERROR }; -static const char *const log_level_strings[] = {"DEBUG", "INFO", "WARNING", - "ERROR"}; - -int _write(int file, char *ptr, int len); - -#define LOG_MESSAGE(level, format, arg...) \ - do { \ - uint32_t time = get_clock_ticks(); \ - printf("%" PRIu32 ",%s,%s:%d,%s," format "\n", time, \ - log_level_strings[level], __FILE__, __LINE__, __func__, \ - ##arg); \ - } while (0) - -#define LOG_DEBUG(format, arg...) LOG_MESSAGE(LOG_LEVEL_DEBUG, format, ##arg) -#define LOG_INFO(format, arg...) LOG_MESSAGE(LOG_LEVEL_INFO, format, ##arg) -#define LOG_WARNING(format, arg...) \ - LOG_MESSAGE(LOG_LEVEL_WARNING, format, ##arg) -#define LOG_ERROR(format, arg...) LOG_MESSAGE(LOG_LEVEL_ERROR, format, ##arg) - -#endif /* __FORMATTING_H */ diff --git a/src/logging.c b/src/logging.c index 7e62df5b..bb6e7e9e 100644 --- a/src/logging.c +++ b/src/logging.c @@ -1,5 +1,52 @@ #include "logging.h" +static volatile bool data_logging; +static void (*data_logging_function)(void); + +/** + * @brief Start data logging. + * + * This function is called from `main` and enables data logging. The provided + * data logging function will then be called on each SYSTICK. + * + * @param[in] log_function The data logging function to call on each SYSTICK. + */ +void start_data_logging(void (*log_function)(void)) +{ + LOG_INFO("Data logging on"); + data_logging_function = log_function; + sleep_ticks(2); + led_right_on(); + data_logging = true; +} + +/** + * @brief Stop data logging. + * + * This function is called from `main` and disables any previously enabled + * data logging function. + */ +void stop_data_logging(void) +{ + data_logging = false; + led_right_off(); + sleep_ticks(2); + LOG_INFO("Data logging off"); +} + +/** + * @brief Log data calling the `data_logging_function()`. + * + * This function is called from the SYSTICK periodically. It will not log + * any data if `data_logging` is set to false (i.e.: data logging is disabled). + */ +void log_data(void) +{ + if (!data_logging) + return; + data_logging_function(); +} + /** * @brief Log the current battery voltage. */ @@ -15,10 +62,6 @@ void log_configuration_variables(void) { float micrometers_per_count = get_micrometers_per_count(); float wheels_separation = get_wheels_separation(); - float max_linear_speed = get_max_linear_speed(); - float linear_acceleration = get_linear_acceleration(); - float linear_deceleration = get_linear_deceleration(); - float angular_acceleration = get_angular_acceleration(); float kp_linear = get_kp_linear(); float kd_linear = get_kd_linear(); float kp_angular = get_kp_angular(); @@ -30,10 +73,6 @@ void log_configuration_variables(void) LOG_INFO("{\"micrometers_per_count\":%f," "\"wheels_separation\":%f," - "\"max_linear_speed\":%f," - "\"linear_acceleration\":%f," - "\"linear_deceleration\":%f," - "\"angular_acceleration\":%f," "\"kp_linear\":%f," "\"kd_linear\":%f," "\"kp_angular\":%f," @@ -42,10 +81,9 @@ void log_configuration_variables(void) "\"ki_angular_front\":%f," "\"kp_angular_side\":%f," "\"kp_angular_front\":%f}", - micrometers_per_count, wheels_separation, max_linear_speed, - linear_acceleration, linear_deceleration, angular_acceleration, - kp_linear, kd_linear, kp_angular, kd_angular, ki_angular_side, - ki_angular_front, kp_angular_side, kp_angular_front); + micrometers_per_count, wheels_separation, kp_linear, kd_linear, + kp_angular, kd_angular, ki_angular_side, ki_angular_front, + kp_angular_side, kp_angular_front); } /** @@ -104,23 +142,6 @@ void log_sensors_distance(void) LOG_INFO("%f,%f,%f,%f", sl_dist, sr_dist, fl_dist, fr_dist); } -/** - * @brief Log all sensor distance readings, published for real-time. - */ -void log_sensors_distance_pub(void) -{ - float reading; - - reading = get_side_left_distance(); - LOG_INFO("PUB,line,left-side,%f", reading); - reading = get_front_left_distance(); - LOG_INFO("PUB,line,left-front,%f", reading); - reading = get_front_right_distance(); - LOG_INFO("PUB,line,right-front,%f", reading); - reading = get_side_right_distance(); - LOG_INFO("PUB,line,right-side,%f", reading); -} - /** * @brief Log all sensors raw readings. */ @@ -139,51 +160,44 @@ void log_sensors_raw(void) } /** - * @brief Log all sensor raw readings, published for real-time. + * @brief Log front sensors variables for calibration. */ -void log_sensors_raw_pub(void) +void log_data_front_sensors_calibration(void) { + float left_distance = get_front_left_distance(); + float right_distance = get_front_right_distance(); uint16_t off[NUM_SENSOR]; uint16_t on[NUM_SENSOR]; + int32_t micrometers = get_encoder_average_micrometers(); get_sensors_raw(off, on); - LOG_INFO("PUB,line,left-side-raw-on,%u", on[SENSOR_SIDE_LEFT_ID]); - LOG_INFO("PUB,line,left-front-raw-on,%u", on[SENSOR_FRONT_LEFT_ID]); - LOG_INFO("PUB,line,right-front-raw-on,%u", on[SENSOR_FRONT_RIGHT_ID]); - LOG_INFO("PUB,line,right-side-raw-on,%u", on[SENSOR_SIDE_RIGHT_ID]); - LOG_INFO("PUB,line,left-side-raw-off,%u", off[SENSOR_SIDE_LEFT_ID]); - LOG_INFO("PUB,line,left-front-raw-off,%u", off[SENSOR_FRONT_LEFT_ID]); - LOG_INFO("PUB,line,right-front-raw-off,%u", off[SENSOR_FRONT_RIGHT_ID]); - LOG_INFO("PUB,line,right-side-raw-off,%u", off[SENSOR_SIDE_RIGHT_ID]); + LOG_DATA("[%" PRId32 ",%d,%d,%d,%d,%.4f,%.4f]", micrometers, + on[SENSOR_FRONT_LEFT_ID], off[SENSOR_FRONT_LEFT_ID], + on[SENSOR_FRONT_RIGHT_ID], off[SENSOR_FRONT_RIGHT_ID], + left_distance, right_distance); } /** - * @brief Log front sensors variables for calibration. + * @brief Log all interesting control variables. */ -void log_front_sensors_calibration(void) +void log_data_control(void) { - float sensors_error = get_front_sensors_error(); - float left_distance = get_front_left_distance(); - float right_distance = get_front_right_distance(); - uint16_t off[NUM_SENSOR]; - uint16_t on[NUM_SENSOR]; - int32_t micrometers = get_encoder_average_micrometers(); - - get_sensors_raw(off, on); - - LOG_INFO("{\"micrometers\":%" PRId32 "," - "\"left_raw_on\":%d," - "\"left_raw_off\":%d," - "\"right_raw_on\":%d," - "\"right_raw_off\":%d," - "\"left_distance\":%f," - "\"right_distance\":%f," - "\"distance_error\":%f}", - micrometers, on[SENSOR_FRONT_LEFT_ID], - off[SENSOR_FRONT_LEFT_ID], on[SENSOR_FRONT_RIGHT_ID], - off[SENSOR_FRONT_RIGHT_ID], left_distance, right_distance, - sensors_error); + float front_left_distance = get_front_left_distance(); + float front_right_distance = get_front_right_distance(); + float side_left_distance = get_side_left_distance(); + float side_right_distance = get_side_right_distance(); + float ideal_linear = get_ideal_linear_speed(); + float ideal_angular = get_ideal_angular_speed(); + float measured_linear = get_measured_linear_speed(); + float measured_angular = get_measured_angular_speed(); + int left_pwm = get_left_pwm(); + int right_pwm = get_right_pwm(); + + LOG_DATA("[%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%d,%d]", + front_left_distance, front_right_distance, side_left_distance, + side_right_distance, ideal_linear, measured_linear, + ideal_angular, measured_angular, left_pwm, right_pwm); } /** @@ -230,27 +244,3 @@ void log_walls_detection(void) "\"wall_front\":%d}", left_wall, right_wall, front_wall); } - -/** - * @brief Log gyroscope's Z-axis raw readings, published for real-time. - */ -void log_gyro_raw_pub(void) -{ - LOG_INFO("PUB,line,gyro_raw,%f", (float)get_gyro_z_raw()); -} - -/** - * @brief Log gyroscope's Z-axis DPS readings, published for real-time. - */ -void log_gyro_dps_pub(void) -{ - LOG_INFO("PUB,line,gyro_dps,%f", (float)get_gyro_z_dps()); -} - -/** - * @brief Log gyroscope's Z-axis degrees readings, published for real-time. - */ -void log_gyro_degrees_pub(void) -{ - LOG_INFO("PUB,line,gyro_degrees,%f", get_gyro_z_degrees()); -} diff --git a/src/logging.h b/src/logging.h index 7980e6fe..9d7bbb3b 100644 --- a/src/logging.h +++ b/src/logging.h @@ -1,30 +1,102 @@ #ifndef __LOGGING_H #define __LOGGING_H +#include +#include +#include +#include + #include "battery.h" +#include "clock.h" #include "control.h" #include "detection.h" #include "encoder.h" -#include "formatting.h" #include "move.h" #include "mpu.h" +#include "serial.h" #include "speed.h" +#define LOG_MESSAGE_TIMEOUT 10 + +enum { LOG_LEVEL_DEBUG, LOG_LEVEL_INFO, LOG_LEVEL_WARNING, LOG_LEVEL_ERROR }; +static const char *const log_level_strings[] = {"DEBUG", "INFO", "WARNING", + "ERROR"}; + +/** + * @brief Log a message with a provided level and format. + * + * The `format` string is formatted with the passed extra parameters. Some + * other parameters are prepended to that string, in CSV-formatted style: + * + * - Time (in clock ticks) + * - Log level + * - File and line where the macro was called + * - Function from where the macro was called + * + * Log messages always end with a `\n` character. + * + * This function waits for `LOG_MESSAGE_TIMEOUT` for the serial interface to + * be available before writing the log message. After the timeout occurs, it + * will always write the message to the DMA buffer anyways. + */ +#define LOG_MESSAGE(level, format, arg...) \ + do { \ + uint32_t time = get_clock_ticks(); \ + static char tx_buffer[400]; \ + sprintf(tx_buffer, "%" PRIu32 ",%s,%s:%d,%s," format "\n", \ + time, log_level_strings[level], __FILE__, __LINE__, \ + __func__, ##arg); \ + serial_wait_send_available(LOG_MESSAGE_TIMEOUT); \ + serial_send(tx_buffer, strlen(tx_buffer)); \ + } while (0) + +/** + * @brief Log some data with a provided format. + * + * The `format` string is formatted with the passed extra parameters. Some + * other parameters are prepended to that string, in CSV-formatted style: + * + * - Time (in clock ticks) + * - Log level, which is always "DATA" + * - File and line where the macro was called is omitted + * - Function from where the macro was called is omitted + * + * Data logs always end with a `\n` character. + * + * This macro checks if the serial interface is not busy sending data. If it is + * it will simply do nothing and discard the data log. + */ +#define LOG_DATA(format, arg...) \ + do { \ + uint32_t time = get_clock_ticks(); \ + static char tx_buffer[100]; \ + if (!serial_transfer_complete()) \ + break; \ + sprintf(tx_buffer, "%" PRIu32 ",DATA,,," format "\n", time, \ + ##arg); \ + serial_send(tx_buffer, strlen(tx_buffer)); \ + } while (0) + +#define LOG_DEBUG(format, arg...) LOG_MESSAGE(LOG_LEVEL_DEBUG, format, ##arg) +#define LOG_INFO(format, arg...) LOG_MESSAGE(LOG_LEVEL_INFO, format, ##arg) +#define LOG_WARNING(format, arg...) \ + LOG_MESSAGE(LOG_LEVEL_WARNING, format, ##arg) +#define LOG_ERROR(format, arg...) LOG_MESSAGE(LOG_LEVEL_ERROR, format, ##arg) + +void start_data_logging(void (*log_function)(void)); +void stop_data_logging(void); +void log_data(void); +void log_data_front_sensors_calibration(void); +void log_data_control(void); void log_battery_voltage(void); void log_configuration_variables(void); void log_linear_speed(void); void log_angular_speed(void); void log_sensors_distance(void); -void log_sensors_distance_pub(void); void log_encoders_counts(void); void log_sensors_raw(void); -void log_sensors_raw_pub(void); void log_side_sensors_error(void); -void log_front_sensors_calibration(void); void log_front_sensors_error(void); void log_walls_detection(void); -void log_gyro_raw_pub(void); -void log_gyro_dps_pub(void); -void log_gyro_degrees_pub(void); #endif /* __LOGGING_H */ diff --git a/src/main.c b/src/main.c index 971cd4c2..9f2b8c9f 100644 --- a/src/main.c +++ b/src/main.c @@ -29,6 +29,7 @@ void sys_tick_handler(void) update_gyro_readings(); update_encoder_readings(); motor_control(); + log_data(); } /** @@ -175,7 +176,7 @@ int main(void) while (1) { if (button_left_read_consecutive(500)) training(); - execute_commands(); + execute_command(); } return 0; diff --git a/src/mylibopencm3.c b/src/mylibopencm3.c new file mode 100644 index 00000000..a0cc1973 --- /dev/null +++ b/src/mylibopencm3.c @@ -0,0 +1,47 @@ +#include "mylibopencm3.h" + +/** + * @brief USART idle line interrupt enable. + * + * @param[in] usart USART block register address base. + */ +void usart_enable_idle_line_interrupt(uint32_t usart) +{ + USART_CR1(usart) |= USART_CR1_IDLEIE; +} + +/** + * @brief USART idle line interrupt disable. + * + * @param[in] usart USART block register address base. + */ +void usart_disable_idle_line_interrupt(uint32_t usart) +{ + USART_CR1(usart) &= ~USART_CR1_IDLEIE; +} + +/** + * @brief Check USART idle line detected bit in Status Register. + * + * @param[in] usart USART block register address base. + */ +bool usart_idle_line_detected(uint32_t usart) +{ + return ((USART_SR(usart) & USART_SR_IDLE) != 0); +} + +/** + * @brief Clear USART idle line detected bit in Status Register. + * + * Clear is performed by a software sequence: a read to the USART_SR register + * followed by a read to the USART_DR register. + * + * @param[in] usart USART block register address base. + * + * @see Reference Manual (RM0008): Status register (USART_SR). + */ +void usart_clear_idle_line_detected(uint32_t usart) +{ + USART_SR(usart); + USART_DR(usart); +} diff --git a/src/mylibopencm3.h b/src/mylibopencm3.h new file mode 100644 index 00000000..1f14fd4f --- /dev/null +++ b/src/mylibopencm3.h @@ -0,0 +1,11 @@ +#ifndef __MYLIBOPENCM3_H +#define __MYLIBOPENCM3_H + +#include + +void usart_enable_idle_line_interrupt(uint32_t usart); +void usart_disable_idle_line_interrupt(uint32_t usart); +bool usart_idle_line_detected(uint32_t usart); +void usart_clear_idle_line_detected(uint32_t usart); + +#endif /* __MYLIBOPENCM3_H */ diff --git a/src/serial.c b/src/serial.c index 86319ab2..50d627db 100644 --- a/src/serial.c +++ b/src/serial.c @@ -1,70 +1,147 @@ #include "serial.h" -#define BUFFER_SIZE 256 +#define RECEIVE_BUFFER_SIZE 256 -static struct data_buffer { - char data[BUFFER_SIZE]; - unsigned int size; -} buffer; +static volatile bool received; +static char receive_buffer[RECEIVE_BUFFER_SIZE]; -static bool run_angular_speed_profile_signal; -static bool run_linear_speed_profile_signal; -static bool run_static_turn_right_profile_signal; -static bool run_front_sensors_calibration_signal; -static char run_movement_sequence_signal[BUFFER_SIZE]; +/** + * @brief Check if the serial transfer is complete. + * + * @return Whether the transfer has been completed. + */ +bool serial_transfer_complete(void) +{ + return (bool)usart_get_flag(USART3, USART_SR_TC); +} /** - * @brief Push a single char to the serial received buffer. + * @brief Wait until able to send through serial, or timeout. + * + * @param[in] timeout Timeout duration, in ticks. */ -static void push_received(char data) +void serial_wait_send_available(uint32_t timeout) { - if (buffer.size >= BUFFER_SIZE) { - LOG_ERROR("Serial buffer overflow!"); - return; - } - buffer.data[buffer.size++] = data; + wait_until(serial_transfer_complete, timeout); } /** - * @brief Clear the serial received buffer. + * @brief Send data through serial. + * + * DMA is configured to read from `data` a number `size` of bytes. It then + * writes all those bytes to USART3 (Bluetooth). + * + * An interruption is generated when the transfer is complete. + * + * @param[in] data Data to send. + * @param[in] size Size (number of bytes) to send. */ -static void clear_received(void) +void serial_send(char *data, int size) { - buffer.data[0] = '\0'; - buffer.size = 0; + dma_channel_reset(DMA1, DMA_CHANNEL2); + + dma_set_peripheral_address(DMA1, DMA_CHANNEL2, (uint32_t)&USART3_DR); + dma_set_memory_address(DMA1, DMA_CHANNEL2, (uint32_t)data); + dma_set_number_of_data(DMA1, DMA_CHANNEL2, size); + dma_set_read_from_memory(DMA1, DMA_CHANNEL2); + dma_enable_memory_increment_mode(DMA1, DMA_CHANNEL2); + dma_set_peripheral_size(DMA1, DMA_CHANNEL2, DMA_CCR_PSIZE_8BIT); + dma_set_memory_size(DMA1, DMA_CHANNEL2, DMA_CCR_MSIZE_8BIT); + dma_set_priority(DMA1, DMA_CHANNEL2, DMA_CCR_PL_VERY_HIGH); + + dma_enable_transfer_complete_interrupt(DMA1, DMA_CHANNEL2); + + dma_enable_channel(DMA1, DMA_CHANNEL2); + + usart_enable_tx_dma(USART3); } /** - * @brief Parse a float number in the received buffer. + * @brief Receive data from serial. * - * The parsing will start after stripping a defined string from the start of - * the buffer. + * DMA is configured to read from USART3 (Bluetooth) a number `size` of bytes. + * It writes all those bytes to `data`. + * + * An interruption is generated when the transfer is complete. */ -static float parse_float(const char *left_strip) +static void serial_receive(void) { - char *pointer; + dma_channel_reset(DMA1, DMA_CHANNEL3); - pointer = buffer.data; - pointer += strlen(left_strip); - return strtof(pointer, NULL); + dma_set_peripheral_address(DMA1, DMA_CHANNEL3, (uint32_t)&USART3_DR); + dma_set_memory_address(DMA1, DMA_CHANNEL3, (uint32_t)receive_buffer); + dma_set_number_of_data(DMA1, DMA_CHANNEL3, RECEIVE_BUFFER_SIZE); + dma_set_read_from_peripheral(DMA1, DMA_CHANNEL3); + dma_enable_memory_increment_mode(DMA1, DMA_CHANNEL3); + dma_set_peripheral_size(DMA1, DMA_CHANNEL3, DMA_CCR_PSIZE_8BIT); + dma_set_memory_size(DMA1, DMA_CHANNEL3, DMA_CCR_MSIZE_8BIT); + dma_set_priority(DMA1, DMA_CHANNEL3, DMA_CCR_PL_HIGH); + + dma_enable_transfer_complete_interrupt(DMA1, DMA_CHANNEL3); + + dma_enable_channel(DMA1, DMA_CHANNEL3); + + usart_enable_rx_dma(USART3); +} + +/** + * @brief DMA 1 channel 2 interruption routine. + * + * Executed on serial transfer complete. Clears the interruption flag, and + * disables serial transfer DMA until next call to `serial_send()`. + */ +void dma1_channel2_isr(void) +{ + if (dma_get_interrupt_flag(DMA1, DMA_CHANNEL2, DMA_TCIF)) + dma_clear_interrupt_flags(DMA1, DMA_CHANNEL2, DMA_TCIF); + + dma_disable_transfer_complete_interrupt(DMA1, DMA_CHANNEL2); + usart_disable_tx_dma(USART3); + dma_disable_channel(DMA1, DMA_CHANNEL2); } /** - * @brief Parse a float number in the received buffer. + * @brief DMA 1 channel 3 interruption routine. + * + * Executed on serial receive complete. Clears the interruption flag, and + * resets the receive DMA by calling `serial_receive()`. + * + * This function should never be reached. Commands should be processed before + * on USART idle line interruption. + **/ +void dma1_channel3_isr(void) +{ + if (dma_get_interrupt_flag(DMA1, DMA_CHANNEL3, DMA_TCIF)) + dma_clear_interrupt_flags(DMA1, DMA_CHANNEL3, DMA_TCIF); + + dma_disable_transfer_complete_interrupt(DMA1, DMA_CHANNEL3); + usart_disable_rx_dma(USART3); + dma_disable_channel(DMA1, DMA_CHANNEL3); + LOG_ERROR("Receive buffer is full! Resetting..."); + serial_receive(); +} + +/** + * @brief Parse a float number in a given string. * * The parsing will start after a defined number of spaces that are expected * before the float in the string. + * + * This function assumes that `string` is of size `RECEIVE_BUFFER_SIZE`. + * + * @param[in] string String to parse the float from. + * @param[in] spaces_before Number of spaces expected before the float. */ -static float parse_spaced_float(int spaces_before) +static float parse_float(char *string, int spaces_before) { unsigned int i; char *pointer; - pointer = buffer.data; - for (i = 0; i < buffer.size; i++) { - if (buffer.data[i] == ' ') + pointer = string; + for (i = 0; i < RECEIVE_BUFFER_SIZE; i++) { + if (string[i] == ' ') spaces_before--; - if (buffer.data[i] == '\0') + if (string[i] == '\0') return 0.; pointer++; if (spaces_before == 0) @@ -75,65 +152,68 @@ static float parse_spaced_float(int spaces_before) /** * @brief Check if the received buffer starts with the given string. + * + * @param[in] string String buffer. + * @param[in] start_string Prefix to look for at the start of the string buffer. */ -static bool starts_with(char *start_string) +static bool starts_with(char *string, char *start_string) { - return (bool)!strncmp(buffer.data, start_string, strlen(start_string)); + return (bool)!strncmp(string, start_string, strlen(start_string)); } /** * @brief Process a command received. + * + * @param[in] string String buffer potentially containing the command. */ -static void process_command(void) +static void process_command(char *string) { - LOG_DEBUG("Processing \"%s\"", buffer.data); + LOG_DEBUG("Processing \"%s\"", string); - if (!strcmp(buffer.data, "battery")) + if (!strcmp(string, "battery")) log_battery_voltage(); - else if (!strcmp(buffer.data, "configuration_variables")) + else if (!strcmp(string, "configuration_variables")) log_configuration_variables(); - else if (!strcmp(buffer.data, "run linear_speed_profile")) - run_linear_speed_profile_signal = true; - else if (!strcmp(buffer.data, "run angular_speed_profile")) - run_angular_speed_profile_signal = true; - else if (!strcmp(buffer.data, "run static_turn_right_profile")) - run_static_turn_right_profile_signal = true; - else if (!strcmp(buffer.data, "run front_sensors_calibration")) - run_front_sensors_calibration_signal = true; - else if (starts_with("move ")) - strcpy(run_movement_sequence_signal, buffer.data); - else if (starts_with("set micrometers_per_count ")) - set_micrometers_per_count(parse_spaced_float(2)); - else if (starts_with("set wheels_separation ")) - set_wheels_separation(parse_spaced_float(2)); - else if (starts_with("set max_linear_speed ")) - set_max_linear_speed(parse_spaced_float(2)); - else if (starts_with("set linear_acceleration ")) - set_linear_acceleration(parse_spaced_float(2)); - else if (starts_with("set linear_deceleration ")) - set_linear_deceleration(parse_spaced_float(2)); - else if (starts_with("set angular_acceleration ")) - set_angular_acceleration(parse_spaced_float(2)); - else if (starts_with("set kp_linear ")) - set_kp_linear(parse_float("set kp_linear ")); - else if (starts_with("set kd_linear ")) - set_kd_linear(parse_float("set kd_linear ")); - else if (starts_with("set kp_angular ")) - set_kp_angular(parse_float("set kp_angular ")); - else if (starts_with("set kd_angular ")) - set_kd_angular(parse_float("set kd_angular ")); - else if (starts_with("set ki_angular_side ")) - set_ki_angular_side(parse_spaced_float(2)); - else if (starts_with("set ki_angular_front ")) - set_ki_angular_front(parse_spaced_float(2)); - else if (starts_with("set kp_angular_side ")) - set_kp_angular_side(parse_spaced_float(2)); - else if (starts_with("set kp_angular_front ")) - set_kp_angular_front(parse_spaced_float(2)); + else if (!strcmp(string, "run linear_speed_profile")) + run_linear_speed_profile(); + else if (!strcmp(string, "run angular_speed_profile")) + run_angular_speed_profile(); + else if (!strcmp(string, "run static_turn_right_profile")) + run_static_turn_right_profile(); + else if (!strcmp(string, "run front_sensors_calibration")) + run_front_sensors_calibration(); + else if (starts_with(string, "move ")) + run_movement_sequence(string); + else if (starts_with(string, "set micrometers_per_count ")) + set_micrometers_per_count(parse_float(string, 2)); + else if (starts_with(string, "set wheels_separation ")) + set_wheels_separation(parse_float(string, 2)); + else if (starts_with(string, "set max_linear_speed ")) + set_max_linear_speed(parse_float(string, 2)); + else if (starts_with(string, "set linear_acceleration ")) + set_linear_acceleration(parse_float(string, 2)); + else if (starts_with(string, "set linear_deceleration ")) + set_linear_deceleration(parse_float(string, 2)); + else if (starts_with(string, "set angular_acceleration ")) + set_angular_acceleration(parse_float(string, 2)); + else if (starts_with(string, "set kp_linear ")) + set_kp_linear(parse_float(string, 2)); + else if (starts_with(string, "set kd_linear ")) + set_kd_linear(parse_float(string, 2)); + else if (starts_with(string, "set kp_angular ")) + set_kp_angular(parse_float(string, 2)); + else if (starts_with(string, "set kd_angular ")) + set_kd_angular(parse_float(string, 2)); + else if (starts_with(string, "set ki_angular_side ")) + set_ki_angular_side(parse_float(string, 2)); + else if (starts_with(string, "set ki_angular_front ")) + set_ki_angular_front(parse_float(string, 2)); + else if (starts_with(string, "set kp_angular_side ")) + set_kp_angular_side(parse_float(string, 2)); + else if (starts_with(string, "set kp_angular_front ")) + set_kp_angular_front(parse_float(string, 2)); else - LOG_WARNING("Unknown command: `%s`!", buffer.data); - - clear_received(); + LOG_ERROR("Unknown command: `%s`!", string); } /** @@ -141,40 +221,25 @@ static void process_command(void) * * On RX interruption it will process the data received and, in case it finds * a `'\0'` character, the `process_command` function will be executed. - **/ + */ void usart3_isr(void) { - static uint8_t data; - - /* Only execute on RX interrupt */ - if (((USART_CR1(USART3) & USART_CR1_RXNEIE) != 0) && - ((USART_SR(USART3) & USART_SR_RXNE) != 0)) { - data = usart_recv(USART3); - push_received(data); - if (data == '\0') - process_command(); + /* Only execute on idle interrupt */ + if (((USART_CR1(USART3) & USART_CR1_IDLEIE) != 0) && + usart_idle_line_detected(USART3)) { + received = true; + usart_clear_idle_line_detected(USART3); + serial_receive(); } } /** - * @brief Execute commands received. + * @brief Execute a command received. */ -void execute_commands(void) +void execute_command(void) { - if (run_linear_speed_profile_signal) { - run_linear_speed_profile_signal = false; - run_linear_speed_profile(); - } else if (run_angular_speed_profile_signal) { - run_angular_speed_profile_signal = false; - run_angular_speed_profile(); - } else if (run_static_turn_right_profile_signal) { - run_static_turn_right_profile_signal = false; - run_static_turn_right_profile(); - } else if (run_front_sensors_calibration_signal) { - run_front_sensors_calibration_signal = false; - run_front_sensors_calibration(); - } else if (strlen(run_movement_sequence_signal)) { - run_movement_sequence(run_movement_sequence_signal); - run_movement_sequence_signal[0] = '\0'; - } + if (!received || receive_buffer[0] == '\0') + return; + process_command(receive_buffer); + received = false; } diff --git a/src/serial.h b/src/serial.h index 33d982eb..dfefb0f7 100644 --- a/src/serial.h +++ b/src/serial.h @@ -13,6 +13,9 @@ #include "move.h" #include "speed.h" -void execute_commands(void); +bool serial_transfer_complete(void); +void serial_wait_send_available(uint32_t timeout); +void serial_send(char *data, int size); +void execute_command(void); #endif /* __SERIAL_H */ diff --git a/src/setup.c b/src/setup.c index afcbfa69..7a0b2b01 100644 --- a/src/setup.c +++ b/src/setup.c @@ -49,6 +49,9 @@ static void setup_clock(void) rcc_periph_clock_enable(RCC_ADC1); rcc_periph_clock_enable(RCC_ADC2); + /* DMA */ + rcc_periph_clock_enable(RCC_DMA1); + /* Enable clock cycle counter */ dwt_enable_cycle_counter(); } @@ -61,13 +64,17 @@ static void setup_clock(void) * * Exception priorities: * + * - TIM1_UP with priority 0. * - Systick priority to 1 with SCB. + * - DMA 1 channel 2 with priority 2 with NVIC. + * - DMA 1 channel 3 with priority 2 with NVIC. * - USART3 with priority 2 with NVIC. - * - TIM1_UP with priority 0. * * Interruptions enabled: * * - TIM1 Update interrupt. + * - DMA 1 channel 2 interrupt. + * - DMA 1 channel 3 interrupt. * - USART3 interrupt. * * @note The priority levels are assigned on steps of 16 because the processor @@ -79,9 +86,13 @@ static void setup_exceptions(void) { nvic_set_priority(NVIC_TIM1_UP_IRQ, 0); nvic_set_priority(NVIC_SYSTICK_IRQ, PRIORITY_FACTOR * 1); + nvic_set_priority(NVIC_DMA1_CHANNEL2_IRQ, PRIORITY_FACTOR * 2); + nvic_set_priority(NVIC_DMA1_CHANNEL3_IRQ, PRIORITY_FACTOR * 2); nvic_set_priority(NVIC_USART3_IRQ, PRIORITY_FACTOR * 2); nvic_enable_irq(NVIC_TIM1_UP_IRQ); + nvic_enable_irq(NVIC_DMA1_CHANNEL2_IRQ); + nvic_enable_irq(NVIC_DMA1_CHANNEL3_IRQ); nvic_enable_irq(NVIC_USART3_IRQ); } @@ -144,7 +155,7 @@ static void setup_usart(void) usart_set_flow_control(USART3, USART_FLOWCONTROL_NONE); usart_set_mode(USART3, USART_MODE_TX_RX); - USART_CR1(USART3) |= USART_CR1_RXNEIE; + usart_enable_idle_line_interrupt(USART3); usart_enable(USART3); } diff --git a/src/setup.h b/src/setup.h index 4b3edf11..9a35e992 100644 --- a/src/setup.h +++ b/src/setup.h @@ -6,12 +6,15 @@ #include #include #include +#include #include #include #include #include #include +#include "mylibopencm3.h" + #include /** Universal constants */ #define MICROMETERS_PER_METER 1000000 diff --git a/src/solve.h b/src/solve.h index bc2142ad..d4f5e847 100644 --- a/src/solve.h +++ b/src/solve.h @@ -3,7 +3,7 @@ #include "detection.h" #include "eeprom.h" -#include "formatting.h" +#include "logging.h" #include "move.h" #include "path.h" #include "search.h" diff --git a/src/speed.c b/src/speed.c index 33e57636..45bcf58d 100644 --- a/src/speed.c +++ b/src/speed.c @@ -23,6 +23,16 @@ const float max_linear_speed_run_defaults[NUM_MODES] = {1., 1.5, 2., 2.5}; uint8_t speed_configuration; +/** + * Parameters that define a turn. + * + * - Meters to travel in straight line before turning + * - Meters to travel in straight line after turning + * - Linear speed, in meters per second, at which to turn + * - Duration, in milliseconds, of the positive angular acceleration phase + * - Duration, in milliseconds, of the whole turn + * - Maximum angular speed at which to turn, in radians per second + */ struct turn_parameters { float before; float after; diff --git a/src/speed.h b/src/speed.h index 6f7fd586..8c7f485f 100644 --- a/src/speed.h +++ b/src/speed.h @@ -1,7 +1,6 @@ #ifndef __SPEED_H #define __SPEED_H -#include "formatting.h" #include "move.h" #include "path.h" #include "setup.h"