Allow in-systick logging over bluetooth with DMA

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',

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",

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
+}

src/battery.h

@@ -6,7 +6,6 @@
 #include <libopencm3/stm32/gpio.h>
 
 #include "common.h"
-#include "formatting.h"
 #include "hmi.h"
 #include "setup.h"
 

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();
 

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;
+}

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);

src/control.c

@@ -219,15 +219,15 @@ 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;
 }
 
 /**
  * @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);