Implement real-time logging of control variables

scripts/notebooks/show_data.ipynb

@@ -0,0 +1,160 @@
+{
+ "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.head()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "diffs = df['timestamp'].diff().shift(-1)\n",
+    "df = df[(diffs > 0) & (diffs < 4)]\n",
+    "df.head()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {
+    "scrolled": true
+   },
+   "outputs": [],
+   "source": [
+    "df['timestamp'] /= 1000\n",
+    "df.set_index('timestamp')\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
+}

src/logging.c

@@ -160,6 +160,28 @@ void log_data_front_sensors_calibration(void)
 		 left_distance, right_distance);
 }
 
+/**
+ * @brief Log all interesting control variables.
+ */
+void log_data_control(void)
+{
+	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);
+}
+
 /**
  * @brief Log front sensors distances and error.
  */

src/logging.h

@@ -49,6 +49,7 @@ 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);